BT for pose estimation

rbs_bt_executor/bt_trees/6Dpe_test.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="PoseEstimation">
+    <BehaviorTree ID="PoseEstimation">
+        <Sequence>
+            <Action ID="PoseEstimation" 
+            ObjectName="fork"
+            ReqKind = "configure"
+            server_name="/image_sub2/change_state" 
+            server_timeout="1000"/>
+            <Action ID="PoseEstimation" 
+            ObjectName="None"
+            ReqKind = "activate"
+            server_name="/image_sub2/change_state" 
+            server_timeout="1000"/>
+        </Sequence>
+    </BehaviorTree>
+</root>

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -8,82 +8,98 @@ import yaml
 import math
 
 
-def construct_angle_radians(loader, node):
-    """Utility function to construct radian values from yaml."""
-    value = loader.construct_scalar(node)
-    try:
-        return float(value)
-    except SyntaxError:
-        raise Exception("invalid expression: %s" % value)
+# def construct_angle_radians(loader, node):
+#     """Utility function to construct radian values from yaml."""
+#     value = loader.construct_scalar(node)
+#     try:
+#         return float(value)
+#     except SyntaxError:
+#         raise Exception("invalid expression: %s" % value)
 
-def construct_angle_degrees(loader, node):
-    """Utility function for converting degrees into radians from yaml."""
-    return math.radians(construct_angle_radians(loader, node))
+# def construct_angle_degrees(loader, node):
+#     """Utility function for converting degrees into radians from yaml."""
+#     return math.radians(construct_angle_radians(loader, node))
 
-def load_yaml(package_name, file_path):
-    package_path = get_package_share_directory(package_name)
-    absolute_file_path = os.path.join(package_path, file_path)
+# def load_yaml(package_name, file_path):
+#     package_path = get_package_share_directory(package_name)
+#     absolute_file_path = os.path.join(package_path, file_path)
 
-    try:
-        yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
-        yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
-    except Exception:
-        raise Exception("yaml support not available; install python-yaml")
+#     try:
+#         yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
+#         yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
+#     except Exception:
+#         raise Exception("yaml support not available; install python-yaml")
 
-    try:
-        with open(absolute_file_path) as file:
-            return yaml.safe_load(file)
-    except OSError:  # parent of IOError, OSError *and* WindowsError where available
-        return None
+#     try:
+#         with open(absolute_file_path) as file:
+#             return yaml.safe_load(file)
+#     except OSError:  # parent of IOError, OSError *and* WindowsError where available
+#         return None
+
+
+# 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")
+
+#     robot_description_semantic_content = Command(
+#         [
+#             PathJoinSubstitution([FindExecutable(name="xacro")]),
+#             " ",
+#             PathJoinSubstitution(
+#                 [FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
+#             ),
+#             " ",
+#             "name:=",
+#             "ur",
+#             " ",
+#             "prefix:=",
+#             "",
+#             " ",
+#         ]
+#     )
+#     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
+    
+
+
+#     return LaunchDescription([
+#         Node(
+#             package='behavior_tree',
+#             namespace='',
+#             executable='bt_engine',
+#             #prefix=['xterm -e gdb -ex run --args'],
+#             parameters=[
+#                 {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
+#                 {'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
+#                 gripperPoints,
+#                 robot_description_semantic
+#             ]
+#         )
+#     ])
 
 
 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")
-
-    robot_description_semantic_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
-            ),
-            " ",
-            "name:=",
-            "ur",
-            " ",
-            "prefix:=",
-            "",
-            " ",
-        ]
-    )
-    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
-    
-
-
     return LaunchDescription([
         Node(
             package='behavior_tree',
             namespace='',
             executable='bt_engine',
-            #prefix=['xterm -e gdb -ex run --args'],
+            prefix=['xterm -e gdb -ex run --args'],
             parameters=[
-                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
-                {'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
-                gripperPoints,
-                robot_description_semantic
+                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/6Dpe_test.xml')},
+                {'plugins':["rbs_pose_estimation"]},
             ]
         )
     ])

rbs_bt_executor/src/PoseEstimation.cpp

@@ -21,7 +21,7 @@ public:
     _set_params_client =
         this->_node->create_client<rcl_interfaces::srv::SetParameters>(
             "/image_sub2/set_parameters");
-
+    
     _model_name = getInput<std::string>("ObjectName").value();
   }
 
@@ -52,7 +52,7 @@ private:
   uint8_t transition_id_{};
   std::string _model_name{};
   std::string _model_path{};
-  std::string _req_type;
+  std::string _req_type{};
   rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr
       _set_params_client;
   std::vector<rcl_interfaces::msg::Parameter> _params;
@@ -87,6 +87,7 @@ private:
   void set_mesh_param() {
     auto _package_name =
         ament_index_cpp::get_package_share_directory("rbs_perception");
+    RCLCPP_INFO_STREAM(_node->get_logger(), _package_name);
     _model_path = build_model_path(_model_name, _package_name);
 
     auto param_request =