将URDF与robot_state_发布者结合使用 [待校准@9049]
Goal目标: Simulate模拟一个在URDF中建模的步行机器人，并在Rviz中查看它。 [待校准@9050]
Tutorial教程级别: Intermediate中级 [待校准@6713]
时间: 15分钟 [Alyssa@6755]
内容
背景
本教程将向您展示如何为步行机器人建模，将状态发布为 tf2 信息，并在Rviz中查看仿真。首先，我们创建描述机器人装配的URDF模型。接下来，我们编写一个模拟运动并发布连接状态和转换的节点。然后，我们使用 robot_state_publisher 将整个机器人状态发布给 /tf2 。 [待校准@9051]
任务
1创建包
mkdir -p ~/dev_ws/src # change as needed
cd ~/dev_ws/src
ros2 pkg create urdf_tutorial_r2d2 --build-type ament_python --dependencies rclpy
cd urdf_tutorial_r2d2
你现在应该看到一个 urdf_tutorial_r2d2 文件夹。接下来，您将对其进行一些更改。 [待校准@9053]
2创建URDF文件 [待校准@9054]
创建我们将在其中存储一些资产的目录: [待校准@9055]
mkdir -p urdf
下载: URDF file and save it as ~/dev_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml. Download the Rviz configuration file ，并将其另存为 “~/dev_ws/src/urdf_tutorial_r2d2/urdf/r2d2.Rviz”。 [待校准@9056]
3发布状态 [待校准@9057]
现在我们需要一种方法来指定机器人处于什么状态。为此，我们必须指定所有三个关节和整体里程计。 [待校准@9058]
启动您最喜欢的编辑器，并将以下代码粘贴到 urdf_tutorial_r2d2/state_publisher.py 中 [待校准@9059]
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()
4创建一个launch文件 [待校准@9060]
创建一个新的 launch 文件夹。打开编辑器并粘贴以下代码，将其保存为 launch/demo.launch.py [待校准@9061]
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_r2d2'),
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_r2d2',
executable='state_publisher',
name='state_publisher',
output='screen'),
])
5编辑setup.py文件 [待校准@9062]
您必须告诉colcon构建工具如何安装Python包。按如下方式编辑 setup.py 文件: [待校准@9063]
包括这些导入语句 [待校准@9064]
import os
from glob import glob
from setuptools import setup
from setuptools import find_packages
将这两条线添加到
data_files中 [待校准@9065]
data_files=[
...
(os.path.join('share', package_name), glob('launch/*.py')),
(os.path.join('share', package_name), glob('urdf/*'))
],
修改
entry_points表，以便以后可以从控制台运行'state_publisher' [待校准@9066]
'console_scripts': [
'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],
用您的更改保存 setup.py 文件。 [待校准@9067]
6安装软件包 [待校准@9068]
cd ~/dev_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2
source install/setup.bash
7查看结果 [待校准@9069]
Launch包装 [待校准@9070]
ros2 launch urdf_tutorial_r2d2 demo.launch.py
打开一个新的终端，运行Rviz使用 [待校准@9071]
rviz2 -d ~/dev_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz
有关如何使用Rviz的详细信息，请参阅 User Guide 。 [待校准@9072]
总结
您创建了一个JointState发布者节点，并将其与 robot_state_publisher 耦合以模拟步行机器人。在这些例子中使用的代码可以找到 here 。 [待校准@9073]
这份 ROS 1 tutorial 的作者受到了赞扬，其中一些内容被重复使用。 [待校准@9074]