【ros-humble】4.C++写法巡场海龟（服务通讯）

新添加通讯类型，可以看到编译后会生成C++和python的库。

float32 target_x #目标x
float32 target_y #目标y
---
int8 SUCCESS = 1
int8 FAIL = 0 
int8 result #结果

创造功能包

ros2 pkg create cpp_service --build-type  ament_cmake  --dependencies chapt4_interfaces rclcpp geometry_msgs turtlesim --license Apache-2.0

创建服务端和话题发布者

/*********************发布*************************** */
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using namespace std::chrono_literals;
using Partol = chapt4_interfaces::srv::Patrol;class Turtle_Control : public rclcpp::Node
{
public:explicit Turtle_Control(const std::string &node_name);~Turtle_Control();private:/* data */rclcpp::Service<Partol>::SharedPtr _patrol_service;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr _publisher; // 声明话题发布者指针rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr _subscriber;  // 声明话题订阅者指针void _pose_subscriber(const turtlesim::msg::Pose::SharedPtr pose);double _target_x{1.0};double _target_y{1.0};double k{1.0};double max_speed{3.0};
};
Turtle_Control::Turtle_Control(const std::string &node_name) : Node(node_name)
{_patrol_service = this->create_service<Partol>("patrol",[&](const Partol::Request::SharedPtr request,Partol::Response::SharedPtr response) -> void{if ((request->target_x > 0) && (request->target_x < 12.0f) && (request->target_y > 0) && (request->target_y < 12.0f)){this->_target_x = request->target_x;this->_target_y = request->target_y;response->result = Partol::Response::SUCCESS;}elseresponse->result = Partol::Response::FAIL;});_publisher = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10); // 创建发布者_subscriber = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10,std::bind(&Turtle_Control::_pose_subscriber, this, std::placeholders::_1));
}
void Turtle_Control::_pose_subscriber(const turtlesim::msg::Pose::SharedPtr pose)
{auto current_x = pose->x;auto current_y = pose->y;RCLCPP_INFO(get_logger(), "x = %f,y = %f", current_x, current_y);auto distance = std::sqrt((_target_x - current_x) * (_target_x - current_x) +(_target_y - current_y) * (_target_y - current_y));auto angle = std::atan2((_target_y - current_y), (_target_x - current_x)) - pose->theta;auto msg = geometry_msgs::msg::Twist();if (distance > 0.1){if (fabs(angle) > 0.2){msg.angular.z = fabs(angle);}elsemsg.linear.x = k * distance;}if (msg.linear.x > max_speed){msg.linear.x = max_speed;}_publisher->publish(msg);
}Turtle_Control::~Turtle_Control()
{
}int main(int argc, char *argv[])
{rclcpp::init(argc, argv);auto node = std::make_shared<Turtle_Control>("Turtle_Control");rclcpp::spin(node);rclcpp::shutdown();return 0;
}

创造客户端

#include <chrono>
#include <ctime> //创造随机数
#include "rclcpp/rclcpp.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using namespace std::chrono_literals;
using Partol = chapt4_interfaces::srv::Patrol;class TurtleControler : public rclcpp::Node
{
private:rclcpp::Client<Partol>::SharedPtr _client;rclcpp::TimerBase::SharedPtr _timer;public:TurtleControler(const std::string &node_name);~TurtleControler();
};TurtleControler::TurtleControler(const std::string &node_name) : Node(node_name)
{srand(time(NULL));_client = this->create_client<Partol>("patrol");_timer = this->create_wall_timer(10s, [&]() -> void{// 等待服务while (!this->_client->wait_for_service(1s)){if (!rclcpp::ok()){RCLCPP_ERROR(this->get_logger(), "等待失败");return;}RCLCPP_INFO(this->get_logger(), "等待上线...");}auto request = std::make_shared<Partol::Request>();request->target_x = rand() % 15;request->target_y = rand() % 15;RCLCPP_INFO(this->get_logger(), "x:%f,y:%f", request->target_x, request->target_y);// 发送请求this->_client->async_send_request(request, [&](rclcpp::Client<Partol>::SharedFuture future_result) -> void {auto response = future_result.get();if(response->result == Partol::Response::SUCCESS){RCLCPP_INFO(this->get_logger(),"request success");}else{RCLCPP_INFO(this->get_logger(),"request error");}}); });
}TurtleControler::~TurtleControler()
{
}int main(int argc, char *argv[])
{rclcpp::init(argc, argv);auto node = std::make_shared<TurtleControler>("turtle_control_client");rclcpp::spin(node);rclcpp::shutdown();return 0;
}