办公问答网

用户名  找回密码
 立即注册
帖子
热搜: 活动 交友 discuz
查看: 178|回复: 3

ROS 2演示教程(五)——在robot_state_publisher 节点中 ...

[复制链接]

2

主题

6

帖子

8

积分

新手上路

Rank: 1

积分
8
发表于 2022-9-20 08:32:32 | 显示全部楼层 |阅读模式
20 在robot_state_publisher 节点中使用URDF

目标:对使用URDF建模的步行机器人进行仿真,并在Rviz 中浏览仿真效果。
教程级别:中级
时间:15分钟
20.1 背景知识

本教程将会展示如何对步行机器人进行建模,将机器人状态发布为tf2消息并在Rviz中浏览仿真效果。首先是创建描述机器人装配的URDF模型。接着会编写一个节点来对机器人的运动进行仿真,并发布JointState和平移消息。然后会使用robot_state_publisher节点将整个机器人的状态消息发布到/tf2话题上。



20.2 前提条件

已经安装了rviz2软件包,并熟悉rviz2的使用方法。
与往常一样,不要忘记在打开的每个新终端中对ROS 2安装脚本进行source。
20.3 任务

20.3.1 创建软件包

运行以下命令来创建软件包:
mkdir -p ~/dev_ws/src  # change as needed
cd ~/dev_ws/src
ros2 pkg create urdf_tutorial --build-type ament_python --dependencies rclpy
cd urdf_tutorial
现在应该可以看到一个urdf_tutorial目录。接下来将会对该目录作出一些更改。
20.3.2 创建URDF文件

在urdf_tutorial目录下,新建一个用于存储urdf文件的子目录:
mkdir -p urdf
从这里下载URDF文件,并将其保存在~/dev_ws/src/urdf_tutorial/urdf目录中,文件名为r2d2.urdf.xml。同时下载这个Rviz配置文件,并将其保存在~/dev_ws/src/urdf_tutorial/urdf目录中,文件名为r2d2.rviz。
20.3.3 发布机器人状态消息

现在需要一个方法(函数)来指明机器人处于什么状态。为此,必须指明(发布)机器人的所有三个关节和整体的状态。
启动您最喜欢的编辑器并将以下代码粘贴到该软件包的子目录urdf_tutorial下的state_publisher.py文件中:
from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped

class StatePublisher(Node):
def __init__(self):
        rclpy.init()
        super().__init__('state_publisher')
        qos_profile = QoSProfile(depth=10)
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
        self.nodeName = self.get_name()
        self.get_logger().info("{0} started".format(self.nodeName))
        degree = pi / 180.0
        loop_rate = self.create_rate(30)
# robot state
        tilt = 0.
        tinc = degree
        swivel = 0.
        angle = 0.
        height = 0.
        hinc = 0.005
# message declarations
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'axis'
        joint_state = JointState()
try:
while rclpy.ok():
                rclpy.spin_once(self)
# update joint_state
                now = self.get_clock().now()
                joint_state.header.stamp = now.to_msg()
                joint_state.name = ['swivel', 'tilt', 'periscope']
                joint_state.position = [swivel, tilt, height]
# update transform
# (moving in a circle with radius=2)
                odom_trans.header.stamp = now.to_msg()
                odom_trans.transform.translation.x = cos(angle)*2
                odom_trans.transform.translation.y = sin(angle)*2
                odom_trans.transform.translation.z = 0.7
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
                self.joint_pub.publish(joint_state)
                self.broadcaster.sendTransform(odom_trans)
# Create new robot state
                tilt += tinc
if tilt < -0.5 or tilt > 0.0:
                    tinc *= -1
                height += hinc
if height > 0.2 or height < 0.0:
                    hinc *= -1
                swivel += degree
                angle += degree/4
# This will adjust as needed per iteration
                loop_rate.sleep()
except KeyboardInterrupt:
pass

def euler_to_quaternion(roll, pitch, yaw):
    qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
return Quaternion(x=qx, y=qy, z=qz, w=qw)

def main():
    node = StatePublisher()

if __name__ == '__main__':
    main()
20.3.4 创建启动文件

新建一个launch目录。打开编辑器并将以下代码粘贴并保存到launch/demo.launch.py文件中:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    urdf_file_name = 'r2d2.urdf.xml'
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial'),
        urdf_file_name)
with open(urdf, 'r') as infp:
        robot_desc = infp.read()

return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
            arguments=[urdf]),
        Node(
            package='urdf_tutorial',
            executable='state_publisher',
            name='state_publisher',
            output='screen'),
    ])
20.3.5 编辑setup.py文件

必须告知colcon构建工具如何安装该Python软件包。对setup.py文件进行以下编辑:
·在该文件开头插入以下4行Python库/模块导入语句:
import os
from glob import glob
from setuptools import setup
from setuptools import find_packages
·在data_files内添加以下两行代码:
data_files=[
  ...
  (os.path.join('share', package_name), glob('launch/*.py')),
  (os.path.join('share', package_name), glob('urdf/*'))
],
· 修改entry_points,以便以后可从控制台运行“state_publisher”节点:
'console_scripts': [
    'state_publisher = urdf_tutorial.state_publisher:main'
],
保存编辑后的setup.py文件。
20.3.6 安装软件包

运行以下命令来编译、构建和安装该软件包:
cd ~/dev_ws
colcon build --symlink-install --packages-select urdf_tutorial
source install/setup.bash
20.3.7 查看结果

用以下命令启动该软件包:
ros2 launch urdf_tutorial demo.launch.py
打开一个新的终端,并用以下命令运行Rviz:
rviz2 -d ~/dev_ws/install/urdf_tutorial/share/urdf_tutorial/r2d2.rviz
有关如何使用Rviz的详细信息,请参阅其用户指南。
20.4 小结

在本节教程中,创建了一个 JointState 发布者节点,并将其与 robots_state_publisher 节点耦合以对步行机器人进行仿真。可以在这里找到本示例中使用的代码。
归功于这个ROS 1 教程的作者,本教程对其中一些内容进行了重用。
*英语原文地址:https://docs.ros.org/en/foxy/Tutorials/URDF/Using-URDF-with-Robot-State-Publisher.html
回复

举报

0

主题

6

帖子

7

积分

新手上路

Rank: 1

积分
7
发表于 2022-9-20 08:33:28 | 显示全部楼层
把原来落下的几个演示教程再补上吧
回复

举报

0

主题

2

帖子

0

积分

新手上路

Rank: 1

积分
0
发表于 2022-9-20 08:34:16 | 显示全部楼层
launch里有两个结点,下面node就是发消息的,上面那个robot_state_publisher是做啥的呀
回复

举报

2

主题

6

帖子

11

积分

新手上路

Rank: 1

积分
11
发表于 4 天前 | 显示全部楼层
锄禾日当午,发帖真辛苦。谁知坛中餐,帖帖皆辛苦!
回复

举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|办公问答网

GMT+8, 2025-3-16 08:27 , Processed in 0.097479 second(s), 22 queries .

Powered by Discuz! X3.4

© 2001-2013 Comsenz Inc. Templated By 【未来科技 www.veikei.com】设计

快速回复 返回顶部 返回列表