Skip to content
Snippets Groups Projects
Select Git revision
  • 053500d1f78009a2875609b6bd78fb2523f6ec8f
  • main default protected
  • mg2bt
  • Part1
4 results

moveit_mediator.h

Blame
  • user avatar
    KingMaZito authored
    053500d1
    History
    moveit_mediator.h 4.67 KiB
    #ifndef MOVEIT_MEDIATOR_
    #define MOVEIT_MEDIATOR_
    
    #include <ros/ros.h>
    
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit/move_group_interface/move_group_interface.h>
    #include <moveit_msgs/ApplyPlanningScene.h>
    #include <moveit/planning_scene/planning_scene.h>
    #include <moveit/robot_model_loader/robot_model_loader.h>
    #include <moveit/kinematic_constraints/utils.h>
    #include <moveit_visual_tools/moveit_visual_tools.h>
    
    
    #include "impl/abstract_mediator.h"
    #include "impl/abstract_robot.h"
    #include "impl/robot.h"
    #include "impl/moveit_robot.h"
    
    #include "impl/wing_moveit_decorator.h"
    #include "impl/collision_helper.h"
    
    #include <moveit/task_constructor/task.h>
    #include <moveit/task_constructor/stages/compute_ik.h>
    #include <moveit/task_constructor/stages/connect.h>
    #include <moveit/task_constructor/stages/current_state.h>
    #include <moveit/task_constructor/stages/generate_grasp_pose.h>
    #include <moveit/task_constructor/stages/generate_place_pose.h>
    #include <moveit/task_constructor/stages/generate_pose.h>
    #include <moveit/task_constructor/stages/modify_planning_scene.h>
    #include <moveit/task_constructor/stages/move_relative.h>
    #include <moveit/task_constructor/stages/move_to.h>
    #include <moveit/task_constructor/stages/predicate_filter.h>
    #include <moveit/task_constructor/solvers/cartesian_path.h>
    #include <moveit/task_constructor/solvers/pipeline_planner.h>
    #include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
    #include <moveit/task_constructor/properties.h>
    #include <moveit/task_constructor/solvers/joint_interpolation.h>
    #include <moveit/task_constructor/solvers/planner_interface.h>
    #include <moveit/task_constructor/stage.h>
    #include <moveit/task_constructor/stages/fixed_state.h>
    
    #include <eigen_conversions/eigen_msg.h>
    #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
    #include <moveit/planning_scene_monitor/current_state_monitor.h>
    #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
    #include <moveit/moveit_cpp/moveit_cpp.h>
    #include "reader/job_reader.h"
    #include "reader/cuboid_reader.h"
    
    #include <stdint.h>
    
    
    class Moveit_mediator : public Abstract_mediator{
        protected:
        std::shared_ptr<ros::NodeHandle> nh_;
        std::shared_ptr<moveit::core::RobotModel> robot_model_;
        std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
    
        // planning Interfaces and planing scene 
        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_;
        std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
        std::shared_ptr<planning_scene::PlanningScene> ps_;
        std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_;
        std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
    
    
        // task constructor
        std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
        std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
        std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
    
        std::unique_ptr<Job_reader> job_reader_; 
        std::unique_ptr<Cuboid_reader> cuboid_reader_;
    
        std::map<std::string, std::vector<uint8_t>> acm_;
        std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
    
    
    
    
    
        public:
        Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
    
        void setup_task();
        inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
    
    
        bool check_collision(const int& robot) override;
        void mediate() override;
        void build_wings(std::bitset<3>& wing,int& robot) override;
        void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
        void connect_robots(Abstract_robot* robot) override;
    
    
        void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
        void merge_acm(moveit_msgs::PlanningScene& in);
    
        void task_planner();
        void publish_tables();
        void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
        void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
        moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
        inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
        inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
    
    
    
    };
    
    #endif