Add demo launch args for Gazebo (Ignition) sim

henningkayser · henningkayser · commit 748afcf3b275 · 2023-05-31T18:54:19.000Z
diff --git a/panda_moveit_config/config/initial_positions.yaml b/panda_moveit_config/config/initial_positions.yaml
@@ -1,9 +1,9 @@
 # Default initial positions for the panda arm's ros2_control fake system
 initial_positions:
   panda_joint1: 0.0
-  panda_joint2: 0.0
+  panda_joint2: -0.785
   panda_joint3: 0.0
-  panda_joint4: 0.0
+  panda_joint4: -2.356
   panda_joint5: 0.0
   panda_joint6: 1.571
   panda_joint7: 0.785
diff --git a/panda_moveit_config/config/initial_positions_zero.yaml b/panda_moveit_config/config/initial_positions_zero.yaml
@@ -0,0 +1,14 @@
+# Initial positions for the panda arm's ros2_control Gazebo support
+# NOTE: These modified values from initial_positions.yaml move the default state
+# closer to 0 for Humble backwards support. The original values would cause
+# a strong initial motion, leading to self-collision. Reason is that initial_values
+# are not supported by ign_ros2_control
+# See: https://github.com/ros-controls/gz_ros2_control/pull/27
+initial_positions:
+  panda_joint1: 0.0
+  panda_joint2: 0.0
+  panda_joint3: 0.0
+  panda_joint4: 0.0
+  panda_joint5: 0.0
+  panda_joint6: 1.571
+  panda_joint7: 0.785
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
@@ -2,7 +2,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler
 from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals, LaunchConfigurationEquals
 from launch_ros.actions import Node
 from launch.actions import ExecuteProcess
 from launch.event_handlers import OnProcessExit
@@ -24,8 +24,22 @@ def generate_launch_description():
 
     ros2_control_hardware_type = DeclareLaunchArgument(
         "ros2_control_hardware_type",
-        default_value="sim_ignition",
-        description="ROS2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
+        default_value="mock_components",
+        description="ROS2 control hardware interface type to use for the launch file",
+        choices=['mock_components', 'isaac', 'sim_ignition']
+    )
+
+    declare_initial_positions_file = DeclareLaunchArgument(
+        "initial_positions_file",
+        default_value="initial_positions.yaml",
+        description="Initial joint positions to use for ros2_control fake components and simulation -- expected to be a yaml file inside the config directory",
+    )
+
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="False",
+        description="Use simulation (Gazebo) clock if True",
     )
 
     moveit_config = (
@@ -35,6 +49,9 @@ def generate_launch_description():
             mappings={
                 "ros2_control_hardware_type": LaunchConfiguration(
                     "ros2_control_hardware_type"
+                ),
+                "initial_positions_file": LaunchConfiguration(
+                    "initial_positions_file"
                 )
             },
         )
@@ -52,7 +69,8 @@ def generate_launch_description():
         package="moveit_ros_move_group",
         executable="move_group",
         output="log",
-        parameters=[moveit_config.to_dict()],
+        parameters=[moveit_config.to_dict(),
+                    {"use_sim_time": use_sim_time}],
         arguments=["--ros-args", "--log-level", "fatal"], # MoveIt is spamming the log because of unknown '*_mimic' joints
     )
 
@@ -89,6 +107,7 @@ def generate_launch_description():
             moveit_config.planning_pipelines,
             moveit_config.robot_description_kinematics,
             moveit_config.joint_limits,
+            {"use_sim_time": use_sim_time},
         ],
         condition=UnlessCondition(tutorial_mode),
     )
@@ -99,6 +118,7 @@ def generate_launch_description():
         executable="static_transform_publisher",
         name="static_transform_publisher",
         output="log",
+        parameters=[{"use_sim_time": use_sim_time}],
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
     )
 
@@ -108,62 +128,59 @@ def generate_launch_description():
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="both",
-        parameters=[moveit_config.robot_description],
+        parameters=[moveit_config.robot_description,
+                    {"use_sim_time": use_sim_time}],
     )
 
-    # NOTE: Comment out ros2_control for now
     # ros2_control using FakeSystem as hardware
-    # ros2_controllers_path = os.path.join(
-    #     get_package_share_directory("moveit_resources_panda_moveit_config"),
-    #     "config",
-    #     "ros2_controllers.yaml",
-    # )
-    # ros2_control_node = Node(
-    #     package="controller_manager",
-    #     executable="ros2_control_node",
-    #     parameters=[moveit_config.robot_description, ros2_controllers_path],
-    #     output="screen",
-    # )
-
-    # Parse xacro file to pass as string to Ignition
-    import xacro
-    robot_description_config = xacro.process_file(
-        os.path.join(
-            get_package_share_directory("moveit_resources_panda_moveit_config"),
-            "config",
-            "panda.urdf.xacro",
-        ),
-        mappings={"ros2_control_hardware_type": "sim_ignition"}
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="screen",
+        condition=LaunchConfigurationNotEquals("ros2_control_hardware_type", "sim_ignition")
     )
 
     ignition_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
-        arguments=[
-                   # '-string', robot_description_config.toxml(),
-                   '-topic', 'robot_description',
+        arguments=['-topic', 'robot_description',
                    '-name', 'panda',
                    '-allow-renaming', 'true'],
+        condition=LaunchConfigurationEquals("ros2_control_hardware_type", "sim_ignition")
     )
 
+    # Clock Bridge
+    sim_clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
+        output="screen",
+        condition=LaunchConfigurationEquals("ros2_control_hardware_type", "sim_ignition")
+    )
 
     load_joint_state_broadcaster = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
              'joint_state_broadcaster'],
         output='screen'
     )
 
-    panda_arm_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    panda_arm_controller_spawner = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'panda_arm_controller'],
+        output='screen'
     )
 
-    panda_hand_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    panda_hand_controller_spawner = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'panda_hand_controller'],
+        output='screen'
     )
 
     # Warehouse mongodb server
@@ -184,21 +201,22 @@ def generate_launch_description():
         tutorial_arg,
         db_arg,
         ros2_control_hardware_type,
+        declare_use_sim_time_cmd,
+        declare_initial_positions_file,
+        sim_clock_bridge,
         # Launch gazebo environment
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_ign_gazebo'),
                               'launch', 'ign_gazebo.launch.py')]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])],
+            condition=LaunchConfigurationEquals("ros2_control_hardware_type", "sim_ignition")),
         move_group_node,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=ignition_spawn_entity,
-                on_exit=[load_joint_state_broadcaster,
-                         panda_arm_controller_spawner,
-                         panda_hand_controller_spawner,],
-            )
-        ),
+        ignition_spawn_entity,
+        ros2_control_node,
+        load_joint_state_broadcaster,
+        panda_arm_controller_spawner,
+        panda_hand_controller_spawner,
         rviz_node,
         rviz_node_tutorial,
         static_tf_node,
