之前我们学习了如何使用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() # 关闭rclpy
3.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() # 关闭rclpy
4. 关闭原发布者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,可以看到如图,左轮旋转,右轮静止: