robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

Merge remote-tracking branch 'origin/main' into 12-get_entity_state-BT-node

Browse source
This commit is contained in:
Ilya Uraev 2022-01-26 23:58:17 +04:00
commit 4137af69a1
33 changed files with 470 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

24
.gitlab-ci.yml Normal file
View file

@ -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

9
rasmt_moveit_config/config/rasmt.srdf
View file

@ -13,7 +13,11 @@
<chain base_link="rasmt_Base_Link" tip_link="rasmt_Dock_Link"/>
</group>
<group name="rasmt_hand_arm_group">
<link name="rasmt_Grip_Body"/>
<link name="rasmt_Grip_L"/>
<link name="rasmt_Grip_R"/>
<joint name="rasmt_Slide_1"/>
<joint name="rasmt_Slide_2"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="home_position" group="rasmt_arm_group">
@ -42,15 +46,18 @@
</group_state>
<group_state name="home_hand" group="rasmt_hand_arm_group">
<joint name="rasmt_Slide_1" value="0"/>
<joint name="rasmt_Slide_2" value="0"/>
</group_state>
<group_state name="full_open" group="rasmt_hand_arm_group">
<joint name="rasmt_Slide_1" value="0.04"/>
<joint name="rasmt_Slide_2" value="0.04"/>
</group_state>
<group_state name="full_close" group="rasmt_hand_arm_group">
<joint name="rasmt_Slide_1" value="-0.02"/>
<joint name="rasmt_Slide_2" value="-0.02"/>
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="rasmt_hand" parent_link="rasmt_Dock_Link" group="rasmt_hand_arm_group" parent_group="rasmt_arm_group"/>
<!--end_effector name="rasmt_hand" parent_link="rasmt_Dock_Link" group="rasmt_hand_arm_group" parent_group="rasmt_arm_group"/-->
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="rasmt_Base_Link" link2="rasmt_Fork_1" reason="Adjacent"/>
<disable_collisions link1="rasmt_Base_Link" link2="rasmt_Fork_2" reason="Never"/>

10
rasmt_moveit_config/config/rasmt_moveit.rviz
View file

@ -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

5
rasmt_moveit_config/config/rasmt_moveit_controllers.yaml
View file

@ -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
- rasmt_Slide_2

11
rasmt_support/config/rasmt_ros2_controllers.yaml
View file

@ -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
joints:
- rasmt_Slide_1
- rasmt_Slide_2
command_interfaces:
- position
state_interfaces:
- position
- velocity

BIN
rasmt_support/meshes/collision/Base_Link.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Dock_Link.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Fork_1.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Fork_2.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Fork_3.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Grip_Body.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Grip_L.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Grip_R.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Link_1.STL
View file

Binary file not shown.

BIN
rasmt_support/meshes/collision/Link_2.STL
View file

Binary file not shown.

2
rasmt_support/urdf/rasmt.xacro
View file

@ -8,7 +8,7 @@
<link name="world"/>
<xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)"/>
<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0"/>
<xacro:if value="$(arg grip)">
<xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_macro.xacro"/>

4
rasmt_support/urdf/robot/rasmt_single_macro.xacro
View file

@ -4,14 +4,14 @@
<xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_control.xacro"/>
<xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_gazebo.xacro"/>
<xacro:macro name="rasmt_single" params="prefix parent sim:=^|true">
<xacro:macro name="rasmt_single" params="prefix parent sim xyz">
<joint name="to_${parent}" type="fixed">
<parent link="${parent}"/>
<child link="${prefix}_Base_Link"/>
<!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="${xyz}" rpy="0 0 0"/>
<axis xyz="0 0 0"/>
</joint>

4
rasmt_support/urdf/tools/rasmt_hand_control.xacro
View file

@ -28,8 +28,8 @@
</joint>
<joint name="${prefix}_Slide_2">
<param name="mimic">${prefix}_Slide_1</param>
<param name="multiplier">1</param>
<!-- <param name="mimic">${prefix}_Slide_1</param>
<param name="multiplier">1</param> -->
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>

2
rasmt_support/urdf/tools/rasmt_hand_macro.xacro
View file

@ -179,7 +179,7 @@
lower="-0.02"
upper="0.04"
velocity="0.2"/>
<mimic joint="${prefix}_Slide_1"/>
<!--mimic joint="${prefix}_Slide_1"/-->
</joint>
<xacro:rasmt_hand_hi prefix="${prefix}" sim="${sim}"/>

3
robossembler/CMakeLists.txt
View file

@ -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)

11
robossembler/README.md
View file

@ -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)
```

42
robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml Normal file
View file

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<ReactiveFallback name="PartEstimate">
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
</ReactiveFallback>
<Fallback name="Move">
<Condition ID="PathFree"/>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
</Fallback>
<Action ID="GraspPart" arm_group="${arg3}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg4}"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

7
robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml Normal file
View file

@ -0,0 +1,7 @@
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<MoveGripper name="move_gripper" gripper_group="${arg0}" relative_gap="${arg1}"/>
</Sequence>
</BehaviorTree>
</root>

10
robossembler/config/params.yaml
View file

@ -10,3 +10,13 @@ pick_and_place:
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

2
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
View file

@ -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

56
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp Normal file
View file

@ -0,0 +1,56 @@
#pragma once
#include <string>
#include <vector>
#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<std::string>("gripper_group"),
BT::InputPort<std::string>("relative_gap")
};
}
private:
const double max_gripper_gap_ = 0.04;
std::map<std::string, double> relative_gaps_;
std::vector<std::string> available_groups_;
std::string gripper_group_;
std::atomic_bool _halt_requested;
};
} // namespace task_planner

65
robossembler/launch/robossembler_bringup.launch.py
View file

@ -33,13 +33,22 @@ def generate_launch_description():
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
@ -55,6 +64,7 @@ def generate_launch_description():
]
)
<<<<<<< HEAD
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
@ -106,6 +116,59 @@ 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"))
]
)
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)

11
robossembler/launch/task_planner.launch.py
View file

@ -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'),

3
robossembler/package.xml
View file

@ -15,6 +15,7 @@
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>gazebo_msgs</depend>
<depend>moveit_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
@ -25,8 +26,8 @@
<depend>plansys2_planner</depend>
<depend>plansys2_problem_expert</depend>
<depend>plansys2_pddl_parser</depend>
<depend>ament_index_cpp</depend>
<depend>plansys2_bt_actions</depend>
<depend>ament_index_cpp</depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>

9
robossembler/pddl/commands
View file

@ -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

12
robossembler/pddl/domain.pddl
View file

@ -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 ;;;;;;;;;;;;;;;;;;;;;;;;

7
robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
View file

@ -2,7 +2,7 @@
#include <thread>
#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());
}
}

152
robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp Normal file
View file

@ -0,0 +1,152 @@
#include <iostream>
#include <thread>
#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<std::string> 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<std::string>("gripper_group", gripper_group);
std::string gap;
getInput<std::string>("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<task_planner::MoveGripper>("MoveGripper");
}