ROS2的lifecycle的相关问题

小猴同学

系统ubuntu22.04,ROS2humble
各位大佬们,我在项目中遇到这么一个问题,就是我的设备目前情况是这样的:
在短途内通过wifi发送上电指令,上电后,通过PLC的上电状况来启动和暂停节点,举个问题的例子因为我的IMU和DVL是通过串口转socket向工控机发送消息,如果首先开启IMU的发布节点,而IMU没有上电,IMU节点会因为socket通信迟迟没有连接从而报错退出节点,因此我考虑使用lifecycle来管理节点,通过上电后返回的上电状态来启动和暂停节点的运行,但是lifecycle的客户端总是在响应过后,能够正常启动服务端节点,但是每次启动后,客户端总是会退出,必须重新运行节点,我想能让他持续等待下一次的下电状态,请问我应该怎么修改比较好.
大佬们,我采用官方给的客户端程序如下(学的比较基础)

#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <auv_msgs/msg/wifidata.hpp>
#include "lifecycle_msgs/msg/state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "auv_msgs/msg/wifidata.hpp"
#include "rclcpp/rclcpp.hpp"
#include "auv_lifecycle/lifecycle_var.h"

#include "rcutils/logging_macros.h"
using namespace std;
using std::placeholders::_1;
using namespace std::chrono_literals;

static constexpr char const *lifecycle_node = "ctd_sensor_publisher";

static constexpr char const *node_get_state_topic = "ctd_sensor_publisher/get_state";
static constexpr char const *node_change_state_topic = "ctd_sensor_publisher/change_state";
template <typename FutureT, typename WaitTimeT>
std::future_status
wait_for_result(
    FutureT &future,
    WaitTimeT time_to_wait)
{
  auto end = std::chrono::steady_clock::now() + time_to_wait;
  std::chrono::milliseconds wait_period(100);
  std::future_status status = std::future_status::timeout;
  do
  {
    auto now = std::chrono::steady_clock::now();
    auto time_left = end - now;
    if (time_left <= std::chrono::seconds(0))
    {
      break;
    }
    status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
  } while (rclcpp::ok() && status != std::future_status::ready);
  return status;
}

class LifecycleServiceClient : public rclcpp::Node
{
public:
  explicit LifecycleServiceClient(const std::string &node_name)
      : Node(node_name)
  {
  }

