前文《用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()
# 发布关节数据
self.joint_states_publisher_.publish(self.joint_states)
self.pub_rate.sleep()
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = RotateWheelNode("rotate_wheel_node") # 新建一个节点
node.update_speed([15.0,-15.0])
rclpy.spin(node) # 保持节点运行,检测是否收到退出指令(Ctrl+C)
rclpy.shutdown() # 关闭rclpy
三、编译发布
colcon build
source install/setup.bash
1、打开node
ros2 run rviz2_4_udf_package rotate_wheel
2、打开rviz
ros2 launch rviz2_4_udf_package display_rviz2.launch.py
rviz rotation1
四、纠正修改
我们在运行的时候发现,轮子的转动方向不是很正确。
一方面,我们在构建模型的时候,轮子的方向已经不正确了,另一方面,在转动的时候,代码是照着Z轴来转动的。所以会发现现在运动的轨迹非常诡异。
现在来对代码进行修改。
1、第一步,可以修改urdf
其中,要注意<axis xyz="0 0 1"/>
设置转动轴
<?xml version="1.0"?>
<robot name="mecanum_robot">
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.5 0.1"/> <!-- Length, Width, Height -->
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/> <!-- RGBA Colors -->
</material>
</visual>
<collision>
<geometry>
<box size="0.5 0.5 0.1"/>
</geometry>
</collision>
</link>
<!-- Mecanum Wheels -->
<!-- Wheel Front Left -->
<link name="wheel_front_left">
<visual>
<geometry>
<cylinder length="0.1" radius="0.05"/> <!-- Height and radius of the wheel -->
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<!-- Joint connecting front left wheel to base -->
<joint name="joint_front_left" type="continuous">
<parent link="base_link"/>
<child link="wheel_front_left"/>
<origin xyz="0.25 0.25 -0.05" rpy="1.5708 0 0"/> <!-- Position and rotation of the wheel -->
<axis xyz="0 0 1"/> <!-- Rotation axis -->
</joint>
<!-- Wheel Front Right -->
<link name="wheel_front_right">
<visual>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="joint_front_right" type="continuous">
<parent link="base_link"/>
<child link="wheel_front_right"/>
<origin xyz="0.25 -0.25 -0.05" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Wheel Rear Left -->
<link name="wheel_rear_left">
<visual>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="joint_rear_left" type="continuous">
<parent link="base_link"/>
<child link="wheel_rear_left"/>
<origin xyz="-0.25 0.25 -0.05" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Wheel Rear Right -->
<link name="wheel_rear_right">
<visual>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="joint_rear_right" type="continuous">
<parent link="base_link"/>
<child link="wheel_rear_right"/>
<origin xyz="-0.25 -0.25 -0.05" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
</robot>
2、再次编译发布
会发现,已经变得正常了。
rviz rotation2
3、最后一个问题
现在所有的情况看起来都是正常的,只不过我们的小车有四个轮子,现在能动的只有两个,为了让四个轮子都动起来,也为了能更好的理解rotate_wheel这个node。我们把代码再改一下。
要修改 RotateWheelNode 类以支持四个轮子(即前左、前右、后左和后右),需要扩展一些部分,包括关节名称、初始位置、速度,以及更新这些状态的方法。下面是如何调整代码以适应四个轮子的详细步骤:
a. 扩展关节状态初始化
需要修改 _init_joint_states 方法,以包括所有四个轮子的初始化数据。这涉及到扩展关节名称、位置和速度的列表。
b. 修改更新速度方法
update_speed 方法也需要修改,以接受四个轮子的速度。
c. 更新线程发布方法
_thread_pub 方法中,需要确保所有四个轮子的位置和速度都被正确更新。
下面是更新后的代码示例:
import rclpy
from rclpy.node import Node
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..")
# 创建并初始化发布者成员属性
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, 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', 'joint_rear_left', 'joint_rear_right']
# 关节的位置
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):
if len(speeds) == 4:
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()
# 更新所有四个轮子的位置
for i in range(4):
self.joint_states.position[i] += delta_time * self.joint_states.velocity[i]
# 更新速度
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_wheel_node") # 新建一个节点
node.update_speed([15.0, -15.0, 10.0, -10.0]) # 更新四个轮子的速度
rclpy.spin(node) # 保持节点运行
rclpy.shutdown() # 关闭rclpy
if __name__ == '__main__':
main()