Merge branch 'main' into 96-inspection-skill

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -1,29 +1,22 @@
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from ament_index_python.packages import get_package_share_directory 
-import os
-import yaml
-import math
-from rbs_launch_utils.launch_common import load_yaml
-
+# from ament_index_python.packages import get_package_share_directory 
+# import os
 
 def generate_launch_description():
 
-    # bt_config = os.path.join(
-    #     get_package_share_directory('rbs_bt_executor'),
-    #     'config',
-    #     'params.yaml'
-    # )
-    # points = load_yaml(
-    #     package_name="rbs_bt_executor",
-    #     file_path="config/points.yaml"
-    # )
-
-    gripperPoints = load_yaml(
-        package_name="rbs_bt_executor",
-        file_path="config/gripperPositions.yaml")
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "bt_file",
+            default_value="test_tree_env_manager.xml",
+            description="BehaviorTree file for run.",
+        )
+    )
+    bt_file = LaunchConfiguration("bt_file")
 
     robot_description_semantic_content = Command(
         [
@@ -42,14 +35,19 @@ def generate_launch_description():
         ]
     )
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
-    
-    
 
-    return LaunchDescription([
+    btfile_description = PathJoinSubstitution(
+        [FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
+    )
+    btfile_param = {"bt_file_path": btfile_description}
+
+    nodes_to_start = [
         Node(
             package='behavior_tree',
             executable='bt_engine',
             # prefix=['gdbserver localhost:3000'],
             parameters=["/home/bill-finger/rbs_ws/src/robossembler-ros2/rbs_bt_executor/config/params.yaml"]
         )
-    ])
+    ]
+
+    return LaunchDescription(declared_arguments + nodes_to_start)