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
+