diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 85e720efd862f0186b026050ac9089fe49892e14..175011743be0cb94d8ace6e5e701acf1faa720d3 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 26292 +/home/matteo/reachability/devel/./cmake.lock 26305 /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 6510241a217d208e1a6d57778c0f25f34eebc913..7a7a5c17fcd9173653df9247ae7439c0066859f1 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 = "bottle0"; + bottle1.id = "bottle1"; 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 = "bottle1"; + bottle2.id = "bottle2"; 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++){ - std::stringstream ss; - ss << "bottle" << std::to_string(i); Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]); + std::stringstream ss; + ss << "bottle" << std::to_string(robots_[i]->name().back()); 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]); @@ -256,10 +256,13 @@ void Moveit_mediator::setup_task(){ ROS_INFO("entry names"); for (auto den : ps1->allowed_collision_matrix.entry_names){ ROS_INFO("%s", den.c_str()); + ROS_INFO("count %i", ps1->allowed_collision_matrix.entry_names.size()); } ROS_INFO("entry values"); for (auto den : ps1->allowed_collision_matrix.entry_values){ for (auto ene : den.enabled) ROS_INFO("%i", ene); + ROS_INFO("count %i", den.enabled.size()); + } ROS_INFO("[Robot 1] fft"); for (auto ft : ps1->fixed_frame_transforms){ @@ -326,10 +329,13 @@ void Moveit_mediator::setup_task(){ ROS_INFO("entry names"); for (auto den : ps2->allowed_collision_matrix.entry_names){ ROS_INFO("%s", den.c_str()); + ROS_INFO("count %i", ps2->allowed_collision_matrix.entry_names.size()); + } ROS_INFO("entry values"); for (auto den : ps2->allowed_collision_matrix.entry_values){ for (auto ene : den.enabled) ROS_INFO("%i", ene); + ROS_INFO("count %i", den.enabled.size()); } ROS_INFO("[Robot 2] fft"); for (auto ft : ps2->fixed_frame_transforms){ @@ -403,6 +409,9 @@ void Moveit_mediator::setup_task(){ } void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){ + for (auto acm : merge->allowed_collision_matrix.default_entry_names){ + // dafd + } }