From 56ed60e0fe4465888df25745d33f1df30509dfdf Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Sun, 20 Nov 2022 17:57:54 +0100
Subject: [PATCH] ...

---
 .catkin_tools/profiles/default/devel_collisions.txt | 2 +-
 src/mtc/src/impl/moveit_mediator.cpp                | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 1750117..66bfbea 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 7a7a5c1..f0ed7ca 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]);
-- 
GitLab