moveit_mediator.h
Go to the documentation of this file.
1 #ifndef MOVEIT_MEDIATOR_
2 #define MOVEIT_MEDIATOR_
3 
4 #include <ros/ros.h>
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>
40 #include <stdint.h>
41 
42 
46 #include "reader/job_reader.h"
47 #include "reader/cuboid_reader.h"
48 #include "reader/wing_reader.h"
49 #include "bt/execution.h"
50 #include "bt/position_condition.h"
51 #include "bt/parallel_robot.h"
52 
53 
55 
59  protected:
60  std::shared_ptr<moveit::core::RobotModel> robot_model_;
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_;
64  std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_;
65  std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
66  std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
67  std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
68  std::unique_ptr<JobReader> job_reader_;
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_;
73 
74 
75 
76 
77  public:
79 
83  MoveitMediator(std::shared_ptr<ros::NodeHandle> const& nh);
84 
85  inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
86 
88 
91  void mediate() override;
92 
94 
98  void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override;
99 
100 
102 
107  void mergePS(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, AbstractRobotDecorator* mr);
108 
110 
114  void parallelExec(AbstractRobotDecorator & mr, moveit_msgs::RobotTrajectory rt);
115 
117 
120  void mergeACM(moveit_msgs::PlanningScene& in);
121 
123 
126  void taskPlanner();
127 
129  void publishTables();
130 
132 
136  void manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::PlanningScene& ps);
137 
139 
144  moveit::task_constructor::Task createTask(AbstractRobotDecorator* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
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_;};
147 
148  inline void setTasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
149 
150 
152  void setPanel() override;
153 
154 };
155 
156 #endif
MoveitMediator::setPanel
void setPanel() override
Sets panels for robots.
Definition: moveit_mediator.cpp:57
MoveitMediator::rs_
std::map< std::string, std::vector< uint8_t > > rs_
shared robot state between all robots
Definition: moveit_mediator.h:70
MoveitMediator::acm_
std::map< std::string, std::vector< uint8_t > > acm_
shared allowed collision matrix between robots
Definition: moveit_mediator.h:69
cuboid_reader.h
parallel_robot.h
job_reader.h
MoveitMediator::taskMap
std::map< std::string, std::vector< moveit::task_constructor::Task > > & taskMap()
Definition: moveit_mediator.h:146
MoveitMediator::mergeACM
void mergeACM(moveit_msgs::PlanningScene &in)
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:521
log_decorator.h
MoveitMediator
Refined MoveitMediator.
Definition: moveit_mediator.h:58
MoveitMediator::mgi_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_
Move Group Interface of the whole multi-cell.
Definition: moveit_mediator.h:61
abstract_robot_decorator.h
MoveitMediator::executions_
std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene > > executions_
Shared execution map containing groot node information.
Definition: moveit_mediator.h:72
MoveitMediator::job_reader_
std::unique_ptr< JobReader > job_reader_
jobReader instancde which reads task information
Definition: moveit_mediator.h:68
MoveitMediator::taskPlanner
void taskPlanner()
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:236
MoveitMediator::task_map_
std::map< std::string, std::vector< moveit::task_constructor::Task > > task_map_
Tasks mapped to Robot.
Definition: moveit_mediator.h:67
MoveitMediator::MoveitMediator
MoveitMediator(std::shared_ptr< ros::NodeHandle > const &nh)
AbstractMediator constructor.
Definition: moveit_mediator.cpp:12
MoveitMediator::tasks_
std::multimap< std::string, std::pair< tf2::Vector3, std::queue< moveit_task_constructor_msgs::ExecuteTaskSolutionGoal > > > tasks_
tasks multimap
Definition: moveit_mediator.h:71
MoveitMediator::ps_
std::shared_ptr< planning_scene::PlanningScene > ps_
Shared Planning Scene.
Definition: moveit_mediator.h:63
AbstractMediator
AbstractMediator.
Definition: abstract_mediator.h:43
MoveitMediator::cartesian_planner_
std::shared_ptr< moveit::task_constructor::solvers::CartesianPath > cartesian_planner_
Moveit task Constructior cartesian planner.
Definition: moveit_mediator.h:66
MoveitMediator::parallelExec
void parallelExec(AbstractRobotDecorator &mr, moveit_msgs::RobotTrajectory rt)
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:585
MoveitMediator::manipulateACM
void manipulateACM(AbstractRobotDecorator *mr, moveit_msgs::PlanningScene &ps)
Manipulate ACM by extracting inforamtion of spicified robot.
Definition: moveit_mediator.cpp:118
wing_reader.h
MoveitMediator::executions
std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene > > & executions()
Definition: moveit_mediator.h:145
AbstractRobotDecorator
Abstract Robot Decorator.
Definition: abstract_robot_decorator.h:22
MoveitMediator::planning_scene_diff_publisher_
std::shared_ptr< ros::Publisher > planning_scene_diff_publisher_
Publisher to manage PlanningScene diffs.
Definition: moveit_mediator.h:64
MoveitMediator::setTasks
void setTasks(std::multimap< std::string, std::pair< tf2::Vector3, std::queue< moveit_task_constructor_msgs::ExecuteTaskSolutionGoal >>> &tasks)
Definition: moveit_mediator.h:148
MoveitMediator::connectRobots
void connectRobots(std::unique_ptr< AbstractRobotDecorator > robot) override
connect robot and initialize Moveit components
Definition: moveit_mediator.cpp:36
MoveitMediator::psi_
std::unique_ptr< moveit::planning_interface::PlanningSceneInterface > psi_
PlanningSceneInteface to manage Scene Objects.
Definition: moveit_mediator.h:62
position_condition.h
MoveitMediator::sampling_planner_
std::shared_ptr< moveit::task_constructor::solvers::PipelinePlanner > sampling_planner_
Moveit task Constructior simple planner.
Definition: moveit_mediator.h:65
MoveitMediator::robot_model_
std::shared_ptr< moveit::core::RobotModel > robot_model_
Moveit Robot-Model as specified in SDF.
Definition: moveit_mediator.h:60
MoveitMediator::createTask
moveit::task_constructor::Task createTask(AbstractRobotDecorator *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Create Task with Moveit Task Constructor.
Definition: moveit_mediator.cpp:589
execution.h
MoveitMediator::mergePS
void mergePS(moveit_msgs::PlanningScene &out, moveit_msgs::PlanningScene in, AbstractRobotDecorator *mr)
Merge Planning scene.
Definition: moveit_mediator.cpp:545
MoveitMediator::mgi
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi()
Definition: moveit_mediator.h:85
MoveitMediator::publishTables
void publishTables()
publish all panels in the planning scene
Definition: moveit_mediator.cpp:94
moveit_panel.h
MoveitMediator::mediate
void mediate() override
mediator function
Definition: moveit_mediator.cpp:111


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51