【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;
                                                       }
                                                       else
                                                           response->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);
        }
        else
            msg.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;
}