diff --git a/src/ign_world/launch/moveit_launch.py b/src/ign_world/launch/moveit_launch.py index 78a23df..5b91ab2 100644 --- a/src/ign_world/launch/moveit_launch.py +++ b/src/ign_world/launch/moveit_launch.py @@ -4,11 +4,12 @@ import subprocess import yaml from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable from launch_ros.substitutions import FindPackageShare -from launch.substitutions import Command, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory +from launch.event_handlers import OnProcessExit def load_file(package_name, file_path): @@ -70,6 +71,15 @@ def generate_launch_description(): ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml') ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + robot_description_planning = { + "robot_description_planning": PathJoinSubstitution( + [ + FindPackageShare("iisy_config"), + "config/joint_limits.yml" + ] + ) + } + controllers_yaml = load_yaml('iisy_config', 'config/iisy_moveit_controllers.yml') moveit_controllers = {'moveit_simple_controller_manager': controllers_yaml, 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} @@ -92,6 +102,36 @@ def generate_launch_description(): } } + spawn_robot = Node( + package='ros_gz_sim', + executable='create', + arguments=['-world', 'empty', '-string', robot_description_config], + output='screen' + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[robot_description] + ) + + rviz = Node( + package='rviz2', + executable='rviz2', + # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'gpu_lidar_bridge.rviz')], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + ompl_planning_pipeline_config, + kinematics_yaml, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + # octomap_config = {'octomap_frame': 'camera_rgb_optical_frame', # 'octomap_resolution': 0.05, # 'max_range': 5.0} @@ -113,13 +153,14 @@ def generate_launch_description(): # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')) # ), - Node( - package='ros_gz_sim', - executable='create', - arguments=['-world', 'empty', '-string', robot_description_config], - output='screen' + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot, + on_exit=[robot_state_publisher], + ) ), + Node( package='tf2_ros', namespace='gaz_simulation', @@ -182,11 +223,7 @@ def generate_launch_description(): # parameters=[robot_description] # ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - parameters=[robot_description] - ), + # , # Node( @@ -213,8 +250,10 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - kinematics_yaml, + robot_description_planning, ompl_planning_pipeline_config, + kinematics_yaml, + planning, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, @@ -223,16 +262,7 @@ def generate_launch_description(): ] ), - Node( - package='rviz2', - executable='rviz2', - # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'gpu_lidar_bridge.rviz')], - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - planning, - ], - ), + spawn_robot, + rviz ]) diff --git a/src/ign_world/world/gaz_new_test.sdf b/src/ign_world/world/gaz_new_test.sdf index e328515..91a237b 100644 --- a/src/ign_world/world/gaz_new_test.sdf +++ b/src/ign_world/world/gaz_new_test.sdf @@ -40,7 +40,6 @@ /home/ros/standing_retract_arm.dae - diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf index abc47c9..03e0a16 100644 --- a/src/iisy_config/urdf/iisy.urdf +++ b/src/iisy_config/urdf/iisy.urdf @@ -372,8 +372,11 @@ + iisy $(find iisy_config)/config/iisy_controllers.yml - /iisy + + iisy +