《ROS2机器人建模URDF》8.4控制移动机器人轮子运动_实验报告:基于ros2的轮式移动机器人模型构建

本系列教程作者：小鱼
公众号：鱼香ROS
QQ交流群：139707339
教学视频地址：小鱼的B站
完整文档地址：鱼香ROS官网
版权声明：如非允许禁止转载与商业用途。

8.4 控制移动机器人轮子运动

我是小鱼,本节我们来看看如何手动的发送joint_states来控制机器人轮子连续转动

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

1.新建节点

2.创建发布者

3.编写发布逻辑

4.编译测试

1.新建节点

方便起见，我们就在fishbot_describle包中新建节点（参考李四节点代码）

cd fishbot_ws
touch fishbot_description/fishbot_description/rotate_wheel.py
1
2

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


class 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) # 初始化rclpy
    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

配置下setup.py

    entry_points={
        'console_scripts': [
            "rotate_wheel= fishbot_description.rotate_wheel:main"
        ],
    },
1
2
3
4
5

编译运行

colcon build
source install/setup.bash
ros2 run fishbot_description rotate_wheel
1
2
3

2.创建发布者

创建发布者之前，要知道robot_state_pubsher所订阅的话题类型是什么？

回忆前面章节中学习的内容，我们可以采用如下指令查看

ros2 topic info /joint_states
1

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

接着

ros2 interfaces show sensor_msgs/msg/JointState
1

# 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

知道了话题类型，我们就可以来创建发布者了参考代码4.2.1 话题通信实现(Python)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointState

class RotateWheelNode(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info(f"node {name} init..")
        # 2.创建并初始化发布者成员属性pub_joint_states_
        self.pub_joint_states_ = self.create_publisher(JointState,"joint_states", 10) 
1
2
3
4
5
6
7
8
9
10
11
12

3.编写发布逻辑

3.1 多线程定频发布Rate

创建好发布者，我们想让话题按照某个固定的速度进行发布，可以采用ROS2中的定时神器Rate,不清楚Rate的小伙伴可以看看小鱼的这篇文章：ROS中的定频神器你会用吗

为了能够一直循环使用rate，我们单独开一个线程用于发布joint_states话题数据，在ROS2程序中单独开线程进行话题发布的方法为：

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()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

3.2 构造发布数据

接着我们来构造发布的数据：

joint_states有一个头和四个数组需要赋值（可通过ros2 interface指令查询）

std_msgs/Header header #时间戳信息 和 frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort
1
2
3
4
5

对应的含义为：

# 这是一个持有数据的信息，用于描述一组扭矩控制的关节的状态。
#
# 每个关节（渐进式或棱柱式）的状态由以下因素定义。
# #关节的位置（rad或m）。
# #关节的速度（弧度/秒或米/秒）和
# #在关节上施加的力（Nm或N）。
#
# 每个关节都由其名称来唯一标识
# 头部规定了记录关节状态的时间。所有的联合状态
# 必须是在同一时间记录的。
#
# 这个消息由多个数组组成，每个部分的联合状态都有一个数组。
# 目标是让每个字段都是可选的。例如，当你的关节没有
# 扭矩与它们相关，你可以让扭矩数组为空。
#
# 这个信息中的所有数组都应该有相同的大小，或者为空。
# 这是唯一能将关节名称与正确的
# 状态。
string[] name #关节名称数组
float64[] position #关节位置数组
float64[] velocity #关节速度数组
float64[] effort #扭矩数据
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

3.2.1 name

name是关节的名称，要与urdf中的定义的关节名称相同，根据我们的URDF定义有

self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
1

3.2.2 position

表示关节转动的角度值，因为关节类型为continuous,所以其值无上下限制，初始值赋值为0.0

# 关节的位置
self.joint_states.position = [0.0,0.0]
1
2

我们采用速度控制机器人轮子转动，所以机器人的位置更新则可以通过下面式子计算得出

某一段时间内轮子转动的角度 = （当前时刻-上一时刻）*两个时刻之间的轮子转速

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]
1
2
3
4

3.2.3 velocity

因为我们采用速度进行控制，所以对外提供一个速度更改接口。

def update_speed(self,speeds):
	self.joint_speeds = speeds
1
2
'
运行
运行

3.3 完成后代码

#!/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 = ['left_wheel_joint','right_wheel_joint']
        # 关节的位置
        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()
            # 发布关节数据
            self.joint_states_publisher_.publish(self.joint_states)
            self.pub_rate.sleep()

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
    node.update_speed([15.0,-15.0])
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

4.编译测试

编译程序

colcon build --packages-select fishbot_description
1

此时运行关节数据发布节点

ros2 run fishbot_description rotate_wheel
1

测试之前还需要修改下display_rviz2.launch.py文件，注释其joint_state_publisher节点

# ld.add_action(joint_state_publisher_node)
ld.add_action(robot_state_publisher_node)
ld.add_action(rviz2_node)
1
2
3

先运行rviz和robot_state_publisher

source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py
1
2

观察此时rviz界面，可以看到轮子疯狂转动

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vcEZqyIA-1650437375770)(./8.4控制移动机器人轮子运动/imgs/image-20220210212327950.png)]

5.课后练习

尝试将左右轮速度参数化，然后尝试采用rqt动态参数配置工具，实时控制轮子的转速。

---

技术交流&&问题求助：

• 微信公众号及交流群：鱼香ROS

• 小鱼微信：AiIotRobot

• QQ交流群：139707339

• 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

作者介绍：

我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚
初中学习编程，高中开始接触机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小鱼，一起交流技术，学习机器人