Migrate pybullet_ros to ROS 2 Jazzy

CMakeLists.txt

@@ -1,9 +1,21 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(pybullet_ros)
 
-find_package(catkin REQUIRED COMPONENTS
-    std_msgs
-    cv_bridge
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(ament_index_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR pybullet_ros/${PROJECT_NAME})
+
+install(DIRECTORY config launch common resource
+  DESTINATION share/${PROJECT_NAME}
 )
 
-catkin_python_setup()
+ament_package()

config/robots/acrobat_robot_config.yaml

@@ -0,0 +1,21 @@
+plugins:
+  -
+    module: pybullet_ros.plugins.control
+    class: Control
+  -
+    module: pybullet_ros.plugins.joint_state_pub
+    class: joinStatePub
+  -
+    module: pybullet_ros.plugins.rgbd_camera
+    class: RGBDCamera
+
+loop_rate: 80.0
+gravity: -9.81
+max_effort: 5000.0
+use_intertia_from_file: False
+
+rgbd_camera:
+  frame_id: tip_link
+  resolution:
+    width: 640
+    height: 480

launch/bringup_robot_example.launch.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('pybullet_ros')
+    default_config = os.path.join(pkg_share, 'config', 'pybullet_params_example.yaml')
+    default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'r2d2_robot', 'r2d2.urdf.xacro')
+    pybullet_launch = os.path.join(pkg_share, 'launch', 'pybullet_ros_example.launch.py')
+
+    robot_description = ParameterValue(
+        Command(['xacro', LaunchConfiguration('robot_urdf_path')]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('config_file', default_value=default_config,
+                              description='YAML configuration file for pybullet_ros'),
+        DeclareLaunchArgument('plugin_import_prefix', default_value='pybullet_ros.plugins',
+                              description='Python module prefix used for plugin imports'),
+        DeclareLaunchArgument('environment', default_value='environment',
+                              description='Environment plugin name'),
+        DeclareLaunchArgument('pybullet_gui', default_value='true',
+                              description='Enable pybullet GUI'),
+        DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf,
+                              description='Robot URDF or xacro file to load'),
+        DeclareLaunchArgument('pause_simulation', default_value='false',
+                              description='Start simulation paused'),
+        DeclareLaunchArgument('parallel_plugin_execution', default_value='true',
+                              description='Execute plugins in parallel'),
+        DeclareLaunchArgument('robot_pose_x', default_value='0.0'),
+        DeclareLaunchArgument('robot_pose_y', default_value='0.0'),
+        DeclareLaunchArgument('robot_pose_z', default_value='0.7'),
+        DeclareLaunchArgument('robot_pose_yaw', default_value='0.0'),
+        DeclareLaunchArgument('fixed_base', default_value='false'),
+        DeclareLaunchArgument('use_deformable_world', default_value='false'),
+        DeclareLaunchArgument('gui_options', default_value=''),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'robot_description': robot_description}],
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(pybullet_launch),
+            launch_arguments={
+                'config_file': LaunchConfiguration('config_file'),
+                'plugin_import_prefix': LaunchConfiguration('plugin_import_prefix'),
+                'environment': LaunchConfiguration('environment'),
+                'pybullet_gui': LaunchConfiguration('pybullet_gui'),
+                'robot_urdf_path': LaunchConfiguration('robot_urdf_path'),
+                'pause_simulation': LaunchConfiguration('pause_simulation'),
+                'parallel_plugin_execution': LaunchConfiguration('parallel_plugin_execution'),
+                'robot_pose_x': LaunchConfiguration('robot_pose_x'),
+                'robot_pose_y': LaunchConfiguration('robot_pose_y'),
+                'robot_pose_z': LaunchConfiguration('robot_pose_z'),
+                'robot_pose_yaw': LaunchConfiguration('robot_pose_yaw'),
+                'fixed_base': LaunchConfiguration('fixed_base'),
+                'use_deformable_world': LaunchConfiguration('use_deformable_world'),
+                'gui_options': LaunchConfiguration('gui_options'),
+            }.items(),
+        ),
+    ])

