创建一个机器人状态监控系统，利用自定义消息和服务类型进行通信。每一段代码给出文字解释和所在路径。要求: 1.创建一个自定义消息类型，用于表示机器人的状态信息。消息中包含机器人名称(string类型).位置(float32类型的x、y、z坐标)和朝向(float32类型的roll、pitch、yaw角度)等信息。 2.创建一个发布者节点，周期性地发布机器人状态消息到话题上。 3.创建一个订阅者节点，接收机器人状态消息，并在终端上显示机器人的名称和位置信息。再使用ros2中的参数服务器来优化上述机器人的动作参数。要求如下：定义动作相关的参数，如运动参数（速度，加速度，转弯半径），感知参数（摄像头分辨率）等，并使用参数服务器进行参数调整和优化。并且编写节点来读取和更新参数。

创建自定义消息类型：

在ROS2中，使用rosidl来定义和生成消息类型。在终端中执行以下命令创建机器人状态消息类型：

ros2 pkg create my_robot_msgs --build-type ament_cmake --node-name my_robot_msgs

这会在`my_robot_msgs`包中创建一个名为`msg`的目录，用于存储自定义消息类型。

在`msg`目录中创建一个名为`RobotStatus.msg`的文件，并添加以下内容：

string name
float32 x
float32 y
float32 z
float32 roll
float32 pitch
float32 yaw

这个自定义消息类型包含机器人名称（`name`）和位置信息（`x`、`y`、`z`）以及朝向信息（`roll`、`pitch`、`yaw`）。

发布者节点：

在ROS2中，可以使用rclcpp库来编写发布者节点。在`my_robot_msgs`包中创建一个名为`status_publisher.cpp`的文件，并添加以下内容：

#include "rclcpp/rclcpp.hpp"
#include "my_robot_msgs/msg/robot_status.hpp"

using namespace std::chrono_literals;

class StatusPublisher : public rclcpp::Node
{
public:
  StatusPublisher() : Node("status_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<my_robot_msgs::msg::RobotStatus>("robot_status", 10);
    timer_ = this->create_wall_timer(500ms, std::bind(&StatusPublisher::publish_status, this));
  }

private:
  void publish_status()
  {
    auto message = my_robot_msgs::msg::RobotStatus();
    message.name = "robot1";
    message.x = 1.0;
    message.y = 2.0;
    message.z = 0.5;
    message.roll = 0.0;
    message.pitch = 0.0;
    message.yaw = 1.57;

    publisher_->publish(message);

    count_++;
  }

  rclcpp::Publisher<my_robot_msgs::msg::RobotStatus>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t count_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StatusPublisher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

这个发布者节点使用了一个定时器来周期性地发布机器人状态消息。在`publish_status`函数中，创建一个`RobotStatus`消息，并设置机器人的名称、位置和朝向信息。然后，调用`publisher_->publish`函数将消息发布到名为`robot_status`的话题上。

订阅者节点：

同样地，在ROS2中，可以使用rclcpp库来编写订阅者节点。在`my_robot_msgs`包中创建一个名为`status_subscriber.cpp`的文件，并添加以下内容：

#include "rclcpp/rclcpp.hpp"
#include "my_robot_msgs/msg/robot_status.hpp"

using RobotStatus = my_robot_msgs::msg::RobotStatus;

class StatusSubscriber : public rclcpp::Node
{
public:
  StatusSubscriber() : Node("status_subscriber")
  {
    subscription_ = this->create_subscription<RobotStatus>("robot_status", 10, std::bind(&StatusSubscriber::status_callback, this, std::placeholders::_1));
  }

private:
  void status_callback(const RobotStatus::SharedPtr message)
  {
    RCLCPP_INFO(this->get_logger(), "Received robot status: name = %s, position = (%f, %f, %f), orientation = (%f, %f, %f)", message->name.c_str(), message->x, message->y, message->z, message->roll, message->pitch, message->yaw);
  }

