之前我们学习了如何使用URDF来描述一个机器人，现在就开始学习如何让这个机器人控制跑起来。
首先，先把那个圆柱体补全成一个差速结构的小车。
下面开始编辑URDF文件，添加其他link和joint，每次添加后，需要重新编译代码，重启RVIZ2。
1. 添加imu
编写URDF，描述一个位于中心正上方2cm，长宽高均为2cm的正方体。
<!-- imu link --><link name="imu_link"><visual><origin xyz="0 0 0.0" rpy="0 0 0"/><geometry><box size="0.02 0.02 0.02"/></geometry></visual></link><!-- imu joint --><joint name="imu_joint" type="fixed"><parent link="base_link" /><child link="imu_link" /><origin xyz="0 0 0.02" /></joint>给base_link添加一条材料属性，让它变成蓝色且透明度为%50：
<!-- base link --><link name="base_link"><visual><origin xyz="0 0 0.0" rpy="0 0 0"/><geometry><cylinder length="0.12" radius="0.10"/></geometry><material name="blue"><color rgba="0.1 0.1 1.0 0.5" /> </material></visual></link>
2. 添加轮子
2.1 主动轮
2.1.1 编写link
描述：
名称： right_wheel_link
外形：宽为4cm,直径为6.4cm,几何形状是个圆柱体，黑色，透明度0.5
位姿：圆柱体默认朝上的，需要让其绕x轴旋转pi/2
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
完整的URDF描述：
<link name="right_wheel_link"><visual><origin xyz="0 0 0" rpy="1.57079 0 0"/><geometry><cylinder length="0.04" radius="0.032"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link>
2.1.2 编写joint
主要设置origin和axis值的设置。前者表示轮子与主体的相对位置。后者表示轮子运动的轴。
origin描述：
因为base_link的高度是0.12，原点为base_link该圆柱体的几何中心。
-
z表示child相对parent的z轴上的关系，想将轮子固定在机器人的下表面,所以origin的z向下偏移0.12/2=0.06m(向下符号为负)
-
y表示child相对parent的y轴上的关系，base_link的半径是0.10,所以我们让轮子的y轴向负方向偏移0.10m(向左符号为负)
-
x表示child相对parent的x轴上的关系，向后偏移则是x轴向后进行偏移，我们用个差不多的值0.02m(向后符号为负)
axis描述：
关节类型设置为continuous（旋转关节，可以绕单轴无限旋转），轮子的旋转轴为Y轴，即"0 1 0"
完整的Joint代码：
<!-- right_wheel joint-->
<joint name="right_wheel_joint" type="continuous"><parent link="base_link" /><child link="right_wheel_link" /><origin xyz="-0.02 -0.10 -0.06" /><axis xyz="0 1 0" /></joint>=
到此为止，主动轮的建模就完成了，至于左轮，和右轮对称，完整代码：
<!-- right_wheel link--><link name="right_wheel_link"><visual><origin xyz="0 0 0" rpy="1.57079 0 0"/><geometry><cylinder length="0.04" radius="0.032"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><!-- right_wheel joint--><joint name="right_wheel_joint" type="continuous"><parent link="base_link" /><child link="right_wheel_link" /><origin xyz="-0.02 -0.10 -0.06" /><axis xyz="0 1 0" /></joint><!-- left_wheel link--><link name="left_wheel_link"><visual><origin xyz="0 0 0" rpy="1.57079 0 0"/><geometry><cylinder length="0.04" radius="0.032"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><!-- left_wheel joint--><joint name="left_wheel_joint" type="continuous"><parent link="base_link" /><child link="left_wheel_link" /><origin xyz="-0.02 0.10 -0.06" /><axis xyz="0 1 0" /></joint>
2.2 支撑轮
支撑轮子固定在机器人的前方
描述：
小球的直径为0.032m与左右轮子半径相同；设置为半径为0.016m的球体；
主体的圆柱体高度0.12m；设置球体向下偏移0.016+0.06=0.076m,向下值为负
再把支撑轮向前移动0.06m
完整URDF代码：
<link name="caster_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><sphere radius="0.016"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><joint name="caster_joint" type="fixed"><parent link="base_link" /><child link="caster_link" /><origin xyz="0.06 0.0 -0.076" /></joint>
2.3 运行
完整代码：
<?xml version="1.0"?>
<robot name="fishbot"><!-- base link --><link name="base_link"><visual><origin xyz="0 0 0.0" rpy="0 0 0"/><geometry><cylinder length="0.12" radius="0.10"/></geometry><material name="blue"><color rgba="0.1 0.1 1.0 0.5" /> </material></visual></link><!-- laser link --><link name="laser_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><cylinder length="0.02" radius="0.02"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.8" /> </material></visual></link>为<!-- laser joint --><joint name="laser_joint" type="fixed"><parent link="base_link" /><child link="laser_link" /><origin xyz="0 0 0.075" /></joint><!-- imu link --><link name="imu_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><box size="0.02 0.02 0.02"/></geometry><material name="red"><color rgba="1.0 0.1 0.1 0.8" /> </material></visual></link><!-- imu joint --><joint name="imu_joint" type="fixed"><parent link="base_link" /><child link="imu_link" /><origin xyz="0 0 0.02" /></joint><!-- right_wheel link--><link name="right_wheel_link"><visual><origin xyz="0 0 0" rpy="1.57079 0 0"/><geometry><cylinder length="0.04" radius="0.032"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><!-- right_wheel joint--><joint name="right_wheel_joint" type="continuous"><parent link="base_link" /><child link="right_wheel_link" /><origin xyz="-0.02 -0.10 -0.06" /><axis xyz="0 1 0" /></joint><!-- left_wheel link--><link name="left_wheel_link"><visual><origin xyz="0 0 0" rpy="1.57079 0 0"/><geometry><cylinder length="0.04" radius="0.032"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><!-- left_wheel joint--><joint name="left_wheel_joint" type="continuous"><parent link="base_link" /><child link="left_wheel_link" /><origin xyz="-0.02 0.10 -0.06" /><axis xyz="0 1 0" /></joint><!-- caster link --><link name="caster_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><sphere radius="0.016"/></geometry><material name="black"><color rgba="0.0 0.0 0.0 0.5" /> </material></visual></link><!-- caster joint --><joint name="caster_joint" type="fixed"><parent link="base_link" /><child link="caster_link" /><origin xyz="0.06 0.0 -0.076" /></joint></robot>通过joint state的滑条可以控制两个轮子的运动。
2.4 让车轮着地
由于fixed-frame选择的是base_link，所以地面实际上是在主体圆柱体的几何中心上，也就是轮子是在地下的。
通过在添加一个虚拟link，作为base_link的父link，定义相对关系Joint，将base_link抬到地面上。
虚拟link的描述：
虚拟节点不需要形状等参数描述。
只需要设置joint，即与base_link的关系，即base_link在base_footprint的正上方0.076m（车轮直径【0.016m】+主圆柱体高度/2【0.06m】 = 0.76m）
<!-- Robot Footprint --><link name="base_footprint"/><joint name="base_joint" type="fixed"><parent link="base_footprint"/><child link="base_link"/><origin xyz="0.0 0.0 0.076" rpy="0 0 0"/></joint>
编译运行后，如下图所示，需要将Fixed Frame和Reference Frame更改为base_footprint。
然后可以看到，轮子现在处于地平面上。
3. 让车跑起来
运行后节点图
通过操控Joint_state_pubilsher_gui中的滑条，发布话题信息到Joint_states话题中，在传给robot_state_pubilsher最后通过robot_description话题与RVIZ2建立话题通信，在RVIZ2中对机器人的位姿进行更新并可视化。
如果要实现我们手动控制轮子，需要自己编写节点,取代joint_state_publisher发送关节位姿给robot_state_pubsher，robot_state_publisher再发送tf控制机器人的关节转动。
话题内容可以通过打印出来。
ros2 topic echo /joint_states
如下图所示，通过滑条控制轮子转动，并打印joint_states的话题信息。
内容如下：
---
header:stamp:sec: 1711443422nanosec: 148273725frame_id: ''
name:
- right_wheel_joint
- left_wheel_joint
position:
- 1.205743260447762
- 0.0
velocity: []
effort: []
---
大致实现步骤：
- 新建节点
- 创建发布者
- 编写发布逻辑
- 编译测试
3.1 新建节点
cd fishbot_ws
touch fishbot_description/fishbot_description/rotate_wheel.py
节点源码
#!/usr/bin/env python3
import rclpy
from rclpy.node import Nodeclass RotateWheelNode(Node):def __init__(self,name):super().__init__(name)self.get_logger().info(f"node {name} init..")def main(args=None):"""ros2运行该节点的入口函数1. 导入库文件2. 初始化客户端库3. 新建节点4. spin循环节点5. 关闭客户端库"""rclpy.init(args=args) # 初始化rclpynode = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）rclpy.shutdown() # 关闭rclpy
配置setup.py. 定义入口点
entry_points={'console_scripts': ["rotate_wheel= fishbot_description.rotate_wheel:main"],},
编译运行：
colcon build
source install/setup.bash
ros2 run fishbot_description rotate_wheel
执行节点：
3.2 创建发布者
创建发布者发布话题替代Joint_states话题。
首先需要直到Joint_states话题的类型：
通过如下命令查看
ros2 topic info /joint_states
返回结果：
Type: sensor_msgs/msg/JointState
Publisher count: 1
Subscription count: 1
可以看到该话题类型是 sensor_msgs/msg/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 headerstring[] name
float64[] position
float64[] velocity
float64[] effort
至此，明确了话题类型，可以开始编辑发布者代码：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointStateclass RotateWheelNode(Node):def __init__(self,name):super().__init__(name)self.get_logger().info(f"node {name} init..")# 2. 创建并初始化发布者成员属性pub_joint_statesself.pub_joint_states_ = self.create_publisher(JointState,"joint_states",10) # 缓存区域为10def main(args=None):"""ros2运行该节点的入口函数1. 导入库文件2. 初始化客户端库3. 新建节点4. spin循环节点5. 关闭客户端库"""rclpy.init(args=args) # 初始化rclpynode = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）rclpy.shutdown() # 关闭rclpy3.3 编写发布逻辑
3.3.1 多线程定频发布
现在有了发布者，需要发布者按照某个固定的速度发布话题信息去控制机器人。
为此，需要单独开一个线程，并使用多线程定频发布函数Rate。
import threading
from rclpy.node import Nodeclass 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()
3.3.2 构造发布数据
通过刚才的接口查询命令：
ros2 interfaces show sensor_msgs/msg/JointState
可以知道joint_state的数据格式：
# 这是一个持有数据的信息，用于描述一组扭矩控制的关节的状态。
#
# 每个关节（渐进式或棱柱式）的状态由以下因素定义。
# #关节的位置（rad或m）。
# #关节的速度（弧度/秒或米/秒）和
# #在关节上施加的力（Nm或N）。
#
# 每个关节都由其名称来唯一标识
# 头部规定了记录关节状态的时间。所有的联合状态
# 必须是在同一时间记录的。
#
# 这个消息由多个数组组成，每个部分的联合状态都有一个数组。
# 目标是让每个字段都是可选的。例如，当你的关节没有
# 扭矩与它们相关，你可以让扭矩数组为空。
#
# 这个信息中的所有数组都应该有相同的大小，或者为空。
# 这是唯一能将关节名称与正确的
# 状态。std_msgs/Header header #时间戳信息 和 frame_idstring[] name #关节名称数组
float64[] position #关节位置数组
float64[] velocity #关节速度数组
float64[] effort #扭矩数据，本实验暂时不用
设置header
self.joint_states.header.stamp = self.get_clock().now().to_msg()
self.joint_states.header.frame_id = ""
设置name
name是关节的名称，要与URDF中的定义的关节名称相同，根据前文的URDF定义有：
self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
设置position
表示关节转动的角度值，因为关节类型为continuous,所以其值无上下限制，初始值赋值为0.0
# 初始化关节的位置
self.joint_states.position = [0.0,0.0]# 轮子的位置通过速度对时间的积分求得
delta_time = time.time()-last_update_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]
设置velocity
因为是采用速度控制，所以需要一个控制速度的函数接口：
def update_speed(self,speeds):self.joint_speeds = speeds
完整代码：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointStateimport threading
import timeclass 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()# 设置Rate频率self.pub_rate = self.create_rate(30)# 创建线程self.thread_ = threading.Thread(target=self._thread_pub)# 开始线程self.thread_.start()def _init_joint_states(self):# 初始化jonit_states话题对象self.joint_states = JointState()# 设置headerself.joint_states.header.stamp = self.get_clock().now().to_msg()self.joint_states.header.frame_id = ""# 关节名称self.joint_states.name = ['left_wheel_joint','right_wheel_joint']# 关节的位置self.joint_states.position = [0.0,0.0]# 初始左右轮子的速度self.joint_speeds = [0.0,0.0]# 关节速度，通过joint_speed进行赋值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():# 一个更新周期，即前面设置的rate：1/30 sdelta_time = time.time()-last_update_timelast_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# 更新 headerself.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) # 初始化rclpynode = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点node.update_speed([15.0,-15.0])rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）rclpy.shutdown() # 关闭rclpy4. 关闭原发布者Joint_state_publisher_node
因为我们发布了自定义的joint_states话题，需要将原先的发布者关闭，直接修改下display_rviz2.launch.py文件，注释其joint_state_publisher节点。
ld.add_action(robot_state_publisher_node)
# ld.add_action(joint_state_publisher_node)
ld.add_action(rviz2_node)
5. 编译运行
方法一：
colcon build
source install/setup.bash# 先运行rotate_wheel程序来启动rotate_fishbot_wheel节点
ros2 run fishbot_description rotate_wheel
在另外开一个终端launch：display_rviz2.launch.py
source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py
方法二：
直接更改launch文件，把我们创建的rotate_fishbot_wheel节点放到display_rviz2.launch.py 一起启动。
在display_rviz2.launch.py 代码中添加：
rotate_fishbot_wheel=Node(package='fishbot_description', # Python节点所在的包名executable='rotate_wheel', # Python脚本名name='rotate_fishbot_wheel', # 节点的名称arguments=[urdf_model_path]
)
ld.add_action(rotate_fishbot_wheel)
完整display_rviz2.launch.py 代码：
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageSharedef generate_launch_description():package_name = 'fishbot_description'urdf_name = "fishbot_base.urdf"ld = LaunchDescription()pkg_share = FindPackageShare(package=package_name).find(package_name) urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')robot_state_publisher_node = Node(package='robot_state_publisher',executable='robot_state_publisher',arguments=[urdf_model_path])joint_state_publisher_node = Node(package='joint_state_publisher_gui',executable='joint_state_publisher_gui',name='joint_state_publisher_gui',arguments=[urdf_model_path])rviz2_node = Node(package='rviz2',executable='rviz2',name='rviz2',output='screen',)rotate_fishbot_wheel=Node(package='fishbot_description', # 替换为你的Python节点所在的包名executable='rotate_wheel', # 替换为你的Python脚本名name='rotate_fishbot_wheel', # 节点的名称arguments=[urdf_model_path])ld.add_action(rotate_fishbot_wheel)# ld.add_action(joint_state_publisher_node)ld.add_action(robot_state_publisher_node)ld.add_action(rviz2_node)return ld然后直接编译启动就行：
colcon build
source install/setup.bash
# 直接launch就行
ros2 launch fishbot_description display_rviz2.launch.py
运行效果如下，可以看到左右轮子以相同速度相反方向匀速转动：
6. 通过参数控制轮子
前面通过程序给轮子赋了一个固定的速度运行，现在通过设置两个参数，在通过rqt工具控制轮子转速。
在init时定义左右轮转速参数
self.declare_parameter('left_wheel_speed', 0.0) # 添加左轮速度参数，默认值为0.0
self.declare_parameter('right_wheel_speed', 0.0) # 添加右轮速度参数，默认值为0.0
通过参数给joint_states赋值
joint_states.velocity = [self.get_parameter('left_wheel_speed').value, self.get_parameter('right_wheel_speed').value]
注释掉之前的速度赋值相关代码，完整代码如下：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointState
import threading
import timeclass RotateWheelNode(Node):def __init__(self,name):super().__init__(name)self.get_logger().info(f"node {name} init..")# 定义左右轮子速度参数 self.declare_parameter('left_wheel_speed', 0.0) # 添加左轮速度参数，默认值为0.0self.declare_parameter('right_wheel_speed', 0.0) # 添加右轮速度参数，默认值为0.0# 创建并初始化发布者成员属性pub_joint_states_self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10) # 初始化数据self._init_joint_states()# 设置Rate频率self.pub_rate = self.create_rate(30)# 创建线程self.thread_ = threading.Thread(target=self._thread_pub)# 开始线程self.thread_.start()def _init_joint_states(self):# 初始化jonit_states话题对象self.joint_states = JointState()# 设置headerself.joint_states.header.stamp = self.get_clock().now().to_msg()self.joint_states.header.frame_id = ""# 关节名称self.joint_states.name = ['left_wheel_joint','right_wheel_joint']# 关节的位置self.joint_states.position = [0.0,0.0]# 初始左右轮子的速度self.joint_speeds = [0.0,0.0]# 关节速度，通过joint_speed进行赋值# self.joint_states.velocity = self.joint_speeds# 使用参数进行赋值，通过rqt工具设置参数的值以此控制轮子的速度self.joint_states.velocity = [self.get_parameter('left_wheel_speed').value, self.get_parameter('right_wheel_speed').value]# 力 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():# 一个更新周期，即前面设置的rate：1/30 sdelta_time = time.time()-last_update_timelast_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_speedsself.joint_states.velocity = [self.get_parameter('left_wheel_speed').value, self.get_parameter('right_wheel_speed').value]# 更新 headerself.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) # 初始化rclpynode = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点# node.update_speed([15.0,-15.0])rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）rclpy.shutdown() # 关闭rclpy
编译运行
colcon build
source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py
在开一个终端，运行rqt工具
新的运行节点图，可见我们替换了joint_states话题的发布者。更换为了rotate_fishbot_wheel节点。
通过rqt界面左上角Plugins -> Configuration -> Dynamic Reconfigure，然后可以看到如下界面。选择rotate_fishbot_wheel节点，设置left_wheel_speed和right_wheel_speed两个参数的值就可以动态设置左右论转速。
上面将左轮速度设置为-20，可以看到如图，左轮旋转，右轮静止：
