diff --git a/rbs_bringup/CMakeLists.txt b/rbs_bringup/CMakeLists.txt index 42cee8b..806658c 100644 --- a/rbs_bringup/CMakeLists.txt +++ b/rbs_bringup/CMakeLists.txt @@ -16,6 +16,10 @@ install( DESTINATION share/${PROJECT_NAME} ) +# Install Python modules +ament_python_install_package(rbs_launch_utils) +ament_python_install_module(rbs_launch_utils/launch_common.py) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/rbs_bringup/config/rbs.rviz b/rbs_bringup/config/rbs.rviz new file mode 100644 index 0000000..74b8327 --- /dev/null +++ b/rbs_bringup/config/rbs.rviz @@ -0,0 +1,622 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.49496981501579285 + Tree Height: 1468 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /display_contacts + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + box1_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box2_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box3_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box4_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box5_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box6_place: + Alpha: 1 + Show Axes: false + Show Trail: false + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + inner_rgbd_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outer_rgbd_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_grasp_point: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base: + Value: false + base_link: + Value: false + base_link_inertia: + Value: false + box1: + Value: true + box1_place: + Value: true + box2: + Value: true + box2_place: + Value: true + box3: + Value: true + box3_place: + Value: true + box4: + Value: true + box4_place: + Value: true + box5: + Value: true + box5_place: + Value: true + box6: + Value: true + box6_place: + Value: true + flange: + Value: false + forearm_link: + Value: false + fork_gt: + Value: true + ft_frame: + Value: false + inner_rgbd_camera: + Value: false + outer_rgbd_camera: + Value: false + robotiq_85_base_link: + Value: false + robotiq_85_left_finger_link: + Value: false + robotiq_85_left_finger_tip_link: + Value: false + robotiq_85_left_inner_knuckle_link: + Value: false + robotiq_85_left_knuckle_link: + Value: false + robotiq_85_right_finger_link: + Value: false + robotiq_85_right_finger_tip_link: + Value: false + robotiq_85_right_inner_knuckle_link: + Value: false + robotiq_85_right_knuckle_link: + Value: false + robotiq_grasp_point: + Value: true + shoulder_link: + Value: false + tool0: + Value: false + upper_arm_link: + Value: false + world: + Value: false + wrist_1_link: + Value: false + wrist_2_link: + Value: false + wrist_3_link: + Value: false + Marker Scale: 0.4000000059604645 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + base_link: + base: + {} + base_link_inertia: + shoulder_link: + upper_arm_link: + forearm_link: + wrist_1_link: + wrist_2_link: + wrist_3_link: + flange: + tool0: + inner_rgbd_camera: + {} + robotiq_85_base_link: + robotiq_85_left_inner_knuckle_link: + {} + robotiq_85_left_knuckle_link: + robotiq_85_left_finger_link: + robotiq_85_left_finger_tip_link: + {} + robotiq_85_right_inner_knuckle_link: + {} + robotiq_85_right_knuckle_link: + robotiq_85_right_finger_link: + robotiq_85_right_finger_tip_link: + {} + robotiq_grasp_point: + {} + ft_frame: + {} + box1: + {} + box1_place: + {} + box2: + {} + box2_place: + {} + box3: + {} + box3_place: + {} + box4: + {} + box4_place: + {} + box5: + {} + box5_place: + {} + box6: + {} + box6_place: + {} + fork_gt: + {} + outer_rgbd_camera: + {} + Update Interval: 0 + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + box1_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box2_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box3_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box4_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box5_place: + Alpha: 1 + Show Axes: false + Show Trail: false + box6_place: + Alpha: 1 + Show Axes: false + Show Trail: false + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + inner_rgbd_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outer_rgbd_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_grasp_point: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.619869709014893 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2489434778690338 + Y: -0.013962505385279655 + Z: 0.13800443708896637 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6253980398178101 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.2853964567184448 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1872 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 3076 + X: 124 + Y: 54 diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index 7009d59..ffc8681 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/rbs_bringup/launch/bringup.launch.py @@ -9,6 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +# from rbs_launch_utils.launch_common import load_yaml_abs def generate_launch_description(): declared_arguments = [] @@ -192,7 +193,7 @@ def generate_launch_description(): ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("rbs_bringup"), "config", "fork_view.rviz"] + [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"] ) mujoco_model = PathJoinSubstitution( @@ -310,6 +311,24 @@ def generate_launch_description(): 'robot_description_semantic': robot_description_semantic_content }.items(), condition=IfCondition(launch_moveit)) + + skills = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_skill_servers'), + 'launch', + 'skills.launch.py' + ]) + ]), + launch_arguments={ + 'robot_description': robot_description_content, + 'robot_description_semantic': robot_description_semantic_content, + 'robot_description_kinematics': robot_description_kinematics, + 'use_sim_time': use_sim_time, + 'with_gripper_condition': with_gripper_condition, + 'points_params_filepath': "gripperPositions.yaml" + }.items() + ) task_planner = IncludeLaunchDescription( PythonLaunchDescriptionSource([ @@ -343,6 +362,7 @@ def generate_launch_description(): control, simulation, moveit, + skills, task_planner, perception ] diff --git a/rbs_bringup/rbs_launch_utils/__init__.py b/rbs_bringup/rbs_launch_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rbs_bringup/rbs_launch_utils/launch_common.py b/rbs_bringup/rbs_launch_utils/launch_common.py new file mode 100644 index 0000000..c680e87 --- /dev/null +++ b/rbs_bringup/rbs_launch_utils/launch_common.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2021 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Lovro Ivanov + +import math +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + + +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 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: + 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 load_yaml_abs(absolute_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: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml index 256fbc5..410eebc 100644 --- a/rbs_bt_executor/bt_trees/test_tree.xml +++ b/rbs_bt_executor/bt_trees/test_tree.xml @@ -4,7 +4,7 @@ - @@ -27,7 +26,7 @@ - @@ -50,7 +48,7 @@ - @@ -73,7 +70,7 @@ - @@ -96,7 +92,7 @@ - @@ -119,7 +114,7 @@ - diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py index be622f7..b5a3238 100644 --- a/rbs_bt_executor/launch/rbs_executor.launch.py +++ b/rbs_bt_executor/launch/rbs_executor.launch.py @@ -6,48 +6,20 @@ from ament_index_python.packages import get_package_share_directory import os 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_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) - - 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 +from rbs_launch_utils.launch_common import load_yaml 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" - ) + # 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", @@ -76,7 +48,6 @@ def generate_launch_description(): return LaunchDescription([ Node( package='behavior_tree', - namespace='', executable='bt_engine', #prefix=['xterm -e gdb -ex run --args'], parameters=[ @@ -88,7 +59,7 @@ def generate_launch_description(): "rbs_add_planning_scene_object", "rbs_pose_estimation" ]}, - gripperPoints, + # gripperPoints, robot_description_semantic ] ) diff --git a/rbs_bt_executor/src/GetPickPlacePoses.cpp b/rbs_bt_executor/src/GetPickPlacePoses.cpp index edadd66..7563ea7 100644 --- a/rbs_bt_executor/src/GetPickPlacePoses.cpp +++ b/rbs_bt_executor/src/GetPickPlacePoses.cpp @@ -9,9 +9,7 @@ using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses; class GetPickPlacePose : public BtService { public: GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config) - : BtService(name, config) { - RCLCPP_INFO(_node->get_logger(), "Start the node"); - } + : BtService(name, config) {} GetPickPlacePoseClient::Request::SharedPtr populate_request() override { auto request = std::make_shared(); @@ -31,24 +29,23 @@ public: BT::NodeStatus handle_response( GetPickPlacePoseClient::Response::SharedPtr response) override { - RCLCPP_INFO(_node->get_logger(), - "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f " - "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t " - "orientation.z: %f \n\t orientation.w: %f", - response->grasp[2].position.x, response->grasp[2].position.y, - response->grasp[2].position.z, response->grasp[2].orientation.x, - response->grasp[2].orientation.y, - response->grasp[2].orientation.z, - response->grasp[2].orientation.w); + // RCLCPP_INFO(_node->get_logger(), + // "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f " + // "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t " + // "orientation.z: %f \n\t orientation.w: %f", + // response->grasp[1].position.x, response->grasp[1].position.y, + // response->grasp[1].position.z, response->grasp[1].orientation.x, + // response->grasp[1].orientation.y, + // response->grasp[1].orientation.z, + // response->grasp[1].orientation.w); RCLCPP_INFO(_node->get_logger(), "Start handle_response()"); setOutput("GraspPose", response->grasp[0]); setOutput("PreGraspPose", response->grasp[1]); - setOutput("PostGraspPose", response->grasp[3]); - setOutput("PostPostGraspPose", - response->grasp[2]); + setOutput("PostGraspPose", response->grasp[2]); + setOutput("PlacePose", response->place[0]); setOutput("PrePlacePose", response->place[1]); - setOutput("PostPlacePose", response->place[1]); + setOutput("PostPlacePose", response->place[2]); return BT::NodeStatus::SUCCESS; } @@ -60,8 +57,8 @@ public: OutputPort("GraspPose"), OutputPort("PreGraspPose"), OutputPort("PostGraspPose"), - OutputPort( - "PostPostGraspPose"), // TODO: change to std::vector<> + // TODO: change to std::vector<> + OutputPort("PostPostGraspPose"), OutputPort("PlacePose"), OutputPort("PrePlacePose"), OutputPort("PostPlacePose"), diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index 32060b1..f2b9591 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -124,10 +124,10 @@ box6 -0.25 0.75 0.025 0 0 0 - + diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 7163af5..bf14719 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -32,6 +32,8 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) +find_package (Eigen3 3.3 REQUIRED) + # Default to Fortress set(SDF_VER 12) @@ -114,7 +116,7 @@ target_compile_definitions(pick_place_pose_loader PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL") ament_target_dependencies(gripper_action_server ${deps}) -ament_target_dependencies(pick_place_pose_loader ${deps}) +ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3) rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server) rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server) @@ -155,6 +157,11 @@ install( DESTINATION include ) +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + install( TARGETS move_topose_action_server diff --git a/rbs_bt_executor/config/gripperPositions.yaml b/rbs_skill_servers/config/gripperPositions.yaml similarity index 100% rename from rbs_bt_executor/config/gripperPositions.yaml rename to rbs_skill_servers/config/gripperPositions.yaml diff --git a/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp index 2e4a76c..11fce65 100644 --- a/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp +++ b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp @@ -1,29 +1,39 @@ -#include -#include "rclcpp_components/register_node_macro.hpp" -#include -#include #include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" #include "rclcpp/rclcpp.hpp" -#include "moveit_msgs/msg/grasp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include +#include +#include +#include +#include +#include #include #include -namespace rbs_skill_actions -{ - class GetGraspPickPoseServer : public rclcpp::Node - { - public: - explicit GetGraspPickPoseServer(rclcpp::NodeOptions options); - private: - static std::vector collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec); - rclcpp::Service::SharedPtr srv_; - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; - geometry_msgs::msg::TransformStamped tf_data; - void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, - rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response); - std::vector grasp_param_pose; - Eigen::Affine3d get_Affine_from_arr(const std::vector pose); - }; -} + +namespace rbs_skill_actions { +class GetGraspPickPoseServer : public rclcpp::Node { +public: + explicit GetGraspPickPoseServer(rclcpp::NodeOptions options); + +private: + static std::vector + collect_pose(const Eigen::Isometry3d &graspPose, + const geometry_msgs::msg::Vector3 &move_direction, + const Eigen::Vector3d &scale_vec); + rclcpp::Service::SharedPtr srv_; + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_buffer_; + geometry_msgs::msg::TransformStamped place_pose_tf; + geometry_msgs::msg::TransformStamped grasp_pose_tf; + + void handle_server( + const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr + request, + rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr + response); + std::vector grasp_param_pose; + Eigen::Affine3d get_Affine_from_arr(const std::vector pose); +}; +} // namespace rbs_skill_actions RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer); \ No newline at end of file diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py new file mode 100644 index 0000000..7ce7872 --- /dev/null +++ b/rbs_skill_servers/launch/skills.launch.py @@ -0,0 +1,117 @@ +import os +from launch import LaunchDescription, LaunchContext +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + ExecuteProcess, + OpaqueFunction +) +from ament_index_python.packages import get_package_share_directory +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from rbs_launch_utils.launch_common import load_yaml + + +def generate_launch_description(): + + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_description", + default_value="", + description="robot description string", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("robot_description_semantic", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("robot_description_kinematics", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("use_sim_time", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("with_gripper_condition", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("points_params_filepath", default_value="") + ) + + robot_description_decl = LaunchConfiguration("robot_description") + robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic") + robot_description_kinematics = LaunchConfiguration("robot_description_kinematics") + use_sim_time = LaunchConfiguration("use_sim_time") + with_gripper_condition = LaunchConfiguration("with_gripper_condition") + points_params_filepath_decl = LaunchConfiguration("points_params_filepath") + robot_description = {"robot_description": robot_description_decl} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl} + + points_params = load_yaml( + "rbs_skill_servers", "config/gripperPositions.yaml" + ) + + move_topose_action_server = Node( + package="rbs_skill_servers", + executable="move_topose_action_server", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + {"use_sim_time": use_sim_time}, + ] + ) + + gripper_control_node = Node( + package="rbs_skill_servers", + executable="gripper_control_action_server", + parameters= [ + robot_description, + robot_description_semantic, + robot_description_kinematics, + {"use_sim_time": use_sim_time}, + ], + condition=IfCondition(with_gripper_condition) + ) + + move_cartesian_path_action_server = Node( + package="rbs_skill_servers", + executable="move_cartesian_path_action_server", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + {"use_sim_time": use_sim_time}, + ] + ) + + move_joint_state_action_server = Node( + package="rbs_skill_servers", + executable="move_to_joint_states_action_server", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + {"use_sim_time": use_sim_time}, + ] + ) + + grasp_pose_loader = Node( + package="rbs_skill_servers", + executable="pick_place_pose_loader_service_server", + output="screen" + ) + + nodes_to_start =[ + move_topose_action_server, + gripper_control_node, + move_cartesian_path_action_server, + move_joint_state_action_server, + grasp_pose_loader + ] + + return LaunchDescription(declared_arguments + nodes_to_start) \ No newline at end of file diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp index 423f983..028c2e3 100644 --- a/rbs_skill_servers/src/pick_place_pose_loader.cpp +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -1,110 +1,125 @@ -//#include "rbs_skill_servers" #include "rbs_skill_servers/pick_place_pose_loader.hpp" +#include +#include +#include +#include +#include +#include +#include +// #include +// #include +// #include +// #include +// #include +// #include +// #include using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer; using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses; using namespace std::chrono_literals; // rclcpp::Node::SharedPtr g_node = nullptr; - - GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options) -: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) -{ - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + : Node("grasp_place_pose_loader", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - srv_ = create_service("/get_pick_place_pose_service", - std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2)); + srv_ = create_service( + "/get_pick_place_pose_service", + std::bind(&GetGraspPickPoseServer::handle_server, this, + std::placeholders::_1, std::placeholders::_2)); } -void -GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, - rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response) -{ - std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name - try { - tf_data = tf_buffer_->lookupTransform( - "world", rrr.c_str(), - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not transform %s to %s: %s", - rrr.c_str(), "world", ex.what()); - return; - } - // TODO: Here need to check the parameter - // We can use another parameter library from PickNik - grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array(); - RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->grasp_direction.x, - request->grasp_direction.y, - request->grasp_direction.z); - RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->place_direction.x, - request->place_direction.y, - request->place_direction.z); - Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); - Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); - //std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl; - //std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl; - Eigen::Vector3d vec_grasp(0.15,0.15,0.02); - Eigen::Vector3d vec_place(0,0,0.15); - response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp); - response->place = collect_pose(place_pose, request->place_direction, vec_place); +void GetGraspPlacePoseServer::handle_server( + const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr + request, + rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr + response) { + std::string object_name = + request->object_name + "_place"; // TODO: replace with better name + // Load place pose from TF2 + try { + place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(), + tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s", + object_name.c_str(), "world", ex.what()); + return; + } + // Load grasp pose from TF2 + try { + grasp_pose_tf = tf_buffer_->lookupTransform( + "world", request->object_name.c_str(), tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s", + request->object_name.c_str(), "world", ex.what()); + } + + RCLCPP_DEBUG(this->get_logger(), + "Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f", + grasp_pose_tf.transform.translation.x, + grasp_pose_tf.transform.translation.y, + grasp_pose_tf.transform.translation.z); + // TODO: Here need to check the parameter + // We can use another parameter library from PickNik + // grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose") + // .as_double_array(); + RCLCPP_DEBUG(this->get_logger(), + "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->grasp_direction.x, request->grasp_direction.y, + request->grasp_direction.z); + RCLCPP_DEBUG(this->get_logger(), + "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->place_direction.x, request->place_direction.y, + request->place_direction.z); + + auto grasp_pose = tf2::transformToEigen(grasp_pose_tf); + auto place_pose = tf2::transformToEigen(place_pose_tf); + + Eigen::Vector3d scale_grasp(0, 0, 0.10); + Eigen::Vector3d scale_place(0, 0, 0.15); + response->grasp = + collect_pose(grasp_pose, request->grasp_direction, scale_grasp); + response->place = + collect_pose(place_pose, request->place_direction, scale_place); } -std::vector -GetGraspPlacePoseServer::collect_pose( - const Eigen::Affine3d pose, - const geometry_msgs::msg::Vector3 direction, - const Eigen::Vector3d scale_vec) -{ - std::vector pose_v_; - pose_v_.push_back(tf2::toMsg(pose)); - Eigen::Vector3d posedir; - Eigen::Affine3d pose_ = pose; - tf2::fromMsg(direction, posedir); - Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose(); - Ixy.diagonal() = posedir; - Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero(); - Iz.diagonal() = Eigen::Vector3d::UnitZ(); +std::vector GetGraspPlacePoseServer::collect_pose( + const Eigen::Isometry3d &graspPose, + const geometry_msgs::msg::Vector3 &move_direction, + const Eigen::Vector3d &scale_vec) { + std::vector poses{}; + // Add GraspPose as base point + Eigen::Isometry3d grasp_pose{graspPose}; + Eigen::Quaterniond theGraspOrientation(grasp_pose.linear()); + theGraspOrientation = + theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); + grasp_pose.rotate(theGraspOrientation); + poses.push_back(tf2::toMsg(grasp_pose)); + // PreGrasp calculation + Eigen::Vector3d preGraspTranslation = + static_cast(graspPose.translation()) + scale_vec; + Eigen::Quaterniond preGraspOrientation(graspPose.linear()); + preGraspOrientation = + preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity(); + preGraspPose.translate(preGraspTranslation); + preGraspPose.rotate(preGraspOrientation); + poses.push_back(tf2::toMsg(preGraspPose)); - if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0] - { - std::cout << "\n TBD" << std::endl; - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0] - { - // Gp -- grasp point frame - pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Iz * scale_vec); // Gp-y + z - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Ixy * scale_vec); // Gp+z - pose_v_.push_back(tf2::toMsg(pose_)); - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1] - { - pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp - pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp - } - else - { - std::cout << "\n TBD" << std::endl; - } - return pose_v_; -} + // PostGrasp calculation + Eigen::Vector3d postGraspTranslation = + static_cast(graspPose.translation()) + scale_vec; + Eigen::Quaterniond postGraspOrientation(graspPose.linear()); + postGraspOrientation = + postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); -Eigen::Affine3d -GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) -{ - Eigen::Vector3d point(std::vector(pose.begin(), pose.begin()+3).data()); - Eigen::Quaterniond quat(std::vector(pose.begin()+3, pose.end()).data()); - Eigen::Affine3d aff; - aff.translation() = point; - aff.linear() = quat.toRotationMatrix(); - return aff; + Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity(); + postGraspPose.translate(postGraspTranslation); + postGraspPose.rotate(postGraspOrientation); + poses.push_back(tf2::toMsg(postGraspPose)); + + return poses; }