Merged in JenniferBuehler/gazebo/contact_manager_enforcable (pull req…

…uest #2629) Possibility to enforce contact addition in ContactManager even if there are no subscribers Approved-by: Steven Peters <[email protected]> Approved-by: Adam Allevato <[email protected]>
gazebosim · May 9, 2017 · 4a10aa2 · 4a10aa2
2 parents 095ee3a + 1313550
commit 4a10aa2
Show file tree

Hide file tree

Showing 5 changed files with 245 additions and 59 deletions.
diff --git a/Changelog.md b/Changelog.md
@@ -17,6 +17,9 @@
 1. Added GpuLaserDataIterator
     * [Pull request #2637](https://bitbucket.org/osrf/gazebo/pull-request/2637)
 
+1. Added possibility to enforce contact computation
+    * [Pull request #2629](https://bitbucket.org/osrf/gazebo/pull-requests/2629/)
+
 ## Gazebo 8
 
 ## Gazebo 8.x.x (2017-xx-xx)

diff --git a/gazebo/physics/ContactManager.cc b/gazebo/physics/ContactManager.cc
@@ -41,6 +41,7 @@ ContactManager::ContactManager()
 {
   this->contactIndex = 0;
   this->customMutex = new boost::recursive_mutex();
+  this->neverDropContacts = false;
 }
 
 /////////////////////////////////////////////////
@@ -83,59 +84,134 @@ void ContactManager::Init(WorldPtr _world)
 }
 
 /////////////////////////////////////////////////
-Contact *ContactManager::NewContact(Collision *_collision1,
-                                    Collision *_collision2,
-                                    const common::Time &_time)
+void ContactManager::SetNeverDropContacts(const bool _neverDrop)
 {
-  Contact *result = NULL;
+  this->neverDropContacts = _neverDrop;
+}
 
-  if (!_collision1 || !_collision2)
-    return result;
+/////////////////////////////////////////////////
+bool ContactManager::NeverDropContacts() const
+{
+  return this->neverDropContacts;
+}
 
-  // If no one is listening to the default topic, or there are no
-  // custom contact publishers then don't create any contact information.
-  // This is a signal to the Physics engine that it can skip the extra
-  // processing necessary to get back contact information.
+/////////////////////////////////////////////////
+bool ContactManager::SubscribersConnected(Collision *_collision1,
+                                          Collision *_collision2) const
+{
+  if (this->contactPub->HasConnections()) return true;
 
-  std::vector<ContactPublisher *> publishers;
+  boost::recursive_mutex::scoped_lock lock(*this->customMutex);
+  boost::unordered_map<std::string, ContactPublisher *>::const_iterator iter;
+  for (iter = this->customContactPublishers.begin();
+       iter != this->customContactPublishers.end(); ++iter)
   {
-    boost::recursive_mutex::scoped_lock lock(*this->customMutex);
-    boost::unordered_map<std::string, ContactPublisher *>::iterator iter;
-    for (iter = this->customContactPublishers.begin();
-        iter != this->customContactPublishers.end(); ++iter)
+    // A model can simply be loaded later, so check the collisionNames as well.
+    if (!iter->second->collisionNames.empty())
     {
-      // A model can simply be loaded later, so convert ones that are not yet
-      // found
-      if (!iter->second->collisionNames.empty())
+      std::vector<std::string>::const_iterator it;
+      for (it = iter->second->collisionNames.begin();
+           it != iter->second->collisionNames.end(); ++it)
       {
-        std::vector<std::string>::iterator it;
-        for (it = iter->second->collisionNames.begin();
-            it != iter->second->collisionNames.end();)
+        BasePtr b = this->world->BaseByName(*it);
+        if (b)
         {
-          Collision *col = boost::dynamic_pointer_cast<Collision>(
-              this->world->BaseByName(*it)).get();
-          if (!col)
-          {
-            ++it;
-            continue;
-          }
-          else
-            it = iter->second->collisionNames.erase(it);
-          iter->second->collisions.insert(col);
+          return true;
         }
+        // We could do the same transformation which is done in
+        // GetCustomPublishers() here (insert collisions which now have been
+        // loaded), but this would remove the const qualifier of this function.
+        // It would however speed up repeated calls of this function without
+        // a call of NewContact() or GetCustomPublishers() in between.
       }
+    }
+
+    // only reason _collision1 or _collision1 cannot be const parameters
+    // is that compiler can't find const pointers in unordered set
+    if (iter->second->collisions.find(_collision1) !=
+        iter->second->collisions.end() ||
+        iter->second->collisions.find(_collision2) !=
+        iter->second->collisions.end())
+    {
+      return true;
+    }
+  }
+  return false;
+}
 
-      if (iter->second->collisions.find(_collision1) !=
-          iter->second->collisions.end() ||
-          iter->second->collisions.find(_collision2) !=
-          iter->second->collisions.end())
+/////////////////////////////////////////////////
+void ContactManager::GetCustomPublishers(Collision *_collision1,
+                     Collision *_collision2, const bool _getOnlyConnected,
+                     std::vector<ContactPublisher*> &_publishers)
+{
+  boost::recursive_mutex::scoped_lock lock(*this->customMutex);
+  boost::unordered_map<std::string, ContactPublisher *>::iterator iter;
+  for (iter = this->customContactPublishers.begin();
+       iter != this->customContactPublishers.end(); ++iter)
+  {
+    // A model can simply be loaded later, so convert ones that are not yet
+    // found
+    if (!iter->second->collisionNames.empty())
+    {
+      std::vector<std::string>::iterator it;
+      for (it = iter->second->collisionNames.begin();
+          it != iter->second->collisionNames.end();)
+      {
+        Collision *col = boost::dynamic_pointer_cast<Collision>(
+            this->world->BaseByName(*it)).get();
+        if (!col)
+        {
+          ++it;
+          continue;
+        }
+        it = iter->second->collisionNames.erase(it);
+        iter->second->collisions.insert(col);
+      }
+    }
+
+    // only reason _collision1 or _collision1 cannot be const parameters
+    // is that compiler can't find const pointers in unordered set
+    if (iter->second->collisions.find(_collision1) !=
+        iter->second->collisions.end() ||
+        iter->second->collisions.find(_collision2) !=
+        iter->second->collisions.end())
+    {
+      GZ_ASSERT(iter->second->publisher != NULL,
+                "ContactPublisher must have a valid publisher");
+      if (!_getOnlyConnected || iter->second->publisher->HasConnections())
       {
-        publishers.push_back(iter->second);
+        _publishers.push_back(iter->second);
       }
     }
   }
+}
+
+/////////////////////////////////////////////////
+Contact *ContactManager::NewContact(Collision *_collision1,
+                                    Collision *_collision2,
+                                    const common::Time &_time)
+{
+  Contact *result = NULL;
+
+  if (!_collision1 || !_collision2)
+    return result;
 
-  if (this->contactPub->HasConnections() || !publishers.empty())
+  // If no one is listening to the default topic, or there are no
+  // custom contact publishers then don't create any contact information.
+  // This is a signal to the Physics engine that it can skip the extra
+  // processing necessary to get back contact information.
+
+  std::vector<ContactPublisher *> publishers;
+  bool getOnlyConnected = false;
+  // TODO check: getOnlyConnected set to false to keep same behaviour as before.
+  // But should we not only add publishers which are connected, as is done
+  // for this->contactPub->HasConnections() condition?
+  this->GetCustomPublishers(_collision1, _collision2,
+                            getOnlyConnected, publishers);
+
+  if (this->NeverDropContacts() ||
+      this->contactPub->HasConnections() ||
+      !publishers.empty())
   {
     // Get or create a contact feedback object.
     if (this->contactIndex < this->contacts.size())

diff --git a/gazebo/physics/ContactManager.hh b/gazebo/physics/ContactManager.hh
@@ -86,20 +86,60 @@ namespace gazebo
 
       /// \brief Add a new contact.
       ///
-      /// Noramlly this is only used by a Physics/Collision engine when
+      /// Normally this is only used by a Physics/Collision engine when
       /// a new contact is generated. All other users should just make use
       /// of the accessor functions.
       ///
-      /// If no one is listening, then the return value will be NULL.
-      /// This is a signal to the Physics engine that it can skip the extra
-      /// processing necessary to get back contact information.
+      /// If no one is listening, then the return value will be NULL (unless
+      /// SetNeverDropContacts(true) was called).
+      /// This is then a signal to the Physics engine that it can skip the
+      /// extra processing necessary to get back contact information.
+      ///
+      /// \param[in] _collision1 the first collision object
+      /// \param[in] _collision2 the second collision object
+      /// \param[in] _time the time of the contact
+      ///
       /// \return The new contact. The physics engine should populate the
       /// contact's parameters. NULL will be returned if there are no
-      /// subscribers to the contact topic.
+      /// subscribers to the contact topic and NeverDropContacts()
+      /// returns false (default).
       public: Contact *NewContact(Collision *_collision1,
                                   Collision *_collision2,
                                   const common::Time &_time);
 
+      /// \brief If set to true, NewContact() will always add contacts
+      /// even if there are no subscribers.
+      /// \param[in] _neverDrop if true, never drop contact computation
+      public: void SetNeverDropContacts(const bool _neverDrop);
+
+      /// \brief returns the value last set with SetNeverDropContacts().
+      /// \return the value last set with SetNeverDropContacts().
+      /// If SetNeverDropContacts() was never called, this will return false.
+      public: bool NeverDropContacts() const;
+
+      /// \brief Returns true if any subscribers are connected
+      /// which would be interested in contact details of either collision
+      /// \e _collision1 or \e collision2, given that they have been loaded
+      /// into the world already.
+      /// This is the same test which NewContact() uses to determine whether
+      /// there are any subscribers for the contacts, but the test here is
+      /// optimized because it returns as soon as one subscriber is found which
+      /// listens to either of the collisions.
+      /// Note that this function needs to go through the list of custom
+      /// publishers and check whether \e _collsion1 or \e _collision2
+      /// are part of the custom publishers and whether they have been loaded
+      /// into the world, which comes at a cost.
+      /// Unless there are any benefits in calling this function ahead of
+      /// NewContact, it may be better to just use NewContact() directly.
+      /// Also note that in order to exclude that NewContact() returns NULL,
+      /// it is advisable to check NeverDropContacts() first (if it returns
+      /// true, NewContacts() never returns NULL).
+      /// \param[in] _collision1 the first collision object
+      /// \param[in] _collision2 the second collision object
+      /// \return true if any subscribers are connected for this pair
+      public: bool SubscribersConnected(Collision *_collision1,
+                                        Collision *_collision2) const;
+
       /// \brief Return the number of valid contacts.
       public: unsigned int GetContactCount() const;
 
@@ -164,6 +204,17 @@ namespace gazebo
       /// return True if the filter exists.
       public: bool HasFilter(const std::string &_name);
 
+      /// \brief Helper function which gets the custom publishers which publish
+      ///   contacts of either \e _collision1 or \e _collision2.
+      /// \param[in] _collision1 the first collision object
+      /// \param[in] _collision2 the second collision object
+      /// \param[in] _getOnlyConnected return only publishers which currently
+      ///   have subscribers
+      /// \param[out] _publishers the resulting publishers.
+      private: void GetCustomPublishers(Collision *_collision1,
+                       Collision *_collision2, const bool _getOnlyConnected,
+                       std::vector<ContactPublisher*> &_publishers);
+
       private: std::vector<Contact*> contacts;
 
       private: unsigned int contactIndex;
@@ -193,6 +244,11 @@ namespace gazebo
 
       /// \brief Contact publisher.
       private: ignition::transport::Node::Publisher contactPubIgn;
+
+      /// \brief Addition of new contacts happens also with no subscribers.
+      /// This takes effect if NewContact() is called if there
+      /// are no subscribers. Default is false.
+      private: bool neverDropContacts;
     };
     /// \}
   }

diff --git a/gazebo/physics/ContactManager_TEST.cc b/gazebo/physics/ContactManager_TEST.cc
@@ -31,14 +31,14 @@ TEST_F(ContactManagerTest, CreateFilter)
 
   // Get a pointer to the world, make sure world loads
   physics::WorldPtr world = physics::get_world("default");
-  ASSERT_TRUE(world != NULL);
+  ASSERT_TRUE(world != nullptr);
 
   // Verify physics engine type
   physics::PhysicsEnginePtr physics = world->Physics();
-  ASSERT_TRUE(physics != NULL);
+  ASSERT_TRUE(physics != nullptr);
 
   physics::ContactManager *manager = physics->GetContactManager();
-  ASSERT_TRUE(physics != NULL);
+  ASSERT_TRUE(physics != nullptr);
 
   EXPECT_EQ(manager->GetFilterCount(), 0u);
 
@@ -76,14 +76,14 @@ TEST_F(ContactManagerTest, RemoveFilter)
 
   // Get a pointer to the world, make sure world loads
   physics::WorldPtr world = physics::get_world("default");
-  ASSERT_TRUE(world != NULL);
+  ASSERT_TRUE(world != nullptr);
 
   // Verify physics engine type
   physics::PhysicsEnginePtr physics = world->Physics();
-  ASSERT_TRUE(physics != NULL);
+  ASSERT_TRUE(physics != nullptr);
 
   physics::ContactManager *manager = physics->GetContactManager();
-  ASSERT_TRUE(physics != NULL);
+  ASSERT_TRUE(physics != nullptr);
 
   // Add one filter then remove it
   std::map<std::string, physics::CollisionPtr> collisionMap;
@@ -107,7 +107,7 @@ TEST_F(ContactManagerTest, RemoveFilter)
     ss << name << i;
     std::map<std::string, physics::CollisionPtr> collisions;
     collisionMap["collision"] = physics::CollisionPtr();
-    ASSERT_TRUE(collisionMap["collision"] == NULL);
+    ASSERT_TRUE(collisionMap["collision"] == nullptr);
 
     manager->CreateFilter(ss.str(), collisions);
     EXPECT_TRUE(manager->HasFilter(ss.str()));
@@ -136,6 +136,64 @@ TEST_F(ContactManagerTest, RemoveFilter)
   EXPECT_EQ(manager->GetFilterCount(), 0u);
 }
 
+/////////////////////////////////////////////////
+TEST_F(ContactManagerTest, NeverDropContacts)
+{
+  // world needs to be paused in order to use World::Step()
+  // function correctly (second parameter true)
+  Load("test/worlds/box.world", true);
+
+  // Get a pointer to the world, make sure world loads
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_TRUE(world != nullptr);
+
+  // Verify physics engine type
+  physics::PhysicsEnginePtr physics = world->Physics();
+  ASSERT_TRUE(physics != nullptr);
+
+  physics::ContactManager *manager = physics->GetContactManager();
+  ASSERT_TRUE(manager != nullptr);
+
+  // advance the world. Contacts should happen between
+  // box and ground.
+  world->Step(1);
+
+  unsigned int numContacts = manager->GetContactCount();
+
+  // we have not enforced contacts computation (yet), so
+  // we should not have access to any contacts information
+  // without the enforcement.
+  ASSERT_EQ(numContacts, 0u);
+
+  manager->SetNeverDropContacts(true);
+  ASSERT_TRUE(manager->NeverDropContacts());
+
+  // advance the world again, this time the contacts
+  // information should become available.
+  world->Step(1);
+
+  numContacts = manager->GetContactCount();
+  gzdbg << "Number of contacts: " <<numContacts << std::endl;
+  ASSERT_GT(numContacts, 0u);
+
+  // make sure there are no subscribers connected which may have
+  // caused the contacts to become true
+  const std::vector<physics::Contact *>& contacts = manager->GetContacts();
+  for (std::vector<physics::Contact *>::const_iterator it = contacts.begin();
+       it != contacts.end(); ++it)
+  {
+    physics::Contact* contact = *it;
+    physics::Collision* coll1 = contact->collision1;
+    physics::Collision* coll2 = contact->collision2;
+    ASSERT_TRUE(coll1 != nullptr);
+    ASSERT_TRUE(coll2 != nullptr);
+    // we have no subscribers connected and still have gotten the contacts
+    // information, which means that enforcing contacts computation has worked.
+    ASSERT_FALSE(manager->SubscribersConnected(coll1, coll2));
+    ASSERT_FALSE(manager->SubscribersConnected(coll2, coll1));
+  }
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);