Skip to content
Snippets Groups Projects
Select Git revision
  • f87dd3ae79f30ff559b52b4ad1b5d217c613c85a
  • main default protected
  • devel2
  • release
  • devel
5 results

Untitled.pnml

Blame
  • moveit_mediator.h 4.81 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 "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_pose.h>
    #include <moveit/task_constructor/stages/generate_place_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 <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:
        ros::NodeHandle nh_;
        robot_model_loader::RobotModelLoader* robot_model_loader_;
        robot_model::RobotModelPtr kinematic_model_;
        planning_scene::PlanningScene* ps_;
        XmlRpc::XmlRpcValue properties_;
        std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
        std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
        std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
        std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
        ros::Publisher planning_scene_diff_publisher_;
        //trajectory_execution_manager::TrajectoryExecutionManager* tem_;
        moveit::planning_interface::MoveGroupInterface* mgi_;
        std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_ptr_;
    
    
    
    
    
        public:
        Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle 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_ = new moveit::planning_interface::MoveGroupInterface("panda_arms");
            moveit_cpp_ptr_ = std::make_shared<moveit_cpp::MoveItCpp>(nh_);
            
            /*
            ROS_INFO("1");
            planning_scene_monitor::PlanningSceneMonitorPtr psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
            ROS_INFO("2");
            psm_->startSceneMonitor("/move_group/monitored_planning_scene");
            ROS_INFO("3");
            tem_ = new trajectory_execution_manager::TrajectoryExecutionManager(mgi.getRobotModel(), psm_->getStateMonitor());
            ROS_INFO("4");
            */
    
            
            //planning scene publisher
            planning_scene_diff_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
    
            // 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();
    
        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::vector<wing_BP>>& wbp) override;
    
        void merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
        void publish_tables();
        void load_robot_description();
        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