公司网络推广网站就选火13星仁德石家庄seo网站排名
视频讲解:
Rviz 同时显示多个独立 URDF!解决双机械臂+底盘等场景(球体+方块实例演示)
仓库地址:GitHub - LitchiCheng/ros2_package
有小伙伴留言说想看下同时使用多个独立的urdf如何配置,实际上这个场景是很常见的,比如双机械臂,机械臂加底盘,机械臂加双足等等,都是多个独立的urdf文件构成,这个时候同时在Rivz中显示多个模型就很有用了,今天我们以一个球体的urdf和一个方块的urdf为例,介绍下如何搭建。
首先新建一个package
ros2 pkg create --build-type ament_cmake multi_urdf_display --dependencies rclcpp robot_state_publisher tf2_ros
新建一个urdf文件夹
在urdf中新建cube.urdf
<?xml version="1.0"?>
<robot name="cube"><link name="base_link"><visual><geometry><box size="1 1 1"/></geometry><material name="blue"><color rgba="0 0 1 1"/></material></visual><collision><geometry><box size="1 1 1"/></geometry></collision><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/><inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/></inertial></link>
</robot>
再新建一个sphere.urdf
<?xml version="1.0"?>
<robot name="sphere"><link name="base_link"><visual><geometry><sphere radius="0.5"/></geometry><material name="red"><color rgba="1 0 0 1"/></material></visual><collision><geometry><sphere radius="0.5"/></geometry></collision><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/><inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/></inertial></link>
</robot>
创建一个launch文件夹,新建一个display_multi_urdf.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directorydef load_urdf(pkg_name, urdf_file):pkg_path = get_package_share_directory(pkg_name)urdf_path = os.path.join(pkg_path, 'urdf', urdf_file)with open(urdf_path, 'r') as f:return f.read()def generate_launch_description():robot_desc1 = load_urdf('multi_urdf_display', 'cube.urdf')robot_desc2 = load_urdf('multi_urdf_display', 'sphere.urdf')return LaunchDescription([Node(package='robot_state_publisher',executable='robot_state_publisher',name='cube_state_pub',namespace='cube',parameters=[{'robot_description': robot_desc1}], ),Node(package='robot_state_publisher',executable='robot_state_publisher',name='sphere_state_pub',namespace='sphere',parameters=[{'robot_description': robot_desc2}],),Node(package='tf2_ros',executable='static_transform_publisher',namespace='cube',arguments=['0', '0', '0', '0', '0', '0', 'world', 'cube/base_link'],),Node(package='tf2_ros',executable='static_transform_publisher',namespace='sphere',arguments=['2', '0', '0', '0', '0', '0', 'world', 'sphere/base_link'],),Node(package='rviz2',executable='rviz2',name='rviz2',arguments=['-d', os.path.join(get_package_share_directory('multi_urdf_display'),'rviz','display_multi_urdf.rviz')])])
创建一个rviz文件夹,新建一个display_multi_urdf.rviz
<?xml version="1.0" ?>
<rviz config_version="1.12"><VisualizationManager><GlobalOptions><FixedFrame value="world"/><FrameRate value="30"/></GlobalOptions><Display><Class value="rviz/RobotModel"/><Name value="Cube"/><Enabled value="true"/><RobotDescriptionTopic value="/cube/robot_description"/><TFPrefix value="cube"/></Display><Display><Class value="rviz/RobotModel"/><Name value="Sphere"/><Enabled value="true"/><RobotDescriptionTopic value="/sphere/robot_description"/><TFPrefix value="sphere"/></Display></VisualizationManager>
</rviz>
增加如下到CMakeLists.txt文件
install(DIRECTORY launch urdf rvizDESTINATION share/${PROJECT_NAME})
编译运行
colcon build --packages-select multi_urdf_display
source install/setup.bash
ros2 launch multi_urdf_display display_multi_urdf.launch.py
添加pannel Displays后
添加两个RobotModel后,需要配置Topic以及TF Prefix,可以看到两个URDF都显示在了Rviz中