now the black hole is more of a worm hole :) (teleports objects instead of deleting them)

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
         }
 
       }