Go to the documentation of this file. 1 #ifndef MOVEIT_MEDIATOR_
2 #define MOVEIT_MEDIATOR_
5 #include <moveit/planning_scene_interface/planning_scene_interface.h>
6 #include <moveit/move_group_interface/move_group_interface.h>
7 #include <moveit_msgs/ApplyPlanningScene.h>
8 #include <moveit/planning_scene/planning_scene.h>
9 #include <moveit/robot_model_loader/robot_model_loader.h>
10 #include <moveit/kinematic_constraints/utils.h>
11 #include <moveit_visual_tools/moveit_visual_tools.h>
12 #include <moveit/task_constructor/task.h>
13 #include <moveit/task_constructor/stages/compute_ik.h>
14 #include <moveit/task_constructor/stages/connect.h>
15 #include <moveit/task_constructor/stages/current_state.h>
16 #include <moveit/task_constructor/stages/generate_grasp_pose.h>
17 #include <moveit/task_constructor/stages/generate_place_pose.h>
18 #include <moveit/task_constructor/stages/generate_pose.h>
19 #include <moveit/task_constructor/stages/modify_planning_scene.h>
20 #include <moveit/task_constructor/stages/move_relative.h>
21 #include <moveit/task_constructor/stages/move_to.h>
22 #include <moveit/task_constructor/stages/predicate_filter.h>
23 #include <moveit/task_constructor/solvers/cartesian_path.h>
24 #include <moveit/task_constructor/solvers/pipeline_planner.h>
25 #include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
26 #include <moveit/task_constructor/properties.h>
27 #include <moveit/task_constructor/solvers/joint_interpolation.h>
28 #include <moveit/task_constructor/solvers/planner_interface.h>
29 #include <moveit/task_constructor/stage.h>
30 #include <moveit/task_constructor/stages/fixed_state.h>
31 #include <eigen_conversions/eigen_msg.h>
32 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
33 #include <moveit/planning_scene_monitor/current_state_monitor.h>
34 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
35 #include <moveit/moveit_cpp/moveit_cpp.h>
36 #include <behaviortree_cpp_v3/behavior_tree.h>
37 #include <behaviortree_cpp_v3/tree_node.h>
38 #include <behaviortree_cpp_v3/bt_factory.h>
39 #include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
61 std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi_;
62 std::unique_ptr<moveit::planning_interface::PlanningSceneInterface>
psi_;
63 std::shared_ptr<planning_scene::PlanningScene>
ps_;
67 std::map<std::string, std::vector<moveit::task_constructor::Task>>
task_map_;
69 std::map<std::string, std::vector<uint8_t>>
acm_;
70 std::map<std::string, std::vector<uint8_t>>
rs_;
71 std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>
tasks_;
72 std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>
executions_;
85 inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi() {
return mgi_;};
98 void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot)
override;
120 void mergeACM(moveit_msgs::PlanningScene& in);
145 inline std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>&
executions(){
return executions_;};
146 inline std::map<std::string, std::vector<moveit::task_constructor::Task>>&
taskMap(){
return task_map_;};
148 inline void setTasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {
tasks_ = tasks;};
Abstract Robot Decorator.