Select Git revision
pre_gen_project.py
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