Skip to content
Snippets Groups Projects
Commit e0036b96 authored by KingMaZito's avatar KingMaZito
Browse files

...

parent f29c44ff
No related branches found
No related tags found
No related merge requests found
Pipeline #15004 failed
/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
......@@ -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
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment