让可视化机器人动起来

harebert

已于 2024-04-27 14:16:12 修改

阅读量1.8k

点赞数 37

分类专栏： ROS 文章标签：机器人

于 2024-04-27 11:06:17 首次发布

本文链接：https://blog.csdn.net/harebert/article/details/138243852

版权

ROS 专栏收录该内容

7 篇文章 0 订阅

订阅专栏

前文《用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()

harebert

关注

37
点赞
踩
44

收藏

觉得还不错? 一键收藏
0
评论
让可视化机器人动起来

前文讲述了如何用rviz通过xml创建一个虚拟的机器人。每种不足的事，这个机器人无法进行运动。这篇就如何运动，来写一个node，替代之前的信息发布来给虚拟机器人的joint发送一些指令，让轮子可以转动起来。
复制链接

扫一扫

专栏目录