Skip to content

Commit

Permalink
Add demo launch args for Gazebo (Ignition) sim
Browse files Browse the repository at this point in the history
  • Loading branch information
@henningkayser
henningkayser committed Jun 1, 2023
1 parent 0a5479f commit 83aaa02
Show file tree
Hide file tree
Showing 3 changed files with 155 additions and 73 deletions.
14 changes: 14 additions & 0 deletions panda_moveit_config/config/initial_positions_zero.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
211 changes: 139 additions & 72 deletions panda_moveit_config/launch/demo.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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 = (
Expand All @@ -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")
Expand All @@ -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
Expand Down Expand Up @@ -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),
)
Expand All @@ -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"],
)

Expand All @@ -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
Expand All @@ -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,
]
)
3 changes: 2 additions & 1 deletion panda_moveit_config/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>ros_ign_gazebo</exec_depend>
<exec_depend>ros_gz_bridge</exec_depend>
<exec_depend>ros_gz_sim</exec_depend>
<exec_depend>ros2controlcli</exec_depend>
<exec_depend>xacro</exec_depend>
<!-- TODO(#40): Package not available in ROS 2, find equivalent when migrating launch files
Expand Down

0 comments on commit 83aaa02

Please sign in to comment.