launch/pybullet_ros_example.launch.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+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
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('pybullet_ros')
+    default_config = os.path.join(pkg_share, 'config', 'pybullet_params_example.yaml')
+    default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'r2d2_robot', 'r2d2.urdf')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('config_file', default_value=default_config,
+                              description='YAML configuration file for pybullet_ros'),
+        DeclareLaunchArgument('plugin_import_prefix', default_value='pybullet_ros.plugins',
+                              description='Python module prefix used for plugin imports'),
+        DeclareLaunchArgument('environment', default_value='environment',
+                              description='Environment plugin name'),
+        DeclareLaunchArgument('pybullet_gui', default_value='true',
+                              description='Enable pybullet GUI'),
+        DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf,
+                              description='Robot URDF file to load'),
+        DeclareLaunchArgument('pause_simulation', default_value='false',
+                              description='Start simulation paused'),
+        DeclareLaunchArgument('parallel_plugin_execution', default_value='true',
+                              description='Execute plugins in parallel'),
+        DeclareLaunchArgument('robot_pose_x', default_value='0.0',
+                              description='Robot spawn position X'),
+        DeclareLaunchArgument('robot_pose_y', default_value='0.0',
+                              description='Robot spawn position Y'),
+        DeclareLaunchArgument('robot_pose_z', default_value='1.0',
+                              description='Robot spawn position Z'),
+        DeclareLaunchArgument('robot_pose_yaw', default_value='0.0',
+                              description='Robot spawn yaw'),
+        DeclareLaunchArgument('fixed_base', default_value='false',
+                              description='Spawn robot with fixed base'),
+        DeclareLaunchArgument('use_deformable_world', default_value='false',
+                              description='Enable pybullet soft body simulation'),
+        DeclareLaunchArgument('gui_options', default_value='',
+                              description='Additional GUI options passed to pybullet'),
+        Node(
+            package='pybullet_ros',
+            executable='pybullet_ros_node',
+            name='pybullet_ros',
+            output='screen',
+            parameters=[
+                {
+                    'config_file': LaunchConfiguration('config_file'),
+                    'plugin_import_prefix': LaunchConfiguration('plugin_import_prefix'),
+                    'environment': LaunchConfiguration('environment'),
+                    'pybullet_gui': ParameterValue(LaunchConfiguration('pybullet_gui'), value_type=bool),
+                    'robot_urdf_path': LaunchConfiguration('robot_urdf_path'),
+                    'pause_simulation': ParameterValue(LaunchConfiguration('pause_simulation'), value_type=bool),
+                    'parallel_plugin_execution': ParameterValue(
+                        LaunchConfiguration('parallel_plugin_execution'), value_type=bool
+                    ),
+                    'robot_pose_x': ParameterValue(LaunchConfiguration('robot_pose_x'), value_type=float),
+                    'robot_pose_y': ParameterValue(LaunchConfiguration('robot_pose_y'), value_type=float),
+                    'robot_pose_z': ParameterValue(LaunchConfiguration('robot_pose_z'), value_type=float),
+                    'robot_pose_yaw': ParameterValue(LaunchConfiguration('robot_pose_yaw'), value_type=float),
+                    'fixed_base': ParameterValue(LaunchConfiguration('fixed_base'), value_type=bool),
+                    'use_deformable_world': ParameterValue(
+                        LaunchConfiguration('use_deformable_world'), value_type=bool
+                    ),
+                    'gui_options': LaunchConfiguration('gui_options'),
+                }
+            ],
+        ),
+    ])

launch/robots/acrobat_bringup.launch.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('pybullet_ros')
+    default_config = os.path.join(pkg_share, 'config', 'robots', 'acrobat_robot_config.yaml')
+    default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'acrobat_robot', 'acrobat_robot.urdf')
+    bringup_launch = os.path.join(pkg_share, 'launch', 'bringup_robot_example.launch.py')
+    tools_launch = os.path.join(pkg_share, 'launch', 'tools', 'position_cmd_gui.launch.py')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('config_file', default_value=default_config,
+                              description='Configuration for the acrobat robot'),
+        DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf,
+                              description='URDF file for the acrobat robot'),
+        DeclareLaunchArgument('robot_pose_z', default_value='0.0'),
+        DeclareLaunchArgument('fixed_base', default_value='true'),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(bringup_launch),
+            launch_arguments={
+                'config_file': LaunchConfiguration('config_file'),
+                'robot_urdf_path': LaunchConfiguration('robot_urdf_path'),
+                'robot_pose_z': LaunchConfiguration('robot_pose_z'),
+                'fixed_base': LaunchConfiguration('fixed_base'),
+            }.items(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(tools_launch),
+        ),
+    ])

launch/tools/position_cmd_gui.launch.py

@@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='position_ctrl_cmd_gui',
+            remappings=[('joint_states', 'fake_joint_states')],
+        ),
+        Node(
+            package='pybullet_ros',
+            executable='joint_states_to_float',
+            name='position_ctrl_repub',
+            output='screen',
+        ),
+    ])

package.xml

@@ -1,19 +1,32 @@
 <?xml version="1.0"?>
-<package>
-    <name>pybullet_ros</name>
-    <version>2.0.0</version>
-    <description>
-        ROS wrapper for pybullet simulator
-    </description>
+<package format="3">
+  <name>pybullet_ros</name>
+  <version>2.0.0</version>
+  <description>ROS wrapper for pybullet simulator</description>
 
-    <license>MIT</license>
+  <maintainer email="[email protected]">Oscar Lima</maintainer>
+  <author email="[email protected]">Oscar Lima</author>
+  <license>MIT</license>
 
-    <author email="[email protected]">Oscar Lima</author>
-    <maintainer email="[email protected]">Oscar Lima</maintainer>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
-    <buildtool_depend>catkin</buildtool_depend>
-    <run_depend>std_msgs</run_depend>
-    <run_depend>cv_bridge</run_depend>
-    <run_depend>joint_state_publisher_gui</run_depend>
+  <depend>ament_index_python</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_ros</depend>
+  <depend>cv_bridge</depend>
+  <depend>python3-yaml</depend>
+  <depend>xacro</depend>
 
+  <exec_depend>pybullet</exec_depend>
+  <exec_depend>pybullet_data</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>