Fix WorldInteractiveMarkerViewer bug when removing Skeletons (#252)

* Fix bug from removing Skeletons, then adding new Skeletons with the same name. Use the actual Skeleton as the key in WorldInteractiveMarkerViewer, rather than the Skeleton's name. * Rename getSkeleton(skeleton) to hasSkeleton(skeleton) * Update changelog * Use hasSkeleton() instead of getSkeleton()
personalrobotics · Nov 19, 2017 · 631e606 · 631e606
1 parent fe79417
commit 631e606
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 5 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,7 +8,7 @@
 
 * Planner
 
-  * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243)
+  * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243), [#252](https://github.com/personalrobotics/aikido/pull/252)
   * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246)
 
 * RViz

diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp
@@ -39,6 +39,11 @@ class World
   /// \param name Name of desired Skeleton
   dart::dynamics::SkeletonPtr getSkeleton(const std::string& name) const;
 
+  /// Returns true if the Skeleton is in this World.
+  /// \param skel Desired Skeleton
+  /// \return True if succeeded to find the Skeleton
+  bool hasSkeleton(const dart::dynamics::SkeletonPtr& skel) const;
+
   /// Get the number of Skeletons
   std::size_t getNumSkeletons() const;
 

diff --git a/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp b/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp
@@ -38,8 +38,8 @@ class WorldInteractiveMarkerViewer : public InteractiveMarkerViewer
   /// Thread target for auto-updating the viewer
   void autoUpdate();
 
-  /// Mapping of Skeleton names to SkeletonMarkers
-  std::map<std::string, SkeletonMarkerPtr> mSkeletonMarkers;
+  /// Mapping of Skeletons to SkeletonMarkers
+  std::map<dart::dynamics::SkeletonPtr, SkeletonMarkerPtr> mSkeletonMarkers;
 
   /// World that automatically updates the viewer
   aikido::planner::WorldPtr mWorld;

diff --git a/src/planner/World.cpp b/src/planner/World.cpp
@@ -76,6 +76,13 @@ dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const
   return mSkeletonNameManager.getObject(name);
 }
 
+//==============================================================================
+bool World::hasSkeleton(const dart::dynamics::SkeletonPtr& skel) const
+{
+  return std::find(mSkeletons.begin(), mSkeletons.end(), skel)
+         != mSkeletons.end();
+}
+
 //==============================================================================
 std::size_t World::getNumSkeletons() const
 {

diff --git a/src/rviz/WorldInteractiveMarkerViewer.cpp b/src/rviz/WorldInteractiveMarkerViewer.cpp
@@ -63,7 +63,7 @@ void WorldInteractiveMarkerViewer::update()
     {
       // Either a new SkeletonMarker or a previously-inserted SkeletonMarker
       auto result = mSkeletonMarkers.emplace(
-          skeleton->getName(), CreateSkeletonMarker(skeleton, mFrameId));
+          skeleton, CreateSkeletonMarker(skeleton, mFrameId));
 
       std::unique_lock<std::mutex> lock(skeleton->getMutex(), std::try_to_lock);
       if (lock.owns_lock())
@@ -76,7 +76,7 @@ void WorldInteractiveMarkerViewer::update()
   while (it != std::end(mSkeletonMarkers))
   {
     // Skeleton still exists in the World, do nothing.
-    if (mWorld->getSkeleton(it->first))
+    if (mWorld->hasSkeleton(it->first))
     {
       ++it;
     }