diff --git a/gz_black_hole/src/gz_black_hole.cpp b/gz_black_hole/src/gz_black_hole.cpp index 17e9be4aace1b6c60f8f3985d1236b703d4c97eb..b871501327d9176e1f4192964c9cebacd2933dd8 100644 --- a/gz_black_hole/src/gz_black_hole.cpp +++ b/gz_black_hole/src/gz_black_hole.cpp @@ -20,6 +20,7 @@ class BlackHolePlugin : public ModelPlugin transport::SubscriberPtr contactSub_; physics::ModelPtr model_; physics::ContactManager* contact_manager_; + math::Pose trash_; transport::NodePtr node_; std::vector<std::string> collision_names_, stars_; std::string topic_; // contacts topic @@ -27,7 +28,12 @@ class BlackHolePlugin : public ModelPlugin void read_options(sdf::ElementPtr sdf) { // Read gripper link - sdf::ElementPtr elem = sdf->GetElement("star"); + sdf::ElementPtr elem; + + if (sdf->HasElement("star")) + { + elem = sdf->GetElement("star"); + } while (elem) { @@ -36,6 +42,43 @@ class BlackHolePlugin : public ModelPlugin elem = elem->GetNextElement("star"); ROS_INFO("Added star: %s", star.c_str()); } + + bool ok = false; + + elem.reset(); + + if (sdf->HasElement("trash")) + { + elem = sdf->GetElement("trash"); + } + + if (elem) + { + sdf::Pose trash; + ok = elem->GetValue()->Get(trash); + if (ok) + { + trash_.pos.x = trash.pos.x; + trash_.pos.y = trash.pos.y; + trash_.pos.z = trash.pos.z; + trash_.rot.w = trash.rot.w; + trash_.rot.x = trash.rot.x; + trash_.rot.y = trash.rot.y; + trash_.rot.z = trash.rot.z; + } + else + { + ROS_WARN("Invalid content inside the trash's tag."); + } + } + + if (not elem or not ok) + { + ROS_WARN("Using default trash pose: -3 -2 1.5 0 0 0"); + math::Vector3 pos(-3, -2, 1.5); + math::Quaternion rot(0, 0, 0, 1); + trash_ = math::Pose(pos, rot); + } } /** @@ -97,20 +140,31 @@ class BlackHolePlugin : public ModelPlugin physics::CollisionPtr collision2 = boost::dynamic_pointer_cast< physics::Collision>(model_->GetWorld()->GetEntity( msg->contact(i).collision2())); + + // check that the collisions exist (their models could have been + // deleted in previous executions of this callback) + if (not collision1 or not collision2) + { + continue; + } + physics::ModelPtr model1 = collision1->GetModel(); physics::ModelPtr model2 = collision2->GetModel(); + physics::ModelPtr to_delete; // Check if any of the two models involved in the collision is a star. // If so, delete the star and exit the loop (maximum one deletion per // callback to avoid deleting the same model twice) - std::string to_delete; - if (is_star(model1)) to_delete = model1->GetName(); - else if (is_star(model2)) to_delete = model2->GetName(); + //std::string to_delete; + if (is_star(model1)) to_delete = model1; + else if (is_star(model2)) to_delete = model2; - if (not to_delete.empty()) + if (to_delete) { - ROS_INFO("Deleting model %s", to_delete.c_str()); - transport::requestNoReply(node_, "entity_delete", to_delete); + ROS_INFO("Moving away model %s", to_delete->GetName().c_str()); + to_delete->SetWorldPose(trash_); + //transport::requestNoReply(node_, "entity_delete", to_delete); + break; // do not delete more than one model per callback invokation } }