diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index c831e87..7009d59 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/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?") diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py index 71b74df..be622f7 100644 --- a/rbs_bt_executor/launch/rbs_executor.launch.py +++ b/rbs_bt_executor/launch/rbs_executor.launch.py @@ -8,98 +8,88 @@ 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 - - -# 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 -# ] -# ) -# ]) + 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'], + #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')}, - {'plugins':["rbs_pose_estimation"]}, + {'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", + "rbs_pose_estimation" + ]}, + gripperPoints, + robot_description_semantic ] ) ]) diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp index 0af9ddc..84f36e3 100644 --- a/rbs_bt_executor/src/PoseEstimation.cpp +++ b/rbs_bt_executor/src/PoseEstimation.cpp @@ -18,7 +18,7 @@ public: : BtService(name, config) { RCLCPP_INFO_STREAM(_node->get_logger(), "Start node."); - _set_params_client = std::make_shared(_node, "/image_sub2"); + _set_params_client = std::make_shared(_node, "/pose_estimation"); while (!_set_params_client->wait_for_service(1s)) { if (!rclcpp::ok()) { diff --git a/rbs_perception/config/pose_estimation_config.json b/rbs_perception/config/pose_estimation_config.json index 843b892..7d1f276 100644 --- a/rbs_perception/config/pose_estimation_config.json +++ b/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", diff --git a/rbs_perception/launch/perception.launch.py b/rbs_perception/launch/perception.launch.py index 8c43c3e..290f66e 100644 --- a/rbs_perception/launch/perception.launch.py +++ b/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) diff --git a/rbs_perception/scripts/pose_estimation_lifecycle.py b/rbs_perception/scripts/pose_estimation_lifecycle.py index f368b6e..91956d2 100755 --- a/rbs_perception/scripts/pose_estimation_lifecycle.py +++ b/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"