diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..3cbd117 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,24 @@ +image: ros:foxy-ros-base + +workflow: + rules: + - if: '$CI_COMMIT_BRANCH' + when: never + - if: '$CI_PIPELINE_SOURCE == "merge_request_event"' + +stages: + - build + +build-colcon-job: + stage: build + script: + - apt-get update + - mkdir -p .src/robossembler-ros2 + - mv * .src/robossembler-ros2 + - mv .git .src/robossembler-ros2 + - mv .src src + - vcs import src < src/robossembler-ros2/rasms.repos + - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy + - colcon build --merge-install --symlink-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' -Wall -Wextra -Wpedantic + rules: + - if: $CI_COMMIT_REF_NAME != $CI_DEFAULT_BRANCH 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.rviz b/rasmt_moveit_config/config/rasmt_moveit.rviz index ac60364..ce7b49f 100644 --- a/rasmt_moveit_config/config/rasmt_moveit.rviz +++ b/rasmt_moveit_config/config/rasmt_moveit.rviz @@ -326,18 +326,18 @@ Window Geometry: collapsed: false Height: 1016 Hide Left Dock: false - Hide Right Dock: false + Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000042a0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: - collapsed: false + collapsed: true Width: 1848 - X: 315 - Y: 209 + X: 520 + Y: 200 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/meshes/collision/Base_Link.STL b/rasmt_support/meshes/collision/Base_Link.STL index 916cdba..a06ecd0 100644 Binary files a/rasmt_support/meshes/collision/Base_Link.STL and b/rasmt_support/meshes/collision/Base_Link.STL differ diff --git a/rasmt_support/meshes/collision/Dock_Link.STL b/rasmt_support/meshes/collision/Dock_Link.STL index 3f39d98..21625fa 100644 Binary files a/rasmt_support/meshes/collision/Dock_Link.STL and b/rasmt_support/meshes/collision/Dock_Link.STL differ diff --git a/rasmt_support/meshes/collision/Fork_1.STL b/rasmt_support/meshes/collision/Fork_1.STL index e55a4ce..6a6248c 100644 Binary files a/rasmt_support/meshes/collision/Fork_1.STL and b/rasmt_support/meshes/collision/Fork_1.STL differ diff --git a/rasmt_support/meshes/collision/Fork_2.STL b/rasmt_support/meshes/collision/Fork_2.STL index 3535f86..5271a6a 100644 Binary files a/rasmt_support/meshes/collision/Fork_2.STL and b/rasmt_support/meshes/collision/Fork_2.STL differ diff --git a/rasmt_support/meshes/collision/Fork_3.STL b/rasmt_support/meshes/collision/Fork_3.STL index 1f4df96..7228f7b 100644 Binary files a/rasmt_support/meshes/collision/Fork_3.STL and b/rasmt_support/meshes/collision/Fork_3.STL differ diff --git a/rasmt_support/meshes/collision/Grip_Body.STL b/rasmt_support/meshes/collision/Grip_Body.STL index 02acf19..1ede5a5 100644 Binary files a/rasmt_support/meshes/collision/Grip_Body.STL and b/rasmt_support/meshes/collision/Grip_Body.STL differ diff --git a/rasmt_support/meshes/collision/Grip_L.STL b/rasmt_support/meshes/collision/Grip_L.STL index 7c44aaa..068a300 100644 Binary files a/rasmt_support/meshes/collision/Grip_L.STL and b/rasmt_support/meshes/collision/Grip_L.STL differ diff --git a/rasmt_support/meshes/collision/Grip_R.STL b/rasmt_support/meshes/collision/Grip_R.STL index 7c44aaa..068a300 100644 Binary files a/rasmt_support/meshes/collision/Grip_R.STL and b/rasmt_support/meshes/collision/Grip_R.STL differ diff --git a/rasmt_support/meshes/collision/Link_1.STL b/rasmt_support/meshes/collision/Link_1.STL index 414eb2d..d4237da 100644 Binary files a/rasmt_support/meshes/collision/Link_1.STL and b/rasmt_support/meshes/collision/Link_1.STL differ diff --git a/rasmt_support/meshes/collision/Link_2.STL b/rasmt_support/meshes/collision/Link_2.STL index b984ffe..04d61f6 100644 Binary files a/rasmt_support/meshes/collision/Link_2.STL and b/rasmt_support/meshes/collision/Link_2.STL differ diff --git a/rasmt_support/urdf/rasmt.xacro b/rasmt_support/urdf/rasmt.xacro index 433aaaa..ed44547 100644 --- a/rasmt_support/urdf/rasmt.xacro +++ b/rasmt_support/urdf/rasmt.xacro @@ -8,7 +8,7 @@ - + diff --git a/rasmt_support/urdf/robot/rasmt_single_macro.xacro b/rasmt_support/urdf/robot/rasmt_single_macro.xacro index 52d2974..82e8ccf 100644 --- a/rasmt_support/urdf/robot/rasmt_single_macro.xacro +++ b/rasmt_support/urdf/robot/rasmt_single_macro.xacro @@ -4,14 +4,14 @@ - + - + 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 9b1b73b..2cedf77 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 05a6bc1..321d250 100644 --- a/robossembler/CMakeLists.txt +++ b/robossembler/CMakeLists.txt @@ -51,6 +51,9 @@ add_library(robossembler_move_bt_node SHARED src/behavior_tree_nodes/atomic_skil list(APPEND plugin_libs robossembler_move_bt_node) list(APPEND plugin_libs robossembler_emu_pose_estimation) +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/grasp_part.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml new file mode 100644 index 0000000..26a55ff --- /dev/null +++ b/robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 c5d216b..16b13fa 100644 --- a/robossembler/config/params.yaml +++ b/robossembler/config/params.yaml @@ -9,4 +9,14 @@ pick_and_place: waypoint_coords: one: [0.01, 0.01, 0.6, 0.0 , 0.0 , 0.0, - 1.0] \ No newline at end of file + 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/Move.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp index b2a74e7..6f9660a 100644 --- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp +++ b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp @@ -8,7 +8,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "moveit_msgs/msg/move_it_error_codes.hpp" -#include "plansys2_bt_actions/BTActionNode.hpp" +//#include "plansys2_bt_actions/BTActionNode.hpp" #include "rclcpp/rclcpp.hpp" namespace task_planner 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 4d45e8e..c8f40c1 100644 --- a/robossembler/launch/robossembler_bringup.launch.py +++ b/robossembler/launch/robossembler_bringup.launch.py @@ -12,49 +12,59 @@ 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" ) +<<<<<<< HEAD launch_args.append( DeclareLaunchArgument( name="plansys2", default_value="false", description="enable plansys2" ) +======= + ) + launch_args.append( + DeclareLaunchArgument( + name="plansys2", + default_value="true", + description="enable plansys2" +>>>>>>> origin/main ) + ) - # 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") + ] + ) +<<<<<<< HEAD control = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([ @@ -106,12 +116,65 @@ def generate_launch_description(): ], condition=IfCondition(LaunchConfiguration("plansys2")) ) +======= + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "launch", + "rasmt_control.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("sim", LaunchConfiguration("sim")) + ] + ) - launch_nodes = [] - launch_nodes.append(control) - launch_nodes.append(simulation) - launch_nodes.append(moveit) - launch_nodes.append(task_planner_init) + 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")) + ] + ) + + task_planner_init = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("robossembler"), + "launch", + "task_planner.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content) + ], + condition=IfCondition(LaunchConfiguration("plansys2")) + ) +>>>>>>> origin/main + + 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 d219428..03b3590 100644 --- a/robossembler/launch/task_planner.launch.py +++ b/robossembler/launch/task_planner.launch.py @@ -72,6 +72,17 @@ def generate_launch_description(): robot_description = {"robot_description":robot_description_content} + 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'), @@ -116,7 +127,7 @@ def generate_launch_description(): )""" ld = LaunchDescription() - + ld.add_action(declare_namespace_cmd) #ld.add_action(declare_robot_description) #ld.add_action(gz_get_entity_state) diff --git a/robossembler/package.xml b/robossembler/package.xml index 03ee8f4..2d7f830 100644 --- a/robossembler/package.xml +++ b/robossembler/package.xml @@ -15,6 +15,7 @@ rclcpp_action geometry_msgs tf2_geometry_msgs + gazebo_msgs moveit_msgs moveit_core moveit_ros_planning @@ -25,8 +26,8 @@ plansys2_planner plansys2_problem_expert plansys2_pddl_parser - ament_index_cpp plansys2_bt_actions + ament_index_cpp ament_lint_common ament_lint_auto 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 ca78f52..cc415a3 100644 --- a/robossembler/pddl/domain.pddl +++ b/robossembler/pddl/domain.pddl @@ -4,8 +4,8 @@ ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types robot - entity - gripper + zone + gap );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -13,6 +13,8 @@ (entity_move ?r - robot ?e - entity ?g - gripper) +(gripper_move ?r -robot ?g - gap) + );; end Predicates ;;;;;;;;;;;;;;;;;;;; ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -27,4 +29,10 @@ :effect (at end (entity_move ?r ?e ?g)) ) +(: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/Move.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp index cf54696..393ffe6 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp @@ -2,7 +2,7 @@ #include #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" +//#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/qos.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -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"); +}