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

1. 首先创建一个自定义消息类型，用于表示机器人的状态信息。

在ROS2中，我们可以使用ROS2的消息生成器ament_cmake来生成自定义消息。在终端中执行以下命令：

mkdir -p ~/ros2_ws/src/my_robot_msgs/msg
touch ~/ros2_ws/src/my_robot_msgs/msg/RobotStatus.msg

然后编辑RobotStatus.msg文件，代码如下：

# Robot status message

string robot_name    # 机器人名称
float32 x            # x坐标
float32 y            # y坐标
float32 z            # z坐标
float32 roll         # 滚动角度
float32 pitch        # 俯仰角度
float32 yaw          # 偏航角度

2. 创建一个发布者节点，周期性地发布机器人状态消息到话题上。

在ROS2中，我们可以使用Python编写节点。在终端中执行以下命令：

mkdir -p ~/ros2_ws/src/my_robot_state_publisher/nodes
touch ~/ros2_ws/src/my_robot_state_publisher/nodes/robot_state_publisher.py
chmod +x ~/ros2_ws/src/my_robot_state_publisher/nodes/robot_state_publisher.py

然后编辑robot_state_publisher.py文件，代码如下：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from my_robot_msgs.msg import RobotStatus  # 导入自定义消息包

class RobotStatePublisher(Node):
    def __init__(self):
        super().__init__('robot_state_publisher')
        self.publisher_ = self.create_publisher(RobotStatus, 'robot_status', 10)
        timer_period = 0.5  # 发布周期为0.5秒
        self.timer = self.create_timer(timer_period, self.publish_status)
        self.i = 0

    def publish_status(self):
        robot_status = RobotStatus()
        robot_status.robot_name = 'my_robot'
        robot_status.x = 1.0
        robot_status.y = 2.0
        robot_status.z = 0.0
        robot_status.roll = 0.0
        robot_status.pitch = 0.0
        robot_status.yaw = 0.0
        self.publisher_.publish(robot_status)
        self.get_logger().info('Publishing robot status: %s' % robot_status)

def main(args=None):
    rclpy.init(args=args)
    robot_state_publisher = RobotStatePublisher()
    rclpy.spin(robot_state_publisher)
    robot_state_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3. 创建一个订阅者节点，接收机器人状态消息，并在终端上显示机器人的名称和位置信息。

在终端中执行以下命令：

mkdir -p ~/ros2_ws/src/my_robot_state_subscriber/nodes
touch ~/ros2_ws/src/my_robot_state_subscriber/nodes/robot_state_subscriber.py
chmod +x ~/ros2_ws/src/my_robot_state_subscriber/nodes/robot_state_subscriber.py

然后编辑robot_state_subscriber.py文件，代码如下：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from my_robot_msgs.msg import RobotStatus  # 导入自定义消息包

class RobotStateSubscriber(Node):
    def __init__(self):
        super().__init__('robot_state_subscriber')
        self.subscription = self.create_subscription(RobotStatus, 'robot_status', self.callback, 10)
        self.subscription  # 防止被垃圾回收

    def callback(self, msg):
        self.get_logger().info('Robot status received: %s' % msg)
        print('Robot name:', msg.robot_name)
        print('Position: (%.2f, %.2f, %.2f)' % (msg.x, msg.y, msg.z))

def main(args=None):
    rclpy.init(args=args)
    robot_state_subscriber = RobotStateSubscriber()
    rclpy.spin(robot_state_subscriber)
    robot_state_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

4. 使用ROS2中的参数服务器来优化上述机器人的动作参数。

在ROS2中，我们可以使用Python编写节点来读取和更新参数。在终端中执行以下命令：

mkdir -p ~/ros2_ws/src/my_robot_params/nodes
touch ~/ros2_ws/src/my_robot_params/nodes/robot_params.py
chmod +x ~/ros2_ws/src/my_robot_params/nodes/robot_params.py

然后编辑robot_params.py文件，代码如下：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter

class RobotParams(Node):
    def __init__(self):
        super().__init__('robot_params')
        self.declare_parameter('speed', 1.0)
        self.declare_parameter('acceleration', 0.5)
        self.declare_parameter('turn_radius', 1.0)
        self.declare_parameter('camera_resolution', '640x480')
        self.get_logger().info('Parameters initialized: speed=%.2f, acceleration=%.2f, turn_radius=%.2f, camera_resolution=%s' % (self.get_parameter('speed').value, self.get_parameter('acceleration').value, self.get_parameter('turn_radius').value, self.get_parameter('camera_resolution').value))

def main(args=None):
    rclpy.init(args=args)
    robot_params = RobotParams()
    rclpy.spin(robot_params)
    robot_params.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

5. 运行节点。

在终端中执行以下命令：

source ~/ros2_ws/install/local_setup.bash
ros2 run my_robot_state_publisher robot_state_publisher
ros2 run my_robot_state_subscriber robot_state_subscriber
ros2 run my_robot_params robot_params

可以看到，机器人状态发布者每0.5秒发布一次机器人状态消息，机器人状态订阅者接收到机器人状态消息后，在终端上显示机器人的名称和位置信息。同时，参数节点初始化时输出了参数的初始值。可以使用ros2 param set命令来更新参数，例如：

ros2 param set /robot_params speed 2.0
ros2 param set /robot_params camera_resolution 1280x720

然后可以在参数节点的输出中看到参数已经被更新。