Motion planner (MoveIt) and Task planner (Plansys) integration

rbs_simulation/launch/rbs_simulation.launch.py

@@ -260,7 +260,7 @@ def generate_launch_description():
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_ign_gazebo'),
                               'launch', 'ign_gazebo.launch.py')]),
-            launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
+                launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
     # Spawn robot
     gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
                  arguments=[
@@ -380,16 +380,16 @@ def generate_launch_description():
         ]
     )
 
-    move_topose_action_server = Node(
-        package="rbs_skill_servers",
-        executable="move_topose_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ]
-    )
+    #move_topose_action_server = Node(
+    #    package="rbs_skill_servers",
+    #    executable="move_topose_action_server",
+    #    parameters=[
+    #        robot_description,
+    #        robot_description_semantic,
+    #        robot_description_kinematics,
+    #        {"use_sim_time": use_sim_time},
+    #    ]
+    #)
 
     move_cartesian_path_action_server = Node(
         package="rbs_skill_servers",
@@ -424,6 +424,16 @@ def generate_launch_description():
             points_params
         ]
     )
+
+    assemble_state = Node(
+        package="rbs_skill_servers",
+        executable="assemble_state_service_server",
+        output="screen",
+        parameters=[
+            {'assemble_prefix': 'ASSEMBLE_'},
+            {'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'}
+        ]
+    )
     
     # add_planning_scene_object = Node(
     #     package="rbs_skill_servers",
@@ -452,7 +462,8 @@ def generate_launch_description():
         move_cartesian_path_action_server,
         move_joint_state_action_server,
         grasp_pose_loader,
+        assemble_state,
         #add_planning_scene_object
     ]
 
-    return LaunchDescription(declared_arguments + nodes_to_start)
+    return LaunchDescription(declared_arguments + nodes_to_start)

rbs_simulation/worlds/mir.sdf

@@ -290,4 +290,4 @@
         </link>
       </model>  
   </world>
-</sdf>
+</sdf>