From 19090d105bf6f09b3806ab8f280b2550e2611c53 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 30 Oct 2017 02:57:15 -0700 Subject: [PATCH 1/4] 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. --- include/aikido/planner/World.hpp | 5 +++++ include/aikido/rviz/WorldInteractiveMarkerViewer.hpp | 4 ++-- src/planner/World.cpp | 10 ++++++++++ src/rviz/WorldInteractiveMarkerViewer.cpp | 2 +- 4 files changed, 18 insertions(+), 3 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index 71c2d30a48..147df7aba8 100644 --- 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; + /// Find a Skeleton by Skeleton, returning null if it is not in the World + /// \param skel Desired Skeleton + dart::dynamics::SkeletonPtr getSkeleton( + 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 index c01ab14039..97ffd81efb 100644 --- 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 mSkeletonMarkers; + /// Mapping of Skeletons to SkeletonMarkers + std::map mSkeletonMarkers; /// World that automatically updates the viewer aikido::planner::WorldPtr mWorld; diff --git a/src/planner/World.cpp b/src/planner/World.cpp index 71ae145f3b..2b03502718 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -76,6 +76,16 @@ dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const return mSkeletonNameManager.getObject(name); } +//============================================================================== +dart::dynamics::SkeletonPtr World::getSkeleton( + const dart::dynamics::SkeletonPtr skel) const +{ + auto it = std::find(mSkeletons.begin(), mSkeletons.end(), skel); + if (it == mSkeletons.end()) + return nullptr; + return skel; +} + //============================================================================== std::size_t World::getNumSkeletons() const { diff --git a/src/rviz/WorldInteractiveMarkerViewer.cpp b/src/rviz/WorldInteractiveMarkerViewer.cpp index 004033bfe2..743e492937 100644 --- 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 lock(skeleton->getMutex(), std::try_to_lock); if (lock.owns_lock()) From f081e7aad5ce6252d111c6047cfd2fc601acec0e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 17 Nov 2017 15:12:43 -0800 Subject: [PATCH 2/4] Rename getSkeleton(skeleton) to hasSkeleton(skeleton) --- include/aikido/planner/World.hpp | 6 +++--- src/planner/World.cpp | 9 +++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index 147df7aba8..a76cf74ae3 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -39,10 +39,10 @@ class World /// \param name Name of desired Skeleton dart::dynamics::SkeletonPtr getSkeleton(const std::string& name) const; - /// Find a Skeleton by Skeleton, returning null if it is not in the World + /// Returns true if the Skeleton is in this World. /// \param skel Desired Skeleton - dart::dynamics::SkeletonPtr getSkeleton( - const dart::dynamics::SkeletonPtr skel) const; + /// \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/src/planner/World.cpp b/src/planner/World.cpp index 2b03502718..22c6a03a66 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -77,13 +77,10 @@ dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const } //============================================================================== -dart::dynamics::SkeletonPtr World::getSkeleton( - const dart::dynamics::SkeletonPtr skel) const +bool World::hasSkeleton(const dart::dynamics::SkeletonPtr& skel) const { - auto it = std::find(mSkeletons.begin(), mSkeletons.end(), skel); - if (it == mSkeletons.end()) - return nullptr; - return skel; + return std::find(mSkeletons.begin(), mSkeletons.end(), skel) + != mSkeletons.end(); } //============================================================================== From e4cfd04f8dac18335b94d792119ba48090ebd806 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 17 Nov 2017 15:15:50 -0800 Subject: [PATCH 3/4] Update changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2a02d27f57..0e286207ae 100644 --- 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 From ad067b2f9728fe8c9373515856e3341f2828a6cd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 17 Nov 2017 15:19:18 -0800 Subject: [PATCH 4/4] Use hasSkeleton() instead of getSkeleton() --- src/rviz/WorldInteractiveMarkerViewer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/WorldInteractiveMarkerViewer.cpp b/src/rviz/WorldInteractiveMarkerViewer.cpp index 743e492937..a65dfa3617 100644 --- a/src/rviz/WorldInteractiveMarkerViewer.cpp +++ b/src/rviz/WorldInteractiveMarkerViewer.cpp @@ -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; }