  rclcpp::Subscription<RobotStatus>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StatusSubscriber>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

这个订阅者节点创建一个订阅器，订阅名为`robot_status`的话题。当接收到消息时，调用`status_callback`函数来处理消息。在这个例子中，`status_callback`函数简单地打印出机器人的名称和位置信息。

参数服务器：

在ROS2中，使用rclcpp的`Node::declare_parameter`函数来声明节点参数，并使用`Node::get_parameter`和`Node::set_parameter`函数来读取和更新节点参数。这些参数可以保存在ROS2的参数服务器中，以便在多个节点之间共享。

在`status_publisher.cpp`中添加以下代码来声明并读取节点参数：

this->declare_parameter<double>("speed", 0.5);
this->declare_parameter<double>("acceleration", 0.1);
this->declare_parameter<double>("turn_radius", 1.0);

double speed = this->get_parameter("speed").as_double();
double acceleration = this->get_parameter("acceleration").as_double();
double turn_radius = this->get_parameter("turn_radius").as_double();

RCLCPP_INFO(this->get_logger(), "speed = %f, acceleration = %f, turn_radius = %f", speed, acceleration, turn_radius);

这个代码段声明了三个参数：速度(`speed`)、加速度(`acceleration`)和转弯半径(`turn_radius`)。然后，使用`Node::get_parameter`函数来读取这些参数的值，并在终端上打印出来。

在`status_publisher.cpp`中添加以下代码来更新节点参数：

this->set_parameter("speed", 0.8);
this->set_parameter("acceleration", 0.2);
this->set_parameter("turn_radius", 1.5);

speed = this->get_parameter("speed").as_double();
acceleration = this->get_parameter("acceleration").as_double();
turn_radius = this->get_parameter("turn_radius").as_double();

RCLCPP_INFO(this->get_logger(), "updated parameters: speed = %f, acceleration = %f, turn_radius = %f", speed, acceleration, turn_radius);

这个代码段使用`Node::set_parameter`函数来更新节点参数的值，并使用`Node::get_parameter`函数来读取更新后的参数值。然后，在终端上打印出新的参数值。

完整的`status_publisher.cpp`代码如下：

#include "rclcpp/rclcpp.hpp"
#include "my_robot_msgs/msg/robot_status.hpp"

using namespace std::chrono_literals;

using RobotStatus = my_robot_msgs::msg::RobotStatus;

class StatusPublisher : public rclcpp::Node
{
public:
  StatusPublisher() : Node("status_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<RobotStatus>("robot_status", 10);
    timer_ = this->create_wall_timer(500ms, std::bind(&StatusPublisher::publish_status, this));

    this->declare_parameter<double>("speed", 0.5);
    this->declare_parameter<double>("acceleration", 0.1);
    this->declare_parameter<double>("turn_radius", 1.0);

    double speed = this->get_parameter("speed").as_double();
    double acceleration = this->get_parameter("acceleration").as_double();
    double turn_radius = this->get_parameter("turn_radius").as_double();

    RCLCPP_INFO(this->get_logger(), "speed = %f, acceleration = %f, turn_radius = %f", speed, acceleration, turn_radius);

    this->set_parameter("speed", 0.8);
    this->set_parameter("acceleration", 0.2);
    this->set_parameter("turn_radius", 1.5);

    speed = this->get_parameter("speed").as_double();
    acceleration = this->get_parameter("acceleration").as_double();
    turn_radius = this->get_parameter("turn_radius").as_double();

    RCLCPP_INFO(this->get_logger(), "updated parameters: speed = %f, acceleration = %f, turn_radius = %f", speed, acceleration, turn_radius);
  }

private:
  void publish_status()
  {
    auto message = RobotStatus();
    message.name = "robot1";
    message.x = 1.0;
    message.y = 2.0;
    message.z = 0.5;
    message.roll = 0.0;
    message.pitch = 0.0;
    message.yaw = 1.57;

    publisher_->publish(message);

    count_++;
  }

  rclcpp::Publisher<RobotStatus>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t count_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StatusPublisher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

完整的`status_subscriber.cpp`代码如下：

#include "rclcpp/rclcpp.hpp"
#include "my_robot_msgs/msg/robot_status.hpp"

using RobotStatus = my_robot_msgs::msg::RobotStatus;

class StatusSubscriber : public rclcpp::Node
{
public:
  StatusSubscriber() : Node("status_subscriber")
  {
    subscription_ = this->create_subscription<RobotStatus>("robot_status", 10, std::bind(&StatusSubscriber::status_callback, this, std::placeholders::_1));
  }

private:
  void status_callback(const RobotStatus::SharedPtr message)
  {
    RCLCPP_INFO(this->get_logger(), "Received robot status: name = %s, position = (%f, %f, %f), orientation = (%f, %f, %f)", message->name.c_str(), message->x, message->y, message->z, message->roll, message->pitch, message->yaw);
  }

  rclcpp::Subscription<RobotStatus>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StatusSubscriber>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}