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

rbs_task_planner/launch/task_planner.launch.py

@@ -6,7 +6,7 @@ from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-# from launch_ros.actions import Node
+from launch_ros.actions import Node
 
 
 def load_file(package_name, file_path):
@@ -36,6 +36,7 @@ def load_yaml(package_name, file_path):
 def generate_launch_description():
 
     pkg_dir = get_package_share_directory('rbs_task_planner')
+    bt_exec_dir = get_package_share_directory('rbs_bt_executor')
     namespace = LaunchConfiguration('namespace')
 
     declare_namespace_cmd = DeclareLaunchArgument(
@@ -52,7 +53,6 @@ def generate_launch_description():
           'model_file': pkg_dir + '/domain/atomic_domain.pddl',
           'namespace': namespace
         }.items())
-    """
     assemble = Node(
         package='plansys2_bt_actions',
         executable='bt_action_node',
@@ -61,20 +61,16 @@ def generate_launch_description():
         output='screen',
         parameters=[
           pkg_dir + '/config/params.yaml',
-          robot_description,
-          robot_description_semantic,
-          kinematics_yaml,
           {
             'action_name': 'assemble',
-            # 'publisher_port': 1666,
-            # 'server_port': 1667,
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
+            'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml',
           }
         ])
-    """
     ld = LaunchDescription()
     ld.add_action(declare_namespace_cmd)
 
     # Declare the launch options
     ld.add_action(plansys2_cmd)
+    ld.add_action(assemble)
+
     return ld