1 #ifndef MOVEIT_MEDIATOR_
2 #define MOVEIT_MEDIATOR_
6 #include <moveit/planning_scene_interface/planning_scene_interface.h>
7 #include <moveit/move_group_interface/move_group_interface.h>
8 #include <moveit_msgs/ApplyPlanningScene.h>
9 #include <moveit/planning_scene/planning_scene.h>
10 #include <moveit/robot_model_loader/robot_model_loader.h>
11 #include <moveit/kinematic_constraints/utils.h>
12 #include <moveit_visual_tools/moveit_visual_tools.h>
23 #include <moveit/task_constructor/task.h>
24 #include <moveit/task_constructor/stages/compute_ik.h>
25 #include <moveit/task_constructor/stages/connect.h>
26 #include <moveit/task_constructor/stages/current_state.h>
27 #include <moveit/task_constructor/stages/generate_grasp_pose.h>
28 #include <moveit/task_constructor/stages/generate_place_pose.h>
29 #include <moveit/task_constructor/stages/generate_pose.h>
30 #include <moveit/task_constructor/stages/modify_planning_scene.h>
31 #include <moveit/task_constructor/stages/move_relative.h>
32 #include <moveit/task_constructor/stages/move_to.h>
33 #include <moveit/task_constructor/stages/predicate_filter.h>
34 #include <moveit/task_constructor/solvers/cartesian_path.h>
35 #include <moveit/task_constructor/solvers/pipeline_planner.h>
36 #include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
37 #include <moveit/task_constructor/properties.h>
38 #include <moveit/task_constructor/solvers/joint_interpolation.h>
39 #include <moveit/task_constructor/solvers/planner_interface.h>
40 #include <moveit/task_constructor/stage.h>
41 #include <moveit/task_constructor/stages/fixed_state.h>
43 #include <eigen_conversions/eigen_msg.h>
44 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
45 #include <moveit/planning_scene_monitor/current_state_monitor.h>
46 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
47 #include <moveit/moveit_cpp/moveit_cpp.h>
56 std::shared_ptr<ros::NodeHandle>
nh_;
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_;
71 std::map<std::string, std::vector<moveit::task_constructor::Task>>
task_map_;
76 std::map<std::string, std::vector<uint8_t>>
acm_;
77 std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>
tasks_;
84 Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle>
const& nh);
87 inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
mgi() {
return mgi_;};
92 void build_wings(std::bitset<3>& wing,
int& robot)
override;
93 void set_wings(std::vector<std::pair<std::vector<object_data>,
int>>& wbp)
override;
97 void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in,
Moveit_robot* mr);
98 void merge_acm(moveit_msgs::PlanningScene& in);
104 moveit::task_constructor::Task
create_Task(
Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
105 inline std::map<std::string, std::vector<moveit::task_constructor::Task>>&
task_map(){
return task_map_;};
106 inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {
tasks_ = tasks;};