5.通过JointStates控制关节转动

5.通过JointStates控制关节转动

原理：

在上节，我们通过创建 joint_state_publisher 与 robot_states_publisher 来对机器人模型进行了可视化， robot_states_publisher 通过发布 URDF 来将我们的机器人模型信息告诉给 RVIZ2，以便RVIZ2进行显示；joint_state_publisher 则负责将机器人关节的位姿状态信息以 /joint_states 的话题形式发布出来以供 robot_state_publisher 进行接收并转换为TF来控制RVIZ2中机器人模型关节的转动。

在本节，为了实现小车模型关节转动的效果，我们需要自己编写节点来取代默认的 joint_state_publisher ，自己构造并发送 joint_states话题。

新建结点

首先在 ros_car_description/ros_car_description下创建一个py文件，用于编写关节位姿信息发布结点。

rotate_wheel.py：

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

class RotateWheelNode(Node):  # 创建一个控制轮子转动的类，继承 Node 类型
    def __init__(self,name):
        super().__init__(name)  # 初始化父类
        self.get_logger().info(f"node {name} init..")  # 输出结点初始化测试信息

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy，即ROS2的 Python 控制库
    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个关节位姿信息发布节点
    rclpy.spin(node) # 保持节点运行，监测并处理结点的相关信息
    rclpy.shutdown() # 若退出了节点，则关闭rclpy，释放占用的资源

我们新建了一个py文件，编写了一个结点用于发布机器人的关节位姿信息后，还需要配置setup.py文件，指明程序的入口函数，修改如下部分：

    entry_points={
        'console_scripts': [
            "rotate_wheel= ros_car_description.rotate_wheel:main"
        ],
    },

至此，我们完成了编写结点前的准备工作，接下来对我们需要开展的工作作进一步分析。

创建发布者

在之前的文章中我们知道了 robot_state_publisher 通过订阅 joint_state_publisher 发布的 /joint_states 话题来接收机器人的关节位姿信息，接收处理之后转化成 TF 数据来控制机器人模型的关节位姿。那么我们的发布者想要发布一个 /joint_states 的话，则需要先了解这个话题的消息接口类型及形式，进而依照其消息接口类型及形式来编写消息，并发布出去，通过以下指令来查看 /joint_states 话题的消息接口：

ros2 topic info /joint_states

显示如下(下方的count代表着目前正在发布与订阅该话题的结点数量)：

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

至此，我们知道 /joint_states 话题发布的是一个以 JointState 为类型的消息，我们进一步探索这个JointState消息，了解其构成。通过以下命令来查看该种消息接口类型的具体内容：

ros2 interfaces show sensor_msgs/msg/JointState

显示如下：

# This is a message that holds data to describe the state of a set of torque controlled joints.
#
# The state of each joint (revolute or prismatic) is defined by:
#  * the position of the joint (rad or m),
#  * the velocity of the joint (rad/s or m/s) and
#  * the effort that is applied in the joint (Nm or N).
#
# Each joint is uniquely identified by its name
# The header specifies the time at which the joint states were recorded. All the joint states
# in one message have to be recorded at the same time.
#
# This message consists of a multiple arrays, one for each part of the joint state.
# The goal is to make each of the fields optional. When e.g. your joints have no
# effort associated with them, you can leave the effort array empty.
#
# All arrays in this message should have the same size, or be empty.
# This is the only way to uniquely associate the joint name with the correct
# states.

std_msgs/Header header

string[] name
float64[] position
float64[] velocity
float64[] effort

header：此处又是一个消息接口，其消息接口类型是 std_msgs/Header。包含着消息的元数据（数据的数据称之为元数据），其包括时间戳和坐标框架。

name：关节的名称数组。每个名称对应一个关节，用于唯一标识各个关节。

position：关节位置的数组。每个元素表示对应关节的当前位置，单位为弧度或米。

velocity：关节速度的数组。每个元素表示对应关节的当前速度，单位为弧度每秒或米每秒。

effort：关节力矩的数组。每个元素表示对应关节的当前力矩，单位为牛·米或牛。

