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 
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>
13 
14 
15 #include "impl/abstract_mediator.h"
16 #include "impl/abstract_robot.h"
17 #include "impl/robot.h"
18 #include "impl/moveit_robot.h"
19 
21 #include "impl/collision_helper.h"
22 
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>
42 
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>
48 #include "reader/job_reader.h"
49 #include "reader/cuboid_reader.h"
50 
51 #include <stdint.h>
52 
53 
55  protected:
56  std::shared_ptr<ros::NodeHandle> nh_;
57  std::shared_ptr<moveit::core::RobotModel> robot_model_;
58  std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
59 
60  // planning Interfaces and planing scene
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<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
66 
67 
68  // task constructor
69  std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
70  std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
71  std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
72 
73  std::unique_ptr<Job_reader> job_reader_;
74  std::unique_ptr<Cuboid_reader> cuboid_reader_;
75 
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_;
78 
79 
80 
81 
82 
83  public:
84  Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
85 
86  void setup_task();
87  inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
88 
89 
90  bool check_collision(const int& robot) override;
91  void mediate() override;
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;
94  void connect_robots(Abstract_robot* robot) override;
95 
96 
97  void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
98  void merge_acm(moveit_msgs::PlanningScene& in);
99 
100  void task_planner();
101  void publish_tables();
102  void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
103  void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
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;};
107 
108 
109 
110 };
111 
112 #endif
Moveit_mediator::planning_scene_diff_publisher_
std::shared_ptr< ros::Publisher > planning_scene_diff_publisher_
Definition: moveit_mediator.h:64
moveit_robot.h
robot.h
cuboid_reader.h
job_reader.h
Abstract_mediator
Abstract mediator.
Definition: impl/abstract_mediator.h:33
Moveit_mediator::publish_tables
void publish_tables()
Definition: moveit_mediator.cpp:24
Moveit_mediator::mgi_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_
Definition: moveit_mediator.h:61
Moveit_mediator::set_wings
void set_wings(std::vector< std::pair< std::vector< object_data >, int >> &wbp) override
Definition: moveit_mediator.cpp:87
Moveit_mediator::task_map
std::map< std::string, std::vector< moveit::task_constructor::Task > > & task_map()
Definition: moveit_mediator.h:105
Moveit_mediator::cartesian_planner_
std::shared_ptr< moveit::task_constructor::solvers::CartesianPath > cartesian_planner_
Definition: moveit_mediator.h:70
Moveit_mediator::nh_
std::shared_ptr< ros::NodeHandle > nh_
Definition: moveit_mediator.h:56
Moveit_mediator::set_tasks
void set_tasks(std::multimap< std::string, std::pair< tf2::Vector3, std::queue< moveit_task_constructor_msgs::ExecuteTaskSolutionGoal >>> &tasks)
Definition: moveit_mediator.h:106
Moveit_mediator::job_reader_
std::unique_ptr< Job_reader > job_reader_
Definition: moveit_mediator.h:73
Moveit_mediator::cuboid_reader_
std::unique_ptr< Cuboid_reader > cuboid_reader_
Definition: moveit_mediator.h:74
Moveit_mediator::merge_ps
void merge_ps(moveit_msgs::PlanningScene &out, moveit_msgs::PlanningScene *in, Moveit_robot *mr)
Definition: moveit_mediator.cpp:828
Moveit_mediator::check_collision
bool check_collision(const int &robot) override
Definition: moveit_mediator.cpp:61
Moveit_mediator::parallel_exec
void parallel_exec(Moveit_robot &r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps)
Definition: moveit_mediator.cpp:871
wing_moveit_decorator.h
Moveit_mediator::connect_robots
void connect_robots(Abstract_robot *robot) override
Definition: moveit_mediator.cpp:14
Moveit_mediator::psi_
std::unique_ptr< moveit::planning_interface::PlanningSceneInterface > psi_
Definition: moveit_mediator.h:62
abstract_mediator.h
abstract_robot.h
Moveit_mediator::Moveit_mediator
Moveit_mediator(std::vector< std::vector< tf2::Transform >> objects, ros::Publisher *pub, std::shared_ptr< ros::NodeHandle > const &nh)
Definition: moveit_mediator.cpp:1262
collision_helper.h
Moveit_mediator::task_map_
std::map< std::string, std::vector< moveit::task_constructor::Task > > task_map_
Definition: moveit_mediator.h:71
Moveit_mediator::merge_acm
void merge_acm(moveit_msgs::PlanningScene &in)
Definition: moveit_mediator.cpp:812
Moveit_robot
Definition: moveit_robot.h:12
Moveit_mediator::setup_task
void setup_task()
Definition: moveit_mediator.cpp:98
Moveit_mediator::mediate
void mediate() override
Definition: moveit_mediator.cpp:67
Moveit_mediator::acm_
std::map< std::string, std::vector< uint8_t > > acm_
Definition: moveit_mediator.h:76
Moveit_mediator
Definition: moveit_mediator.h:54
Moveit_mediator::sampling_planner_
std::shared_ptr< moveit::task_constructor::solvers::PipelinePlanner > sampling_planner_
Definition: moveit_mediator.h:69
Moveit_mediator::planning_scene_monitor_
std::shared_ptr< planning_scene_monitor::PlanningSceneMonitor > planning_scene_monitor_
Definition: moveit_mediator.h:65
Moveit_mediator::ps_
std::shared_ptr< planning_scene::PlanningScene > ps_
Definition: moveit_mediator.h:63
Moveit_mediator::rewrite_task_template
void rewrite_task_template(Abstract_robot *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Definition: moveit_mediator.cpp:1162
Moveit_mediator::robot_model_
std::shared_ptr< moveit::core::RobotModel > robot_model_
Definition: moveit_mediator.h:57
Moveit_mediator::mgi
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi()
Definition: moveit_mediator.h:87
Moveit_mediator::tasks_
std::multimap< std::string, std::pair< tf2::Vector3, std::queue< moveit_task_constructor_msgs::ExecuteTaskSolutionGoal > > > tasks_
Definition: moveit_mediator.h:77
Moveit_mediator::create_Task
moveit::task_constructor::Task create_Task(Moveit_robot *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Definition: moveit_mediator.cpp:877
Moveit_mediator::task_planner
void task_planner()
Definition: moveit_mediator.cpp:696
Abstract_robot
Definition: impl/abstract_robot.h:25
Moveit_mediator::visual_tools_
std::shared_ptr< moveit_visual_tools::MoveItVisualTools > visual_tools_
Definition: moveit_mediator.h:58
Moveit_mediator::build_wings
void build_wings(std::bitset< 3 > &wing, int &robot) override
Definition: moveit_mediator.cpp:76


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43