  void init()
  {
    Power_on_sub_ = this->create_subscription<auv_msgs::msg::WIFIDATA>("wifi_data_state", 10, std::bind(&LifecycleServiceClient::power_on_callback, this, _1));
    client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(node_get_state_topic);
    client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(node_change_state_topic);
  }
  unsigned int get_state(std::chrono::seconds time_out = 1000000s)
  {
    auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();

    if (!client_get_state_->wait_for_service(time_out))
    {
      RCLCPP_ERROR(
          get_logger(),
          "Service %s is not available.",
          client_get_state_->get_service_name());
      return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
    }

    // We send the service request for asking the current
    // state of the lc_talker node.
    auto future_result = client_get_state_->async_send_request(request).future.share();

    // Let's wait until we have the answer from the node.
    // If the request times out, we return an unknown state.
    auto future_status = wait_for_result(future_result, time_out);

    if (future_status != std::future_status::ready)
    {
      RCLCPP_ERROR(
          get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
      return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
    }

    // We have an succesful answer. So let's print the current state.
    if (future_result.get())
    {
      RCLCPP_INFO(
          get_logger(), "Node %s has current state %s.",
          lifecycle_node, future_result.get()->current_state.label.c_str());
      return future_result.get()->current_state.id;
    }
    else
    {
      RCLCPP_ERROR(
          get_logger(), "Failed to get current state for node %s", lifecycle_node);
      return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
    }
  }

  bool change_state(std::uint8_t transition, std::chrono::seconds time_out = 1000000s)
  {
    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = transition;

    if (!client_change_state_->wait_for_service(time_out))
    {
      RCLCPP_ERROR(
          get_logger(),
          "Service %s is not available.",
          client_change_state_->get_service_name());
      return false;
    }

    // We send the request with the transition we want to invoke.
    auto future_result = client_change_state_->async_send_request(request).future.share();

    // Let's wait until we have the answer from the node.
    // If the request times out, we return an unknown state.
    auto future_status = wait_for_result(future_result, time_out);

    if (future_status != std::future_status::ready)
    {
      RCLCPP_ERROR(
          get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
      return false;
    }

    // We have an answer, let's print our success.
    if (future_result.get()->success)
    {
      RCLCPP_INFO(
          get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition));
      return true;
    }
    else
    {
      RCLCPP_WARN(
          get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
      return false;
    }
  }

private:
  std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;
  std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_;
  rclcpp::Subscription<auv_msgs::msg::WIFIDATA>::SharedPtr Power_on_sub_;
  std::mutex mtxlf;
  void power_on_callback(const auv_msgs::msg::WIFIDATA &power_on_msg)
  {
    mtxlf.lock();
    ctd_powerflag = power_on_msg.bcontrolon_cabina;
    // cout<<ctd_powerflag<<endl;
    mtxlf.unlock();
  }
};

void callee_script(std::shared_ptr<LifecycleServiceClient> lc_client)
{
  rclcpp::WallRate time_between_state_changes(0.1); // 10s
  sleep(3);
  cout << ctd_powerflag << endl;
  if (ctd_powerflag == true)

  {
    if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE))
    {
      return;
    }
    if (!lc_client->get_state())
    {
      return;
    }
  }
  if (!rclcpp::ok())
  {
    return;
  }
  if (ctd_powerflag == false)
  {
    if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP))
    {
      return;
    }
    if (!lc_client->get_state())
    {
      return;
    }
  }
  // activate
  // {
  //   time_between_state_changes.sleep();
  //   if (!rclcpp::ok())
  //   {
  //     return;
  //   }
  //   if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE))
  //   {
  //     return;
  //   }
  //   if (!lc_client->get_state())
  //   {
  //     return;
  //   }
  // }

  // deactivate
  // {
  //   time_between_state_changes.sleep();
  //   if (!rclcpp::ok())
  //   {
  //     return;
  //   }
  //   if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE))
  //   {
  //     return;
  //   }
  //   if (!lc_client->get_state())
  //   {
  //     return;
  //   }
  // }

  // // activate it again
  // {
  //   time_between_state_changes.sleep();
  //   if (!rclcpp::ok()) {
  //     return;
  //   }
  //   if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) {
  //     return;
  //   }
  //   if (!lc_client->get_state()) {
  //     return;
  //   }
  // }

  // // and deactivate it again
  // {
  //   time_between_state_changes.sleep();
  //   if (!rclcpp::ok()) {
  //     return;
  //   }
  //   if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) {
  //     return;
  //   }
  //   if (!lc_client->get_state()) {
  //     return;
  //   }
  // }

  // we cleanup

  // time_between_state_changes.sleep();
  //  if (!rclcpp::ok()) {
  //    return;
  //  }

  // and finally shutdown
  // Note: We have to be precise here on which shutdown transition id to call
  // We are currently in the unconfigured state and thus have to call
  // TRANSITION_UNCONFIGURED_SHUTDOWN
  // {
  //   time_between_state_changes.sleep();
  //   if (!rclcpp::ok()) {
  //     return;
  //   }
  //   if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN))
  //   {
  //     return;
  //   }
  //   if (!lc_client->get_state()) {
  //     return;
  //   }
  // }
}

// void wake_executor(std::shared_future<void> future, rclcpp::executors::SingleThreadedExecutor &exec)
void wake_executor(std::shared_future<void> future, rclcpp::executors::MultiThreadedExecutor &exec)

{
  future.wait();
  exec.cancel();
}

int main(int argc, char **argv)
{
  // force flush of the stdout buffer.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  rclcpp::init(argc, argv);

  auto lc_client = std::make_shared<LifecycleServiceClient>("ctd_sensor_publisher");
  lc_client->init();

  // rclcpp::executors::SingleThreadedExecutor exe;
  rclcpp::executors::MultiThreadedExecutor exe;
  exe.add_node(lc_client);
  std::shared_future<void> script = std::async(
      std::launch::async,
      std::bind(callee_script, lc_client));
  auto wake_exec = std::async(
      std::launch::async,
      std::bind(wake_executor, script, std::ref(exe)));

  exe.spin_until_future_complete(script);
  rclcpp::shutdown();

  return 0;
}

小鱼

@yudonghou123 问题看的有点迷糊

在 ROS2的lifecycle的相关问题中说：

exe.spin_until_future_complete(script);

看main函数，这个future完成后，代码就退出了。在script对应的函数里看到很多 return，检查下在哪里出现的return。