好，至此，我们知道了要使用JointState类型的消息接口文件，通过编写这几个参数后才能发布一个标准的 /joint_states 话题，供 robot_state_description 话题订阅。

话题的发布者对于话题的发布是异步发布的，即发布者会按照自己的节奏来发布话题信息，在这里，我们为其创建一个发布周期以实现定期发布话题信息的效果，在这里我们使用ROS2中的 rate 定时频率器来实现这个效果，这里指出实现其功能的主要功能代码：

import threading
from rclpy.node import Node

class RotateWheelNode(Node):
    def __init__(self):
        # 创建一个Rate和线程
        self.pub_rate = self.create_rate(5) #5Hz
        
        # 创建线程
        self.thread_ = threading.Thread(target=self._thread_pub)
        self.thread_.start()

    def _thread_pub(self):
        while rclpy.ok():
            # 使用 rate 来保证循环频率
            self.pub_rate.sleep()

整个编写完成后的代码如下
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..")
        # 创建并初始化发布者,发布者发布的消息类型、话题名称、队列长度(如果发布者发送消息的速度超过了接收者处理消息的速度，最多可以在队列中保存 10 条消息。超过这个数量的消息将被丢弃。)
        self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10)
        
        # 初始化关节及各项数据
        self._init_joint_states()
        
        # 初始化 rate 定时器，使其以30Hz为周期
        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,0.0,0.0]  # 初始全部置0
        
        # 创建机器人关节对象
        self.joint_states = JointState()
        
        # 时间戳
        self.joint_states.header.stamp = self.get_clock().now().to_msg()
        
        # frame_id,用于指定数据所关联的坐标框架，在这里暂时留空即可，不做设置
        self.joint_states.header.frame_id = ""
        
        # 关节名称，要与我们 urdf 模型中的关节一一对应起来
        self.joint_states.name = ['left_wheel1_joint','left_wheel2_joint','right_wheel1_joint','right_wheel2_joint']
        
        # 关节的位置，即当前角度
        self.joint_states.position = [0.0,0.0,0.0,0.0]
        
        # 关节速度，关节的转动速度
        self.joint_states.velocity = self.joint_speeds
        
        # 关节的力矩，留空即可
        self.joint_states.effort = []

		# 关节速度设置函数，用于控制及更新关节转速。
    def update_speed(self,speeds):
        self.joint_speeds = speeds

		# 线程函数，用于时刻更新关节位姿，在此使用线程来执行它，因为ros系统正常运行就需要一直运行这个关节位姿发布函数，其后还有一个node的spin函数需要执行，执行不能卡死在此
    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.position[2]  += delta_time*self.joint_states.velocity[2]
            self.joint_states.position[3]  += delta_time*self.joint_states.velocity[3]
            
            # 更新速度
            self.joint_states.velocity = self.joint_speeds
            
            # 更新 header
            self.joint_states.header.stamp = self.get_clock().now().to_msg()
            
            # 发布关节数据
            self.joint_states_publisher_.publish(self.joint_states)
            
            # 睡眠，产生周期
            self.pub_rate.sleep()

def main(args=None):
    rclpy.init(args=args)  # 初始化rclpy
    node = RotateWheelNode("rotate_ros_car_wheel")  # 新建一个节点
    node.update_speed([15.0,15.0,15.0,15.0])  # 为关节赋予速度值
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy

编译测试

在运行我们编写的结点并发布 /joint_states 话题之前，我们需要先注释掉 urdf_node.launch.py 文件中的这一行，关闭掉这个结点来避免其发布默认的 /joint_states 话题：

# ld.add_action(joint_state_publisher_node)
ld.add_action(robot_state_publisher_node)

编译程序（编译指定功能包形式）：

colcon build --packages-select ros_car_description

运行 urdf_node.launch.py ，发布机器人模型

ros2 launch ros_car_description urdf_node.launch.py

运行 /joint_states 关节位姿发布结点，发布话题信息。

ros2 run ros_car_description rotate_wheel

运行RVIZ2便可观察到我们的小车模型的轮子在转动了。