ROS2在C++下只提供异步服务调用,服务端和客户端分别创建如下:
1. 类里增加成员变量:
private:
rclcpp::Client<robot_msgs::srv::ComCtrlCmd>::SharedPtr com_ctrl_client_;
rclcpp::Service<robot_msgs::srv::RobotCtrlCmd>::SharedPtr robot_ctrl_service_;
2. 类函数实现中增加客户端和服务端的创建代码:
注意需要使用头文件#include <unistd.h> 和空间:using namespace std::chrono_literals;
void CommunicationNode::InitCom(rclcpp::Node::SharedPtr &ros_node)
{
p_robot_node_ = ros_node;
robot_ctrl_service_ = p_robot_node_->create_service<robot_msgs::srv::RobotCtrlCmd>("RobotCtrlCmd", std::bind(&CommunicationNode::HandleRobotCtrlCmdRcv, this, std::placeholders::_1, std::placeholders::_2));
com_ctrl_client_ = p_robot_node_->create_client<robot_msgs::srv::ComCtrlCmd>("ComCtrlCmd");
com_send_thread_ = new std::thread(std::bind(&CommunicationNode::SendProcess, this));
}
3. 实现服务端回调处理:
bool CommunicationNode::HandleRobotCtrlCmdRcv(const std::shared_ptr<robot_msgs::srv::RobotCtrlCmd::Request> req,
std::shared_ptr<robot_msgs::srv::RobotCtrlCmd::Response> res)
{
switch (req->cmd_id)
{
case ComNodeRequestShutDown:
ev.key_long_pressed = true;
break;
case ComNodeRequestStartStop:
ev.auto_key_pressed = true;
break;
default:
break;
}
res->exe_rzt = true;
return true;
}
4. 实现客户端调用:
先检查连接
bool CommunicationNode::WaitForService()
{
while (!com_ctrl_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
BAY_PRINTF("robotctrl", ERROR, "Interruped while waiting for the server.");
return false;
}
BAY_PRINTF("robotctrl", ERROR, "Server not available, waiting again...");
}
return true;
}
实现调用,可以用异步等待返回回调形式:
auto request = std::make_shared<robot_msgs::srv::ComCtrlCmd::Request>(cmd_tx_.front());
cmd_tx_.pop_front();
auto result = com_ctrl_client_->async_send_request(request, std::bind(&CommunicationNode::SendResultCallback, this,
std::placeholders::_1));
void CommunicationNode::SendResultCallback(rclcpp::Client<robot_msgs::srv::ComCtrlCmd>::SharedFuture result_future)
{
robot_msgs::srv::ComCtrlCmd::Response::SharedPtr response = result_future.get();
//result_future.get().
//BAY_PRINTF("robotctrl", INFO, "rzt %ld", response->exe_rzt);
}
也可以直接检查等待返回:
if(!WaitForService())
{
return false;
}
std_srvs::srv::SetBool::Request req_srv;
req_srv.data = true;
auto request = std::make_shared<std_srvs::srv::SetBool::Request>(req_srv);
auto result = ai_ctrl_client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(p_robot_node_, result) == rclcpp::FutureReturnCode::SUCCESS)
{
open_time_ = rclcpp::Clock().now();
BAY_PRINTF("robotctl", INFO, "OpenLocation!\n");
return true;
}
else
{
BAY_PRINTF("robotctl", ERROR, "OpenLocation fail!\n");
is_ready_ = false;
return false;
}