diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 175011743be0cb94d8ace6e5e701acf1faa720d3..66bfbeae2f6bdcf29668b8404782717b57448f43 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 26305 +/home/matteo/reachability/devel/./cmake.lock 26344 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 7a7a5c17fcd9173653df9247ae7439c0066859f1..f0ed7cabb8ff684476fe4573f3f1bfad9c960261 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -140,7 +140,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); @@ -161,7 +161,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); @@ -188,9 +188,9 @@ void Moveit_mediator::setup_task(){ std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks; for (int i = 0 ; i < objects_.size(); i++){ - Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]); std::stringstream ss; - ss << "bottle" << std::to_string(robots_[i]->name().back()); + ss << "bottle" << std::to_string(i); + Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]); std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot; for (int j = 1; j < objects_[i].size(); j++){ moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);