diff --git a/rasmt_moveit_config/config/rasmt.srdf b/rasmt_moveit_config/config/rasmt.srdf index b050512..98cf235 100644 --- a/rasmt_moveit_config/config/rasmt.srdf +++ b/rasmt_moveit_config/config/rasmt.srdf @@ -13,7 +13,11 @@ + + + + @@ -42,15 +46,18 @@ + + + - + diff --git a/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml b/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml index 9e1c114..a02bee5 100644 --- a/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml +++ b/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml @@ -15,8 +15,9 @@ rasmt_arm_controller: - rasmt_Rot_Y_4 rasmt_hand_controller: - action_ns: gripper_cmd - type: GripperCommand + action_ns: follow_joint_trajectory + type: FollowJointTrajectory default: true joints: - - rasmt_Slide_1 \ No newline at end of file + - rasmt_Slide_1 + - rasmt_Slide_2 \ No newline at end of file diff --git a/rasmt_support/config/rasmt_ros2_controllers.yaml b/rasmt_support/config/rasmt_ros2_controllers.yaml index 3a1bcd9..1a021bc 100644 --- a/rasmt_support/config/rasmt_ros2_controllers.yaml +++ b/rasmt_support/config/rasmt_ros2_controllers.yaml @@ -6,7 +6,7 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController rasmt_hand_controller: - type: position_controllers/GripperActionController + type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -29,4 +29,11 @@ rasmt_arm_controller: rasmt_hand_controller: ros__parameters: - joint: rasmt_Slide_1 \ No newline at end of file + joints: + - rasmt_Slide_1 + - rasmt_Slide_2 + command_interfaces: + - position + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/rasmt_support/urdf/tools/rasmt_hand_control.xacro b/rasmt_support/urdf/tools/rasmt_hand_control.xacro index a04f147..6413c15 100644 --- a/rasmt_support/urdf/tools/rasmt_hand_control.xacro +++ b/rasmt_support/urdf/tools/rasmt_hand_control.xacro @@ -28,8 +28,8 @@ - ${prefix}_Slide_1 - 1 + -1 1 diff --git a/rasmt_support/urdf/tools/rasmt_hand_macro.xacro b/rasmt_support/urdf/tools/rasmt_hand_macro.xacro index d5b627e..f173c27 100644 --- a/rasmt_support/urdf/tools/rasmt_hand_macro.xacro +++ b/rasmt_support/urdf/tools/rasmt_hand_macro.xacro @@ -179,7 +179,7 @@ lower="-0.02" upper="0.04" velocity="0.2"/> - + diff --git a/robossembler/CMakeLists.txt b/robossembler/CMakeLists.txt index 1d23679..c448ff9 100644 --- a/robossembler/CMakeLists.txt +++ b/robossembler/CMakeLists.txt @@ -48,6 +48,9 @@ include_directories(include) add_library(robossembler_move_bt_node SHARED src/behavior_tree_nodes/atomic_skills/Move.cpp) list(APPEND plugin_libs robossembler_move_bt_node) +add_library(robossembler_move_gripper_bt_node SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp) +list(APPEND plugin_libs robossembler_move_gripper_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/robossembler/README.md b/robossembler/README.md index b665ae4..35b7f18 100644 --- a/robossembler/README.md +++ b/robossembler/README.md @@ -9,18 +9,20 @@ This package is the main one and contains a task planner working in combination │ └── atomic_skills_xml │ ├── EmuPoseEstimation.xml │ └── move.xml +| └── move_gripper.xml ├── CMakeLists.txt ├── config -│ └── params.yaml # Config params and waypoints which robot move +│ └── params.yaml # Config params for PlanSys2 robot planning ├── include # Libs which used │ └── robot_bt │ └── behavior_tree_nodes │ └── atomic_skills │ ├── EmuPoseEstimation.hpp # Lib for get Pose of model from Gazebo -│ └── Move.hpp # Lib for P2P move +│ └── Move.hpp # Lib for P2P arm move +| └── MoveGripper.hpp # Lib for P2P gripper move ├── launch │ ├── robossembler_bringup.launch.py # Launch file for start all scene -│ └── task_planner.launch.py +│ └── task_planner.launch.py # Launch file for PlanSys2 ├── package.xml ├── pddl │ ├── commands # Example commands for PlanSys2 terminal @@ -32,6 +34,7 @@ This package is the main one and contains a task planner working in combination │ └── atomic_skills │ ├── EmuPoseEstimation.cpp │ └── Move.cpp + | └── MoveGripper.cpp ├── move_action_node.cpp # Temporarily not used - └── move_controller_node.cpp # Execute plan wthout terminal + └── move_controller_node.cpp # Executable move arm controller (plan without terminal) ``` \ No newline at end of file diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml new file mode 100644 index 0000000..2b6963c --- /dev/null +++ b/robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/robossembler/config/params.yaml b/robossembler/config/params.yaml index 3e5b2ea..996d74d 100644 --- a/robossembler/config/params.yaml +++ b/robossembler/config/params.yaml @@ -8,3 +8,13 @@ move: one: [0.01, 0.01, 0.6, 0.0 , 0.0 , 0.0, 1.0] + +move_gripper: + ros__parameters: + plugins: + - robossembler_move_gripper_bt_node + gripper_group: ["rasmt_hand_arm_group"] + relative_gaps: ["open", "close"] + gaps: + open: 0.039 + close: 0.0 diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp index 17c8214..84b9b32 100644 --- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp +++ b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp @@ -11,22 +11,22 @@ namespace task_planner { - class EmuPoseEstimation : public BT::ActionNodeBase + class EmuPoseEstimation : public BT::ActionNodeBase + { + public: + EmuPoseEstimation(const std::string &xml_tag, + const BT::NodeConfiguration &conf); + void resultCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + virtual void halt() override; + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() { - public: - EmuPoseEstimation(const std::string &xml_tag, - const BT::NodeConfiguration &conf); - void resultCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + return {BT::InputPort("model_name")}; + } - virtual void halt() override; - BT::NodeStatus tick() override; - - static BT::PortsList providedPorts() - { - return {BT::InputPort("model_name")}; - } - - private: - std::string model_name; - }; + private: + std::string model_name; + }; } \ No newline at end of file diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp new file mode 100644 index 0000000..0f04425 --- /dev/null +++ b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "moveit_msgs/msg/move_it_error_codes.hpp" +#include "plansys2_bt_actions/BTActionNode.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace task_planner +{ + class MoveGripper : public BT::ActionNodeBase + { + public: + MoveGripper(const std::string &xml_tag_name, + const BT::NodeConfiguration &conf); + + void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg); + /** + * @brief force end of action + */ + virtual void halt(); + + /** + * @brief the main body of the node's action until + * its completion with a return BT::NodeStatus::SUCCESS + * @return BT::NodeStatus + */ + BT::NodeStatus tick(); + + /** + * @brief provides the node with the user + * arguments declared in the action.xml + * @return BT::PortsList + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("gripper_group"), + BT::InputPort("relative_gap") + }; + } + + private: + const double max_gripper_gap_ = 0.04; + + std::map relative_gaps_; + std::vector available_groups_; + std::string gripper_group_; + std::atomic_bool _halt_requested; + }; + +} // namespace task_planner \ No newline at end of file diff --git a/robossembler/launch/robossembler_bringup.launch.py b/robossembler/launch/robossembler_bringup.launch.py index 79211e8..ea3432e 100644 --- a/robossembler/launch/robossembler_bringup.launch.py +++ b/robossembler/launch/robossembler_bringup.launch.py @@ -12,106 +12,106 @@ import xacro import os def generate_launch_description(): - launch_args = [] - launch_args.append( - DeclareLaunchArgument( - name="grip", - default_value="true", - description="On or OFF gripper for launch" - ) + launch_args = [] + launch_args.append( + DeclareLaunchArgument( + name="grip", + default_value="true", + description="On or OFF gripper for launch" ) - launch_args.append( - DeclareLaunchArgument( - name="sim", - default_value="true", - description="On or OFF simulation" - ) + ) + launch_args.append( + DeclareLaunchArgument( + name="sim", + default_value="true", + description="On or OFF simulation" ) - launch_args.append( - DeclareLaunchArgument( - name="robot_name", - default_value="rasmt", - description="Robot name" - ) + ) + launch_args.append( + DeclareLaunchArgument( + name="robot_name", + default_value="rasmt", + description="Robot name" ) - launch_args.append( - DeclareLaunchArgument( - name="plansys2", - default_value="true", - description="enable plansys2" - ) + ) + launch_args.append( + DeclareLaunchArgument( + name="plansys2", + default_value="true", + description="enable plansys2" ) + ) - # Load robot description - robot_description_content = Command( - [ - FindExecutable(name="xacro"), " ", - PathJoinSubstitution( - [FindPackageShare("rasmt_support"), "urdf/rasmt.xacro"] - ), " ", - "robot_name:=", LaunchConfiguration("robot_name"), " ", - "sim:=", LaunchConfiguration("sim"), " ", - "grip:=", LaunchConfiguration("grip") - ] - ) + # Load robot description + robot_description_content = Command( + [ + FindExecutable(name="xacro"), " ", + PathJoinSubstitution( + [FindPackageShare("rasmt_support"), "urdf/rasmt.xacro"] + ), " ", + "robot_name:=", LaunchConfiguration("robot_name"), " ", + "sim:=", LaunchConfiguration("sim"), " ", + "grip:=", LaunchConfiguration("grip") + ] + ) - control = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("rasmt_support"), - "launch", - "rasmt_control.launch.py" - ]) - ), launch_arguments=[ - ("robot_description", robot_description_content), - ("sim", LaunchConfiguration("sim")) - ] - ) + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "launch", + "rasmt_control.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("sim", LaunchConfiguration("sim")) + ] + ) - simulation = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("rasmt_support"), - "launch", - "rasmt_gazebo.launch.py" - ]) - ), launch_arguments=[ - ("robot_name", LaunchConfiguration("robot_name")) - ], - condition=IfCondition(LaunchConfiguration("sim")) - ) + simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "launch", + "rasmt_gazebo.launch.py" + ]) + ), launch_arguments=[ + ("robot_name", LaunchConfiguration("robot_name")) + ], + condition=IfCondition(LaunchConfiguration("sim")) + ) - moveit = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("rasmt_moveit_config"), - "launch", - "rasmt_moveit.launch.py" - ]) - ), launch_arguments=[ - ("robot_description", robot_description_content), - ("sim",LaunchConfiguration("sim")) - ] - ) + moveit = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_moveit_config"), + "launch", + "rasmt_moveit.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("sim",LaunchConfiguration("sim")) + ] + ) - task_planner_init = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("robossembler"), - "launch", - "task_planner.launch.py" - ]) - ), launch_arguments=[ - ("robot_description", robot_description_content) - ], - condition=IfCondition(LaunchConfiguration("plansys2")) - ) + task_planner_init = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("robossembler"), + "launch", + "task_planner.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content) + ], + condition=IfCondition(LaunchConfiguration("plansys2")) + ) - launch_nodes = [] - launch_nodes.append(control) - launch_nodes.append(simulation) - launch_nodes.append(moveit) - launch_nodes.append(task_planner_init) + launch_nodes = [] + launch_nodes.append(control) + launch_nodes.append(simulation) + launch_nodes.append(moveit) + launch_nodes.append(task_planner_init) - return LaunchDescription(launch_args + launch_nodes) + return LaunchDescription(launch_args + launch_nodes) diff --git a/robossembler/launch/task_planner.launch.py b/robossembler/launch/task_planner.launch.py index 984f4a4..1e45dd2 100644 --- a/robossembler/launch/task_planner.launch.py +++ b/robossembler/launch/task_planner.launch.py @@ -1,5 +1,8 @@ import os import yaml +import xacro + +__debug = True from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -51,17 +54,29 @@ def generate_launch_description(): name="namespace", default_value='', description='Namespace') - - declare_robot_description = DeclareLaunchArgument( - name="robot_description", - description="Robot description XML file" - ) + + if __debug == False: + declare_robot_description = DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file" + ) # stdout_linebuf_envvar = SetEnvironmentVariable( # 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') robot_description = {"robot_description":LaunchConfiguration("robot_description")} + if __debug: + # get xacro file path + xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro") + # get error if xacro file if missing + assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file) + # parse xacro file from file with arguments + robot_description_file = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"}) + # convert file to xml format + robot_description_content = robot_description_file.toxml() + robot_description = {'robot_description': robot_description_content} + plansys2_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('plansys2_bringup'), @@ -88,6 +103,23 @@ def generate_launch_description(): 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/move.xml', } ]) + + move_gripper_1 = Node( + package='plansys2_bt_actions', + executable='bt_action_node', + name='move_gripper', + namespace=namespace, + output='screen', + parameters=[ + pkg_dir + '/config/params.yaml', + robot_description, + robot_description_semantic, + kinematics_yaml, + { + 'action_name': 'move_gripper', + 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/move_gripper.xml', + } + ]) # transport_1 = Node( # package='robossembler', @@ -98,13 +130,16 @@ def generate_launch_description(): # parameters=[]) ld = LaunchDescription() - + ld.add_action(declare_namespace_cmd) - ld.add_action(declare_robot_description) + + if __debug == False: + ld.add_action(declare_robot_description) # Declare the launch options ld.add_action(plansys2_cmd) ld.add_action(move_1) + ld.add_action(move_gripper_1) # ld.add_action(transport_1) return ld \ No newline at end of file diff --git a/robossembler/pddl/commands b/robossembler/pddl/commands index 147f8f8..ca6127b 100644 --- a/robossembler/pddl/commands +++ b/robossembler/pddl/commands @@ -1,4 +1,11 @@ +// Move arm to pose set instance rasmt_arm_group robot set instance one zone -set goal (and(robot_move rasmt_arm_group one)) +set goal (and (robot_move rasmt_arm_group one)) +run + +// Move gripper to pose +set instance rasmt_hand_arm_group robot +set instance open gap +set goal (and (gripper_move rasmt_hand_arm_group open)) run \ No newline at end of file diff --git a/robossembler/pddl/domain.pddl b/robossembler/pddl/domain.pddl index 67ac2d8..c15fa22 100644 --- a/robossembler/pddl/domain.pddl +++ b/robossembler/pddl/domain.pddl @@ -5,6 +5,7 @@ (:types robot zone + gap );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -12,6 +13,8 @@ (robot_move ?r - robot ?z - zone) +(gripper_move ?r -robot ?g - gap) + );; end Predicates ;;;;;;;;;;;;;;;;;;;; ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -26,4 +29,10 @@ :effect (and (at end(robot_move ?r ?z))) ) +(:durative-action move_gripper + :parameters (?r - robot ?g - gap) + :duration ( = ?duration 1) + :effect (and (at end(gripper_move ?r ?g))) +) + );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp index 6df2d33..1d4bfa9 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp @@ -11,5 +11,5 @@ int main(int argc, char * argv[]) { - return 0; + return 0; } diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp index 94be412..393ffe6 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp @@ -76,9 +76,8 @@ namespace task_planner pose.orientation.w = coords[6]; } waypoints_[wp] = pose; - } - else - { + } else { + RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str()); } } diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp new file mode 100644 index 0000000..c59c97f --- /dev/null +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp @@ -0,0 +1,152 @@ +#include +#include + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp/qos.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/planning_scene_interface/planning_scene_interface.h" + +#include "robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp" + +namespace task_planner +{ + static const rclcpp::Logger LOGGER = rclcpp::get_logger("task_planner_move_gripper_bt_node"); + + MoveGripper::MoveGripper(const std::string &xml_tag_name, + const BT::NodeConfiguration &conf) + : ActionNodeBase(xml_tag_name, conf) + { + rclcpp::Node::SharedPtr node; + config().blackboard->get("node", node); + + try + { + node->declare_parameter("gripper_group"); + node->declare_parameter("relative_gaps"); + node->declare_parameter("gaps"); + } + catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex) + { + RCLCPP_WARN(LOGGER, "%c", ex.what()); + } + + if (node->has_parameter("gripper_group")) + { + node->get_parameter_or("gripper_group", available_groups_, {}); + }else{ + + RCLCPP_WARN(LOGGER, "No gripper_group names provided"); + } + + if (node->has_parameter("relative_gaps")) + { + std::vector gap_names; + node->get_parameter_or("relative_gaps", gap_names, {}); + for (auto &gap: gap_names) + { + try + { + node->declare_parameter("gaps." + gap); + }catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException & e){ + + } + + double gap_value; + if (node->get_parameter_or("gaps." + gap, gap_value, {})) + { + relative_gaps_[gap] = gap_value; + } else { + + RCLCPP_WARN(LOGGER, "No gap value configured for relative_gap [%s]", gap.c_str()); + } + + } + } + } + + void MoveGripper::halt() + { + _halt_requested = true; + RCLCPP_INFO(LOGGER, "MoveGripper halt"); + } + + BT::NodeStatus MoveGripper::tick() + { + + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_gripper_node = rclcpp::Node::make_shared("robossembler_move_group_interface", node_options); + + config().blackboard->get("node", move_group_gripper_node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_gripper_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + std::string gripper_group; + getInput("gripper_group", gripper_group); + + std::string gap; + getInput("relative_gap", gap); + + if (std::find(available_groups_.begin(), available_groups_.end(), gripper_group) != available_groups_.end()) + { + gripper_group_ = gripper_group; + RCLCPP_INFO(LOGGER, "Provided available move_group [%s]", gripper_group.c_str()); + }else{ + RCLCPP_WARN(LOGGER, "Provided not allowed move_group name [%s]", gripper_group.c_str()); + } + + double gap2moveit; + if (relative_gaps_.find(gap) != relative_gaps_.end()) + { + gap2moveit = relative_gaps_[gap]; + RCLCPP_INFO(LOGGER, "Read gap (%s) value:[%f]", gap, gap2moveit); + } else { + + RCLCPP_WARN(LOGGER, "No value for relative_gap [%s]", gap.c_str()); + } + + static const std::string PLANNING_GROUP = gripper_group_; + RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str()); + + moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description"); + moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_gripper_node, move_group_options); + + auto gripper_joint_values = move_group_interface.getCurrentJointValues(); + moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); + move_group_interface.setStartState(start_state); + + for (auto &joint_value : gripper_joint_values) { + joint_value = gap2moveit > 0? std::min(gap2moveit, max_gripper_gap_): 0.0; + } + + move_group_interface.setJointValueTarget(gripper_joint_values); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + if (success) + { + RCLCPP_INFO(LOGGER, "Planning success"); + move_group_interface.execute(my_plan); + move_group_interface.move(); + } else { + + RCLCPP_WARN(LOGGER, "Failed to generate a plan"); + return BT::NodeStatus::FAILURE; + } + + return BT::NodeStatus::SUCCESS; + } + +} // namespace atomic_skills + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("MoveGripper"); +}