moveit_mediator.h

#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>


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_;


    public:
    Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh) 
    : Abstract_mediator(objects, pub), nh_(nh)
    , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
    , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
    , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
    , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
    , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
    , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){

        robot_model_loader::RobotModelLoaderPtr robot_model_loader;
        robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
        robot_model_ = robot_model_loader->getModel();
        ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);

        // planner 
        sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);

        // cartesian
	    cartesian_planner_->setMaxVelocityScaling(1.0);
	    cartesian_planner_->setMaxAccelerationScaling(1.0);
	    cartesian_planner_->setStepSize(.01);

    };

    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 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_;};


};

#endif