请用C++语言编写代码，要求如下：用ros2创建一个机器人状态监控系统，利用自定义消息和服务类型进行通信。步骤1：创建一个自定义消息另外i行，用于表示机器人的状态信息，消息中包含机器人名称（String类型），位置（float32类型的x,y,z坐标）和朝向（float32类型的roll,pitch,yaw角度） 步骤2：创建一个发布者节点，周期性地发布机器人状态消息到话题上。步骤3：创建一个订阅者节点，接受机器人状态消息，并在终端上显示机器人的名称和位置信息

以下是用C++编写的代码实现：

步骤1：创建一个自定义消息类型

在ROS2中，我们可以使用ROS2的工具生成自定义消息类型。在终端中输入以下命令：

ros2 pkg create my_robot_msgs --build-type ament_cmake --dependencies rclcpp std_msgs
ros2 interface package/create my_robot_msgs

在my_robot_msgs/msg文件夹下会生成一个名为RobotStatus.msg的文件，打开并编辑该文件，添加以下内容：

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

这个消息类型包含机器人名称和位置、朝向信息。

步骤2：创建一个发布者节点

在终端中输入以下命令创建一个名为robot_status_publisher的ROS2节点：

ros2 run my_robot_status robot_status_publisher

在src文件夹下创建robot_status_publisher.cpp文件，添加以下代码：

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

using namespace std::chrono_literals;

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("robot_status_publisher");
  auto publisher = node->create_publisher<my_robot_msgs::msg::RobotStatus>("robot_status", 10);

  auto timer_callback = [&publisher]() -> void {
    auto message = my_robot_msgs::msg::RobotStatus();
    message.robot_name = "robot1";
    message.x = 1.0f;
    message.y = 2.0f;
    message.z = 3.0f;
    message.roll = 0.0f;
    message.pitch = 0.0f;
    message.yaw = 0.0f;

    publisher->publish(message);
  };

  auto timer = node->create_wall_timer(1000ms, timer_callback);

  rclcpp::spin(node);

  rclcpp::shutdown();

  return 0;
}

这个节点会周期性地发布机器人状态消息到robot_status话题上。

步骤3：创建一个订阅者节点

在终端中输入以下命令创建一个名为robot_status_subscriber的ROS2节点：

ros2 run my_robot_status robot_status_subscriber

在src文件夹下创建robot_status_subscriber.cpp文件，添加以下代码：

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

using RobotStatus = my_robot_msgs::msg::RobotStatus;

void robot_status_callback(const RobotStatus::SharedPtr message)
{
  RCLCPP_INFO(rclcpp::get_logger("robot_status_subscriber"), "Received robot status: [%s] position: (%f, %f, %f)",
      message->robot_name.c_str(), message->x, message->y, message->z);
}

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("robot_status_subscriber");
  auto subscriber = node->create_subscription<RobotStatus>("robot_status", 10, robot_status_callback);

  rclcpp::spin(node);

  rclcpp::shutdown();

  return 0;
}

这个节点会接受机器人状态消息，并在终端上显示机器人的名称和位置信息。

在CMakeLists.txt文件中添加以下内容：

find_package(my_robot_msgs REQUIRED)

add_executable(robot_status_publisher src/robot_status_publisher.cpp)
ament_target_dependencies(robot_status_publisher rclcpp my_robot_msgs)

add_executable(robot_status_subscriber src/robot_status_subscriber.cpp)
ament_target_dependencies(robot_status_subscriber rclcpp my_robot_msgs)

编译并运行：

colcon build --packages-select my_robot_status
ros2 run my_robot_status robot_status_publisher
ros2 run my_robot_status robot_status_subscriber

现在就可以看到在终端上显示机器人的名称和位置信息了。