系统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;
}
