diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index df4c3405384153530cb533ae2d4fe8e16102bfc8..5a4ae8758f714576b2c123e9915e687f5b980bf8 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 24515 +/home/matteo/reachability/devel/./cmake.lock 25109 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h index d7bf3707f4d88b52cf9bf3a637f12da78ba1cae1..e58276b081db9bbb536fb0e7aa2840f6e04b828b 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/src/mtc/include/impl/moveit_mediator.h @@ -44,12 +44,17 @@ class Moveit_mediator : public Abstract_mediator{ 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_; 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>()){ + //planning scene publisher + planning_scene_diff_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1); + + // planner sampling_planner_->setProperty("goal_joint_tolerance", 1e-5); @@ -71,7 +76,7 @@ class Moveit_mediator : public Abstract_mediator{ void publish_tables(); void load_robot_description(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); - void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target); + void parallel_exec(Abstract_robot& r, moveit::task_constructor::Task& task); moveit::task_constructor::Task create_Task(Abstract_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_;}; diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h index e798395308e5ee18042669769210f8fa8ac183bd..ae1c707684a95ac47bc6da6152f5f60dbbe826a4 100644 --- a/src/mtc/include/impl/moveit_robot.h +++ b/src/mtc/include/impl/moveit_robot.h @@ -25,7 +25,6 @@ class Moveit_robot : public Robot{ map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str())); map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str())); - } inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;} diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index e88799b9301dfe7644f674f3d7c29cc54c764ae9..a808d5e4adfc4c5fde0828bbd04ceee86cf38359 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -8,7 +8,7 @@ <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/> - <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/> + <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/> <include file="$(find ceti_double)/launch/demo.launch"> diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index b9788bd81632f9f5c5f8f80c101b2095b7b8553e..0a520e402ac5e7a61ff485b66f1274044aa811c9 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -136,7 +136,7 @@ void Moveit_mediator::setup_task(){ } else { moveit_msgs::CollisionObject bottle1; - bottle1.id = "bottle1"; + bottle1.id = "bottle0"; bottle1.header.frame_id = "world"; bottle1.header.stamp = ros::Time::now(); bottle1.primitives.resize(1); @@ -157,7 +157,7 @@ void Moveit_mediator::setup_task(){ bottle1.operation = bottle1.ADD; moveit_msgs::CollisionObject bottle2; - bottle2.id = "bottle2"; + bottle2.id = "bottle1"; bottle2.header.frame_id = "world"; bottle2.header.stamp = ros::Time::now(); bottle2.primitives.resize(1); @@ -180,27 +180,31 @@ void Moveit_mediator::setup_task(){ psi_->applyCollisionObject(bottle1); psi_->applyCollisionObject(bottle2); - /* - for(int j =0; j < objects_[0].size();j++){ - create_Task(robots_[0], psi_->getObjects().at("bottle1"), objects_[0][j]); - } - for(int j =0; j < objects_[1].size();j++){ - create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]); - } - */ - //std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0])); - //std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1])); - //one.join(); - //two.join(); + for (int i = 0 ; i < objects_.size(); i++){ + std::stringstream ss; + ss << "bottle" << std::to_string(i); + for(int j =0; j < objects_[i].size();j++){ + moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at(ss.str()), objects_[i][j]); + if (task.plan(1)){ + parallel_exec(std::ref(*robots_[i]), std::ref(task)); + //std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(task)); + //one.join(); + } + } + } } } -void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target){ - for(int j =0; j < target.size();j++){ - create_Task(&ar, source, target[j]); +void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit::task_constructor::Task& task){ + Moveit_robot* mr = dynamic_cast<Moveit_robot*>(&ar); + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg; + task.solutions().front()->fillMessage(etsg.solution, &task.introspection()); + for(auto st : etsg.solution.sub_trajectory){ + mr->mgi()->execute(st.trajectory); + planning_scene_diff_publisher_.publish(st.scene_diff); } }