refactoring run

rbs_bringup/launch/bringup.launch.py

@@ -153,7 +153,7 @@ def generate_launch_description():
         DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", default_value="true", description="Launch task_planner?")
+        DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
     )
     declared_arguments.append(
         DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -8,98 +8,88 @@ import yaml
 import math
 
 
-# def construct_angle_radians(loader, node):
+def construct_angle_radians(loader, node):
-#     """Utility function to construct radian values from yaml."""
+    """Utility function to construct radian values from yaml."""
-#     value = loader.construct_scalar(node)
+    value = loader.construct_scalar(node)
-#     try:
+    try:
-#         return float(value)
+        return float(value)
-#     except SyntaxError:
+    except SyntaxError:
-#         raise Exception("invalid expression: %s" % value)
+        raise Exception("invalid expression: %s" % value)
 
-# def construct_angle_degrees(loader, node):
+def construct_angle_degrees(loader, node):
-#     """Utility function for converting degrees into radians from yaml."""
+    """Utility function for converting degrees into radians from yaml."""
-#     return math.radians(construct_angle_radians(loader, node))
+    return math.radians(construct_angle_radians(loader, node))
 
-# def load_yaml(package_name, file_path):
+def load_yaml(package_name, file_path):
-#     package_path = get_package_share_directory(package_name)
+    package_path = get_package_share_directory(package_name)
-#     absolute_file_path = os.path.join(package_path, file_path)
+    absolute_file_path = os.path.join(package_path, file_path)
 
-#     try:
+    try:
-#         yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
+        yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
-#         yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
+        yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
-#     except Exception:
+    except Exception:
-#         raise Exception("yaml support not available; install python-yaml")
+        raise Exception("yaml support not available; install python-yaml")
 
-#     try:
+    try:
-#         with open(absolute_file_path) as file:
+        with open(absolute_file_path) as file:
-#             return yaml.safe_load(file)
+            return yaml.safe_load(file)
-#     except OSError:  # parent of IOError, OSError *and* WindowsError where available
+    except OSError:  # parent of IOError, OSError *and* WindowsError where available
-#         return None
+        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/6Dpe_test.xml')},
+                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
-                {'plugins':["rbs_pose_estimation"]},
+                {'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",
+                            "rbs_pose_estimation"
+                ]},
+                gripperPoints,
+                robot_description_semantic
             ]
         )
     ])

rbs_bt_executor/src/PoseEstimation.cpp

@@ -18,7 +18,7 @@ public:
       : BtService<PoseEstimationSrv>(name, config) {
     RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
 
-    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/image_sub2");
+    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/pose_estimation");
 
     while (!_set_params_client->wait_for_service(1s)) {
       if (!rclcpp::ok()) {

rbs_perception/config/pose_estimation_config.json

@@ -1,5 +1,5 @@
 {
- "nodeName":"image_sub2",
+ "nodeName":"pose_estimation",
  "topicImage":"/outer_rgbd_camera/image",
  "topicCameraInfo":"/outer_rgbd_camera/camera_info",
  "topicDepth":"/outer_rgbd_camera/depth_image",

rbs_perception/launch/perception.launch.py

@@ -1,3 +1,4 @@
+import os
 from launch_ros.actions import Node
 from ur_moveit_config.launch_common import load_yaml
 from launch import LaunchDescription
@@ -16,12 +17,13 @@ def generate_launch_description():
             points_params
         ]
     )
-    grasp_marker = Node(
+    pose_estimation = Node(
         package="rbs_perception",
-        executable="grasp_marker_publish.py",
+        executable="pose_estimation_lifecycle.py",
     )
+
     nodes_to_start = [
         grasp_pose_loader,
-        grasp_marker
+        pose_estimation
     ]
     return LaunchDescription(declared_arguments + nodes_to_start)

rbs_perception/scripts/pose_estimation_lifecycle.py

@@ -70,7 +70,7 @@ class PoseEstimator(Node):
         self.tf2_send_pose = 0
         self.megapose_model = None
 
-        self.nodeName = node_name #"image_sub2"
+        self.nodeName = node_name
         self.topicImage = "/outer_rgb_camera/image"
         self.topicCameraInfo = "/outer_rgb_camera/camera_info"
         self.topicDepth = "/outer_rgbd_camera/depth_image"