RO2 C++ 服务使用

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