ros2让海龟画矩形c++

ROS 2 (Robot Operating System) 是一个开源机器人操作系统，它基于 DDS (Data Distribution Service for Real-Time Systems) 提供了分布式系统架构，用于控制、感知和通信。要在ROS 2中使用C++让海龟（通常指`turtlebot3`的模拟器）画出矩形，你可以参考以下步骤：

1. **安装必要的库**:
首先，确保已安装`rclcpp`和` turtlesim`包。如果你还没有安装，可以在终端中运行：

   sudo apt-get install ros-<your_distro>-ros-turtlesim

2. **创建一个新的C++节点**:
使用`ros2 node`命令创建一个新的C++节点，比如`draw_rectangle_node`。在这个文件中，你需要编写控制海龟动作的逻辑。

#include <rclcpp/rclcpp.hpp>
#include <turtlesim/turtlesim_msgs/msg/point.hpp>
#include "geometry_msgs/msg/polygon.hpp"

class DrawRectangle : public rclcpp::Node
{
public:
  explicit DrawRectangle(rclcpp::NodeOptions options)
    : Node("draw_rectangle_node", options),
      turtlesim_client_("turtle1") {}

  void drawRect(const geometry_msgs::msg::Polygon::SharedPtr polygon_msg)
  {
    if (!turtlesim_client_.is_connected()) {
      RCLCPP_ERROR(get_logger(), "Failed to connect to turtle1 service.");
      return;
    }

    // 矩形路径的四个点
    turtlesim_msgs::msg::Point point1, point2, point3, point4;
    set_points(polygon_msg, point1, point2, point3, point4);

    auto request = std::make_shared<turtlesim_msgs::msg::Pen>();
    request->color = 0; // 设定颜色，默认通常是红色
    request->width = 1; // 设定宽度

    turtlesim_msgs::msg::Move msg;
    msg.pen = *request;

    // 发送每个点到海龟的动作消息
    for (const auto &p : {point1, point2, point3, point4, point1}) {
      msg.points.push_back(p);
      turtlesim_client_.call_async(msg, [this, p](auto response) {
        if (!response.success) {
          RCLCPP_ERROR(this->get_logger(), "Failed to move turtle. Reason: %s",
                       response.message.c_str());
        } else {
          RCLCPP_INFO(this->get_logger(),
                     "Moved turtle successfully to (%f, %f)",
                     p.x, p.y);
        }
      });
    }
  }

private:
  void set_points(geometry_msgs::msg::Polygon::SharedPtr polygon_msg,
                  turtlesim_msgs::msg::Point &point1,
                  turtlesim_msgs::msg::Point &point2,
                  turtlesim_msgs::msg::Point &point3,
                  turtlesim_msgs::msg::Point &point4)
  {
    point1.x = polygon_msg->points[0].x;
    point1.y = polygon_msg->points[0].y;
    point2.x = polygon_msg->points[1].x;
    point2.y = polygon_msg->points[1].y;
    point3.x = polygon_msg->points[2].x;
    point3.y = polygon_msg->points[2].y;
    point4.x = polygon_msg->points[3].x;
    point4.y = polygon_msg->points[3].y;
  }

  rclcpp::Client<turtlesim_msgs::srv::Move> turtlesim_client_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = DrawRectangle::make_shared();
  auto future = node->spin_some(); // 开始监听和服务请求

  while (rclcpp::ok()) {
    auto result = future.wait_for(std::chrono::seconds(1));
    if (result == rclcpp::FutureStatus::SUCCESS) {
      node->drawRect(node->get_topic<std_msgs::msg::Polygon>("polygon_topic")); // 从话题获取矩形消息并绘制
    }
  }

  rclcpp::shutdown();
  return 0;
}