From 26269b6678515db31e219cfb6ddaf410a0655c17 Mon Sep 17 00:00:00 2001 From: shalenikol Date: Tue, 14 Nov 2023 13:04:11 +0300 Subject: [PATCH] BT for pose estimation --- rbs_bt_executor/bt_trees/6Dpe_test.xml | 17 +++ rbs_bt_executor/launch/rbs_executor.launch.py | 140 ++++++++++-------- rbs_bt_executor/src/PoseEstimation.cpp | 5 +- 3 files changed, 98 insertions(+), 64 deletions(-) create mode 100644 rbs_bt_executor/bt_trees/6Dpe_test.xml diff --git a/rbs_bt_executor/bt_trees/6Dpe_test.xml b/rbs_bt_executor/bt_trees/6Dpe_test.xml new file mode 100644 index 0000000..d5de82f --- /dev/null +++ b/rbs_bt_executor/bt_trees/6Dpe_test.xml @@ -0,0 +1,17 @@ + + + + + + + + + \ No newline at end of file diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py index 65444ea..71b74df 100644 --- a/rbs_bt_executor/launch/rbs_executor.launch.py +++ b/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"]}, ] ) ]) diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp index f506ad0..92eba20 100644 --- a/rbs_bt_executor/src/PoseEstimation.cpp +++ b/rbs_bt_executor/src/PoseEstimation.cpp @@ -21,7 +21,7 @@ public: _set_params_client = this->_node->create_client( "/image_sub2/set_parameters"); - + _model_name = getInput("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::SharedPtr _set_params_client; std::vector _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 =