From 83aaa02f1bf58ca9ced62c9adb84ef19ecd458ba Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 31 May 2023 18:54:19 +0000 Subject: [PATCH] Add demo launch args for Gazebo (Ignition) sim --- .../config/initial_positions_zero.yaml | 14 ++ panda_moveit_config/launch/demo.launch.py | 211 ++++++++++++------ panda_moveit_config/package.xml | 3 +- 3 files changed, 155 insertions(+), 73 deletions(-) create mode 100644 panda_moveit_config/config/initial_positions_zero.yaml diff --git a/panda_moveit_config/config/initial_positions_zero.yaml b/panda_moveit_config/config/initial_positions_zero.yaml new file mode 100644 index 00000000..54cfafbc --- /dev/null +++ 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 index cd8d86cd..68094b99 100644 --- a/panda_moveit_config/launch/demo.launch.py +++ b/panda_moveit_config/launch/demo.launch.py @@ -1,8 +1,17 @@ import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler +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 +33,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,7 +58,8 @@ def generate_launch_description(): mappings={ "ros2_control_hardware_type": LaunchConfiguration( "ros2_control_hardware_type" - ) + ), + "initial_positions_file": LaunchConfiguration("initial_positions_file"), }, ) .robot_description_semantic(file_path="config/panda.srdf") @@ -52,8 +76,12 @@ def generate_launch_description(): package="moveit_ros_move_group", executable="move_group", output="log", - parameters=[moveit_config.to_dict()], - arguments=["--ros-args", "--log-level", "fatal"], # MoveIt is spamming the log because of unknown '*_mimic' joints + 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 ) # RViz @@ -89,6 +117,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 +128,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 +138,87 @@ 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", + 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" ), - mappings={"ros2_control_hardware_type": "sim_ignition"} ) ignition_spawn_entity = Node( - package='ros_gz_sim', - executable='create', - output='screen', + package="ros_gz_sim", + executable="create", + output="screen", arguments=[ - # '-string', robot_description_config.toxml(), - '-topic', 'robot_description', - '-name', 'panda', - '-allow-renaming', 'true'], + "-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' + 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 @@ -180,28 +235,40 @@ def generate_launch_description(): condition=IfCondition(db_config), ) - return LaunchDescription([ - tutorial_arg, - db_arg, - ros2_control_hardware_type, - # 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'])]), - 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,], - ) - ), - rviz_node, - rviz_node_tutorial, - static_tf_node, - robot_state_publisher, - mongodb_server_node, - ]) + return LaunchDescription( + [ + 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_gz_sim"), + "launch", + "gz_sim.launch.py", + ) + ] + ), + launch_arguments=[("gz_args", [" -r -v 4 empty.sdf"])], + condition=LaunchConfigurationEquals( + "ros2_control_hardware_type", "sim_ignition" + ), + ), + move_group_node, + 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, + robot_state_publisher, + mongodb_server_node, + ] + ) diff --git a/panda_moveit_config/package.xml b/panda_moveit_config/package.xml index 6b84758f..6d483c59 100644 --- a/panda_moveit_config/package.xml +++ b/panda_moveit_config/package.xml @@ -33,7 +33,8 @@ joint_state_publisher joint_state_publisher_gui robot_state_publisher - ros_ign_gazebo + ros_gz_bridge + ros_gz_sim ros2controlcli xacro