让可视化机器人动起来

前文《用RVIZ2创建一个可视化的机器人》讲述了如何用rviz通过xml创建一个虚拟的机器人。
美中不足的是，这个机器人无法进行运动。
这篇就如何运动，来写一个node，替代之前的信息发布来给虚拟机器人的joint发送一些指令，让轮子可以转动起来。

一、基本想法

观察rqt_graph，发现，我们需要自己编写节点,取代joint_state_publisher发送关节位姿给robot_state_pubsher，robot_state_publisher发送tf控制机器人的关节转动。

然后，编写代码实现一个 ROS 2 节点，名为 RotateWheelNode，用于模拟机器人轮子的旋转。它通过发布 JointState 消息来更新和控制轮子的转速和位置。

二、创建一个节点，用来发布信息

1、在创建节点之前，可以先查看一下消息的类型

ros2 topic info /joint_states

使用 ros2 topic info /joint_states 命令时，会查询关于 /joint_states 主题的详细信息。这个命令会显示以下信息：

Type: sensor_msgs/msg/JointState
Publisher count: 1
Subscription count: 1

**发布者的数量（Publishers）：**显示有多少个节点正在向 /joint_states 主题发布消息。
**订阅者的数量（Subscribers）：**显示有多少个节点正在订阅 /joint_states 主题。
**消息类型（Type）：**显示该主题用于传递的消息类型，对于 /joint_states 通常是 sensor_msgs/msg/JointState。

然后用命令输出该消息类型的所有字段及其数据类型。

ros2 interfaces show sensor_msgs/msg/JointState

消息定义
对于 sensor_msgs/msg/JointState，其定义通常包括以下字段：

std_msgs/Header header: 包含时间戳和坐标帧信息的标准消息头。
string[] name: 关节的名称数组。
double[] position: 每个关节的位置（通常是角度或线性位置）。
double[] velocity: 每个关节的速度。
double[] effort: 每个关节施加的力或扭矩。

知道了消息类型，就可以安排编写node了

先在package下新建rotate_wheel.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

# 1.导入消息类型JointState
from sensor_msgs.msg import JointState

import threading
import time

class RotateWheelNode(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info(f"node {
     
     name} init..")
        
         # 创建并初始化发布者成员属性pub_joint_states_
        self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10) 
        # 初始化数据
        self._init_joint_states()
        self.pub_rate = self.create_rate(30)
        self.thread_ = threading.Thread(target=self._thread_pub)
        self.thread_.start()
    def _init_joint_states(self):
        # 初始左右轮子的速度
        self.joint_speeds = [0.0,0.0]
        self.joint_states = JointState()
        self.joint_states.header.stamp = self.get_clock().now().to_msg()
        self.joint_states.header.frame_id = ""
        # 关节名称
        self.joint_states.name = ['joint_front_left','joint_front_right']
        # 关节的位置
        self.joint_states.position = [0.0,0.0]
        # 关节速度
        self.joint_states.velocity = self.joint_speeds
        # 力 
        self.joint_states.effort = []

    def update_speed(self,speeds):
        self.joint_speeds = speeds

    def _thread_pub(self):
        last_update_time = time.time()
        while rclpy.ok():
            delta_time =  time.time()-last_update_time
            last_update_time = time.time()
            # 更新位置
            self.joint_states.position[0]  += delta_time*self.joint_states.velocity[0]
            self.joint_states.position[1]  += delta_time*self.joint_states.velocity[1]
            # 更新速度
            self.joint_states.velocity = self.joint_speeds
            # 更新 header
            self.joint_states.header.stamp = self.get_clock().now().to_msg