|
|
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__ == &#39;__main__&#39;:
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(&#39;use_sim_time&#39;, default=&#39;false&#39;)
urdf_file_name = &#39;r2d2.urdf.xml&#39;
urdf = os.path.join(
get_package_share_directory(&#39;urdf_tutorial&#39;),
urdf_file_name)
with open(urdf, &#39;r&#39;) as infp:
robot_desc = infp.read()
return LaunchDescription([
DeclareLaunchArgument(
&#39;use_sim_time&#39;,
default_value=&#39;false&#39;,
description=&#39;Use simulation (Gazebo) clock if true&#39;),
Node(
package=&#39;robot_state_publisher&#39;,
executable=&#39;robot_state_publisher&#39;,
name=&#39;robot_state_publisher&#39;,
output=&#39;screen&#39;,
parameters=[{&#39;use_sim_time&#39;: use_sim_time, &#39;robot_description&#39;: robot_desc}],
arguments=[urdf]),
Node(
package=&#39;urdf_tutorial&#39;,
executable=&#39;state_publisher&#39;,
name=&#39;state_publisher&#39;,
output=&#39;screen&#39;),
])
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(&#39;share&#39;, package_name), glob(&#39;launch/*.py&#39;)),
(os.path.join(&#39;share&#39;, package_name), glob(&#39;urdf/*&#39;))
],
· 修改entry_points，以便以后可从控制台运行“state_publisher”节点：
&#39;console_scripts&#39;: [
&#39;state_publisher = urdf_tutorial.state_publisher:main&#39;
],
保存编辑后的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 |
|
发表于 2022-9-20 08:32:32