From a63f910ddc3e80ee62eda4e9fe92f80218962c40 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Wed, 18 Oct 2017 15:35:45 -0700 Subject: [PATCH 1/9] Implement aikido kinematic World for planning. --- include/aikido/planner.hpp | 1 + include/aikido/planner/World.hpp | 65 +++++++++++++++ src/planner/CMakeLists.txt | 4 +- src/planner/World.cpp | 134 +++++++++++++++++++++++++++++++ 4 files changed, 203 insertions(+), 1 deletion(-) create mode 100644 include/aikido/planner/World.hpp create mode 100644 src/planner/World.cpp diff --git a/include/aikido/planner.hpp b/include/aikido/planner.hpp index 33e1f9b715..dba843cbe3 100644 --- a/include/aikido/planner.hpp +++ b/include/aikido/planner.hpp @@ -1,5 +1,6 @@ #include "planner/PlanningResult.hpp" #include "planner/SnapPlanner.hpp" +#include "planner/World.hpp" #include "planner/ompl/BackwardCompatibility.hpp" #include "planner/ompl/CRRT.hpp" #include "planner/ompl/CRRTConnect.hpp" diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp new file mode 100644 index 0000000000..342f2522d4 --- /dev/null +++ b/include/aikido/planner/World.hpp @@ -0,0 +1,65 @@ +#ifndef AIKIDO_PLANNER_WORLD_HPP_ +#define AIKIDO_PLANNER_WORLD_HPP_ + +#include +#include + +namespace aikido { +namespace planner { + +class World +{ +public: + /// Construct a kinematic World. + World(const std::string& name); + + virtual ~World() = default; + + /// Create a clone of this World. All Skeletons will be copied over. + std::shared_ptr clone(const std::string& name) const; + + /// Create a clone of this World with the same name. + std::shared_ptr clone() const; + + /// Set the name of this World + const std::string& setName(const std::string& newName); + + /// Get the name of this World + const std::string& getName() const; + + /// Find a Skeleton by index + dart::dynamics::SkeletonPtr getSkeleton(std::size_t i) const; + + /// Find a Skeleton by name + dart::dynamics::SkeletonPtr getSkeleton(const std::string& name) const; + + /// Get the number of Skeletons + std::size_t getNumSkeletons() const; + + /// Add a Skeleton to this World + std::string addSkeleton(const dart::dynamics::SkeletonPtr& skeleton); + + /// Remove a Skeleton from this World + void removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton); + + // TODO: Add methods for registering callbacks? + + // Get the mutex that protects the state of this World. + std::mutex& getMutex() const; + +protected: + /// Name of this World + std::string mName; + + /// Skeletons in this World + std::vector mSkeletons; + + mutable std::mutex mMutex; +}; + +typedef std::shared_ptr WorldPtr; + +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_WORLD_HPP_ diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 4bf374f385..1a771f312f 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -1,4 +1,6 @@ -set(sources SnapPlanner.cpp) +set(sources + SnapPlanner.cpp + World.cpp) add_library("${PROJECT_NAME}_planner" SHARED ${sources}) target_link_libraries("${PROJECT_NAME}_planner" diff --git a/src/planner/World.cpp b/src/planner/World.cpp new file mode 100644 index 0000000000..fa5dac2a7f --- /dev/null +++ b/src/planner/World.cpp @@ -0,0 +1,134 @@ +#include + +namespace aikido { +namespace planner { + +//============================================================================== +World::World(const std::string& name) : mName(name) +{ + // Do nothing +} + +//============================================================================== +WorldPtr World::clone(const std::string& newName) const +{ + WorldPtr worldClone(new World(newName)); + + // Clone and add each Skeleton + worldClone->mSkeletons.reserve(mSkeletons.size()); + for (std::size_t i = 0; i < mSkeletons.size(); ++i) + { + const auto clonedSkeleton = mSkeletons[i]->clone(); + clonedSkeleton->setConfiguration(mSkeletons[i]->getConfiguration()); + worldClone->addSkeleton(std::move(clonedSkeleton)); + } + + return worldClone; +} + +//============================================================================== +WorldPtr World::clone() const +{ + return clone(mName); +} + +//============================================================================== +const std::string& World::setName(const std::string& newName) +{ + mName = newName; + return mName; +} + +//============================================================================== +const std::string& World::getName() const +{ + return mName; +} + +//============================================================================== +dart::dynamics::SkeletonPtr World::getSkeleton(std::size_t i) const +{ + if (i < mSkeletons.size()) + return mSkeletons[i]; + + return nullptr; +} + +//============================================================================== +dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const +{ + for (const auto& skeleton : mSkeletons) + if (skeleton->getName() == name) + return skeleton; + + return nullptr; +} + +//============================================================================== +std::size_t World::getNumSkeletons() const +{ + return mSkeletons.size(); +} + +//============================================================================== +std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton) +{ + std::lock_guard lock(mMutex); + + if (!skeleton) + { + std::cout << "[World::addSkeleton] Attempting to add a nullptr Skeleton to " + << "the world!" << std::endl; + return ""; + } + + // If mSkeletons already has skeleton, then do nothing. + if (std::find(mSkeletons.begin(), mSkeletons.end(), skeleton) + != mSkeletons.end()) + { + std::cout << "[World::addSkeleton] Skeleton named [" << skeleton->getName() + << "] is already in the world." << std::endl; + return skeleton->getName(); + } + + mSkeletons.push_back(skeleton); + + // TODO: handle name conflicts + // dart::simulation::World uses dart::common::NameManager + + return skeleton->getName(); +} + +//============================================================================== +void World::removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton) +{ + std::lock_guard lock(mMutex); + + if (!skeleton) + { + std::cout << "[World::removeSkeleton] Attempting to remove a nullptr " + << "Skeleton from the world!" << std::endl; + return; + } + + // If mSkeletons doesn't have skeleton, then do nothing. + auto skelIt = std::find(mSkeletons.begin(), mSkeletons.end(), skeleton); + if (skelIt == mSkeletons.end()) + { + std::cout << "[World::removeSkeleton] Skeleton [" << skeleton->getName() + << "] is not in the world." << std::endl; + return; + } + + // Remove skeleton from mSkeletons + mSkeletons.erase(skelIt); +} + +//============================================================================== +std::mutex& World::getMutex() const +{ + return mMutex; +} + +} // namespace planner +} // namespace aikido From 79f6f9e722e33421779324f0e295ac8318531420 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Wed, 18 Oct 2017 16:24:04 -0700 Subject: [PATCH 2/9] Address Jeongseok's comments. --- include/aikido/planner/World.hpp | 3 ++- src/planner/World.cpp | 10 ++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index 342f2522d4..a32e34339e 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -16,7 +16,8 @@ class World virtual ~World() = default; /// Create a clone of this World. All Skeletons will be copied over. - std::shared_ptr clone(const std::string& name) const; + /// \param newName Name for the cloned World + std::shared_ptr clone(const std::string& newName) const; /// Create a clone of this World with the same name. std::shared_ptr clone() const; diff --git a/src/planner/World.cpp b/src/planner/World.cpp index fa5dac2a7f..04c0f8b560 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -58,8 +58,10 @@ dart::dynamics::SkeletonPtr World::getSkeleton(std::size_t i) const dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const { for (const auto& skeleton : mSkeletons) + { if (skeleton->getName() == name) return skeleton; + } return nullptr; } @@ -73,8 +75,6 @@ std::size_t World::getNumSkeletons() const //============================================================================== std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton) { - std::lock_guard lock(mMutex); - if (!skeleton) { std::cout << "[World::addSkeleton] Attempting to add a nullptr Skeleton to " @@ -82,6 +82,8 @@ std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton) return ""; } + std::lock_guard lock(mMutex); + // If mSkeletons already has skeleton, then do nothing. if (std::find(mSkeletons.begin(), mSkeletons.end(), skeleton) != mSkeletons.end()) @@ -102,8 +104,6 @@ std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton) //============================================================================== void World::removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton) { - std::lock_guard lock(mMutex); - if (!skeleton) { std::cout << "[World::removeSkeleton] Attempting to remove a nullptr " @@ -111,6 +111,8 @@ void World::removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton) return; } + std::lock_guard lock(mMutex); + // If mSkeletons doesn't have skeleton, then do nothing. auto skelIt = std::find(mSkeletons.begin(), mSkeletons.end(), skeleton); if (skelIt == mSkeletons.end()) From f3609278068714c07fc3bcfb0cbafa4cbf19c309 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Wed, 18 Oct 2017 16:26:57 -0700 Subject: [PATCH 3/9] Change return type of World::setName. --- include/aikido/planner/World.hpp | 2 +- src/planner/World.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index a32e34339e..c812509f70 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -23,7 +23,7 @@ class World std::shared_ptr clone() const; /// Set the name of this World - const std::string& setName(const std::string& newName); + void setName(const std::string& newName); /// Get the name of this World const std::string& getName() const; diff --git a/src/planner/World.cpp b/src/planner/World.cpp index 04c0f8b560..283f8256a0 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -33,10 +33,9 @@ WorldPtr World::clone() const } //============================================================================== -const std::string& World::setName(const std::string& newName) +void World::setName(const std::string& newName) { mName = newName; - return mName; } //============================================================================== From 51843baf66ffd92835b63802eadf28a5b3dd5b96 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 10:50:56 -0700 Subject: [PATCH 4/9] Add tests for aikido::planner::World. --- tests/planner/CMakeLists.txt | 4 ++ tests/planner/test_World.cpp | 93 ++++++++++++++++++++++++++++++++++++ 2 files changed, 97 insertions(+) create mode 100644 tests/planner/test_World.cpp diff --git a/tests/planner/CMakeLists.txt b/tests/planner/CMakeLists.txt index f0af62f389..63e87c82c6 100644 --- a/tests/planner/CMakeLists.txt +++ b/tests/planner/CMakeLists.txt @@ -7,3 +7,7 @@ target_link_libraries(test_SnapPlanner "${PROJECT_NAME}_distance" "${PROJECT_NAME}_trajectory" "${PROJECT_NAME}_planner") + +aikido_add_test(test_World test_World.cpp) +target_link_libraries(test_World + "${PROJECT_NAME}_planner") diff --git a/tests/planner/test_World.cpp b/tests/planner/test_World.cpp new file mode 100644 index 0000000000..55748d6a85 --- /dev/null +++ b/tests/planner/test_World.cpp @@ -0,0 +1,93 @@ +#include +#include +#include + +using std::shared_ptr; +using std::make_shared; + +class WorldTest : public ::testing::Test +{ +public: + using SkeletonPtr = dart::dynamics::SkeletonPtr; + + WorldTest() + : skel1{dart::dynamics::Skeleton::create("skel1")} + , skel2{dart::dynamics::Skeleton::create("skel2")} + , skel3{dart::dynamics::Skeleton::create("skel3")} + { + mWorld = std::make_shared("test"); + } + + // DART setup + SkeletonPtr skel1, skel2, skel3; + aikido::planner::WorldPtr mWorld; +}; + +TEST_F(WorldTest, NameTest) +{ + EXPECT_EQ("test", mWorld->getName()); + mWorld->setName("different"); + EXPECT_EQ("different", mWorld->getName()); +} + +TEST_F(WorldTest, AddGetRemoveSkeletons) +{ + // Add Skeletons + EXPECT_EQ(0, mWorld->getNumSkeletons()); + mWorld->addSkeleton(skel1); + EXPECT_EQ(1, mWorld->getNumSkeletons()); + mWorld->addSkeleton(skel1); + EXPECT_EQ(1, mWorld->getNumSkeletons()); + mWorld->addSkeleton(skel2); + EXPECT_EQ(2, mWorld->getNumSkeletons()); + + // Getting Skeletons by index + EXPECT_EQ(skel1, mWorld->getSkeleton(0)); + EXPECT_EQ(skel2, mWorld->getSkeleton(1)); + EXPECT_TRUE(mWorld->getSkeleton(2) == nullptr); + + // Getting Skeletons by name + EXPECT_EQ(skel1, mWorld->getSkeleton("skel1")); + EXPECT_EQ(skel2, mWorld->getSkeleton("skel2")); + EXPECT_TRUE(mWorld->getSkeleton("skel3") == nullptr); + + // Remove Skeletons + mWorld->removeSkeleton(skel3); + EXPECT_EQ(2, mWorld->getNumSkeletons()); + mWorld->removeSkeleton(skel2); + EXPECT_EQ(1, mWorld->getNumSkeletons()); + mWorld->removeSkeleton(skel2); + EXPECT_EQ(1, mWorld->getNumSkeletons()); + mWorld->removeSkeleton(skel1); + EXPECT_EQ(0, mWorld->getNumSkeletons()); +} + +TEST_F(WorldTest, CloningPreservesSkeletonNamesAndConfigurations) +{ + mWorld->addSkeleton(skel1); + mWorld->addSkeleton(skel2); + mWorld->addSkeleton(skel3); + + auto clone1 = mWorld->clone(); + EXPECT_EQ(mWorld->getName(), clone1->getName()); + + auto clone2 = clone1->clone("clone2"); + EXPECT_EQ("clone2", clone2->getName()); + + EXPECT_EQ(mWorld->getNumSkeletons(), clone1->getNumSkeletons()); + EXPECT_EQ(mWorld->getNumSkeletons(), clone2->getNumSkeletons()); + for (std::size_t i = 0; i < mWorld->getNumSkeletons(); ++i) + { + auto origSkeleton = mWorld->getSkeleton(i); + auto clonedSkeleton1 = clone1->getSkeleton(i); + auto clonedSkeleton2 = clone2->getSkeleton(i); + + EXPECT_EQ(origSkeleton->getName(), clonedSkeleton1->getName()); + EXPECT_EQ(origSkeleton->getName(), clonedSkeleton2->getName()); + + EXPECT_EQ( + origSkeleton->getConfiguration(), clonedSkeleton1->getConfiguration()); + EXPECT_EQ( + origSkeleton->getConfiguration(), clonedSkeleton2->getConfiguration()); + } +} From 346ee260aa6e03773f8d5d0460a25e767bda898c Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 10:51:09 -0700 Subject: [PATCH 5/9] Updated CHANGELOG.md. --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 340d4db983..24ca6cf0e9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### 0.2.0 (201X-XX-XX) +* Planner + + * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243) + * Build & Testing & ETC * Changed to use size_t over std::size_t: [#230](https://github.com/personalrobotics/aikido/pull/230) From d99016e9cd671d34d73e50c578d5c1bd85ddecd4 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 13:02:00 -0700 Subject: [PATCH 6/9] Add World::create convenience method. --- include/aikido/planner/World.hpp | 3 +++ src/planner/World.cpp | 7 +++++++ tests/planner/test_World.cpp | 2 +- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index c812509f70..677b478c15 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -22,6 +22,9 @@ class World /// Create a clone of this World with the same name. std::shared_ptr clone() const; + /// Create a new World inside of a shared_ptr + static std::shared_ptr create(const std::string& name); + /// Set the name of this World void setName(const std::string& newName); diff --git a/src/planner/World.cpp b/src/planner/World.cpp index 283f8256a0..c28b9ac9de 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -32,6 +32,13 @@ WorldPtr World::clone() const return clone(mName); } +//============================================================================== +WorldPtr World::create(const std::string& name) +{ + WorldPtr world(new World(name)); + return world; +} + //============================================================================== void World::setName(const std::string& newName) { diff --git a/tests/planner/test_World.cpp b/tests/planner/test_World.cpp index 55748d6a85..645465aecd 100644 --- a/tests/planner/test_World.cpp +++ b/tests/planner/test_World.cpp @@ -14,8 +14,8 @@ class WorldTest : public ::testing::Test : skel1{dart::dynamics::Skeleton::create("skel1")} , skel2{dart::dynamics::Skeleton::create("skel2")} , skel3{dart::dynamics::Skeleton::create("skel3")} + , mWorld{aikido::planner::World::create("test")} { - mWorld = std::make_shared("test"); } // DART setup From 67749bb7c4cce9c4e58008109e75476cc4215f31 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 16:27:26 -0700 Subject: [PATCH 7/9] Use DART NameManager to automatically generate World IDs. --- include/aikido/planner/World.hpp | 19 ++++++++------ src/planner/World.cpp | 45 ++++++++++++++++++-------------- tests/planner/test_World.cpp | 2 +- 3 files changed, 37 insertions(+), 29 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index 677b478c15..b14fbed795 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -11,22 +11,19 @@ class World { public: /// Construct a kinematic World. - World(const std::string& name); + World(const std::string& name = ""); - virtual ~World() = default; + virtual ~World(); /// Create a clone of this World. All Skeletons will be copied over. /// \param newName Name for the cloned World - std::shared_ptr clone(const std::string& newName) const; - - /// Create a clone of this World with the same name. - std::shared_ptr clone() const; + std::shared_ptr clone(const std::string& newName = "") const; /// Create a new World inside of a shared_ptr - static std::shared_ptr create(const std::string& name); + static std::shared_ptr create(const std::string& name = ""); /// Set the name of this World - void setName(const std::string& newName); + std::string setName(const std::string& newName); /// Get the name of this World const std::string& getName() const; @@ -59,6 +56,12 @@ class World std::vector mSkeletons; mutable std::mutex mMutex; + + /// NameManager for keeping track of Worlds + static dart::common::NameManager worldNameManager; + + /// NameManager for keeping track of Skeletons + dart::common::NameManager mSkeletonNameManager; }; typedef std::shared_ptr WorldPtr; diff --git a/src/planner/World.cpp b/src/planner/World.cpp index c28b9ac9de..16b7aeb2db 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -3,16 +3,27 @@ namespace aikido { namespace planner { +dart::common::NameManager World::worldNameManager{"World", "world"}; + +//============================================================================== +World::World(const std::string& name) +{ + setName(name); + + mSkeletonNameManager.setManagerName("World::Skeleton | " + mName); + mSkeletonNameManager.setDefaultName("skeleton"); +} + //============================================================================== -World::World(const std::string& name) : mName(name) +World::~World() { - // Do nothing + World::worldNameManager.removeName(mName); } //============================================================================== WorldPtr World::clone(const std::string& newName) const { - WorldPtr worldClone(new World(newName)); + WorldPtr worldClone(new World(newName.empty() ? mName : newName)); // Clone and add each Skeleton worldClone->mSkeletons.reserve(mSkeletons.size()); @@ -26,12 +37,6 @@ WorldPtr World::clone(const std::string& newName) const return worldClone; } -//============================================================================== -WorldPtr World::clone() const -{ - return clone(mName); -} - //============================================================================== WorldPtr World::create(const std::string& name) { @@ -40,9 +45,13 @@ WorldPtr World::create(const std::string& name) } //============================================================================== -void World::setName(const std::string& newName) +std::string World::setName(const std::string& newName) { - mName = newName; + if (mName.empty()) + mName = World::worldNameManager.issueNewNameAndAdd(newName, this); + else + mName = World::worldNameManager.changeObjectName(this, newName); + return mName; } //============================================================================== @@ -63,13 +72,7 @@ dart::dynamics::SkeletonPtr World::getSkeleton(std::size_t i) const //============================================================================== dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const { - for (const auto& skeleton : mSkeletons) - { - if (skeleton->getName() == name) - return skeleton; - } - - return nullptr; + return mSkeletonNameManager.getObject(name); } //============================================================================== @@ -101,8 +104,8 @@ std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton) mSkeletons.push_back(skeleton); - // TODO: handle name conflicts - // dart::simulation::World uses dart::common::NameManager + skeleton->setName( + mSkeletonNameManager.issueNewNameAndAdd(skeleton->getName(), skeleton)); return skeleton->getName(); } @@ -130,6 +133,8 @@ void World::removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton) // Remove skeleton from mSkeletons mSkeletons.erase(skelIt); + + mSkeletonNameManager.removeName(skeleton->getName()); } //============================================================================== diff --git a/tests/planner/test_World.cpp b/tests/planner/test_World.cpp index 645465aecd..856bf14187 100644 --- a/tests/planner/test_World.cpp +++ b/tests/planner/test_World.cpp @@ -69,7 +69,7 @@ TEST_F(WorldTest, CloningPreservesSkeletonNamesAndConfigurations) mWorld->addSkeleton(skel3); auto clone1 = mWorld->clone(); - EXPECT_EQ(mWorld->getName(), clone1->getName()); + EXPECT_NE(mWorld->getName(), clone1->getName()); auto clone2 = clone1->clone("clone2"); EXPECT_EQ("clone2", clone2->getName()); From a7be41efde91a0b0ce693d93e7470e4dd04563af Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 16:56:44 -0700 Subject: [PATCH 8/9] Clean up documentation. --- include/aikido/planner/World.hpp | 16 ++++++++++++---- src/planner/World.cpp | 14 +++++++------- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index b14fbed795..a2d6942675 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -11,41 +11,48 @@ class World { public: /// Construct a kinematic World. + /// \param name Name for the new World World(const std::string& name = ""); virtual ~World(); + /// Create a new World inside of a shared_ptr + /// \param name Name for the new World + static std::shared_ptr create(const std::string& name = ""); + /// Create a clone of this World. All Skeletons will be copied over. /// \param newName Name for the cloned World std::shared_ptr clone(const std::string& newName = "") const; - /// Create a new World inside of a shared_ptr - static std::shared_ptr create(const std::string& name = ""); - /// Set the name of this World + /// \param newName New name for this World std::string setName(const std::string& newName); /// Get the name of this World const std::string& getName() const; /// Find a Skeleton by index + /// \param i Index of desired Skeleton dart::dynamics::SkeletonPtr getSkeleton(std::size_t i) const; /// Find a Skeleton by name + /// \param name Name of desired Skeleton dart::dynamics::SkeletonPtr getSkeleton(const std::string& name) const; /// Get the number of Skeletons std::size_t getNumSkeletons() const; /// Add a Skeleton to this World + /// \param skeleton Skeleton to add to the World std::string addSkeleton(const dart::dynamics::SkeletonPtr& skeleton); /// Remove a Skeleton from this World + /// \param skeleton Skeleton to remove from the World void removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton); // TODO: Add methods for registering callbacks? - // Get the mutex that protects the state of this World. + /// Get the mutex that protects the state of this World. std::mutex& getMutex() const; protected: @@ -55,6 +62,7 @@ class World /// Skeletons in this World std::vector mSkeletons; + /// Mutex to protect this World mutable std::mutex mMutex; /// NameManager for keeping track of Worlds diff --git a/src/planner/World.cpp b/src/planner/World.cpp index 16b7aeb2db..cbd63fb9ec 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -20,6 +20,13 @@ World::~World() World::worldNameManager.removeName(mName); } +//============================================================================== +WorldPtr World::create(const std::string& name) +{ + WorldPtr world(new World(name)); + return world; +} + //============================================================================== WorldPtr World::clone(const std::string& newName) const { @@ -37,13 +44,6 @@ WorldPtr World::clone(const std::string& newName) const return worldClone; } -//============================================================================== -WorldPtr World::create(const std::string& name) -{ - WorldPtr world(new World(name)); - return world; -} - //============================================================================== std::string World::setName(const std::string& newName) { From b7c14c1639e3f9148848f344abeb6e4065f1a9f0 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Oct 2017 17:00:50 -0700 Subject: [PATCH 9/9] Address Jeongseok's comments. --- include/aikido/planner/World.hpp | 8 ++++---- src/planner/World.cpp | 16 ++++++++-------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp index a2d6942675..71c2d30a48 100644 --- a/include/aikido/planner/World.hpp +++ b/include/aikido/planner/World.hpp @@ -18,11 +18,11 @@ class World /// Create a new World inside of a shared_ptr /// \param name Name for the new World - static std::shared_ptr create(const std::string& name = ""); + static std::unique_ptr create(const std::string& name = ""); /// Create a clone of this World. All Skeletons will be copied over. /// \param newName Name for the cloned World - std::shared_ptr clone(const std::string& newName = "") const; + std::unique_ptr clone(const std::string& newName = "") const; /// Set the name of this World /// \param newName New name for this World @@ -66,13 +66,13 @@ class World mutable std::mutex mMutex; /// NameManager for keeping track of Worlds - static dart::common::NameManager worldNameManager; + static dart::common::NameManager mWorldNameManager; /// NameManager for keeping track of Skeletons dart::common::NameManager mSkeletonNameManager; }; -typedef std::shared_ptr WorldPtr; +using WorldPtr = std::shared_ptr; } // namespace planner } // namespace aikido diff --git a/src/planner/World.cpp b/src/planner/World.cpp index cbd63fb9ec..7a8e3db385 100644 --- a/src/planner/World.cpp +++ b/src/planner/World.cpp @@ -3,7 +3,7 @@ namespace aikido { namespace planner { -dart::common::NameManager World::worldNameManager{"World", "world"}; +dart::common::NameManager World::mWorldNameManager{"World", "world"}; //============================================================================== World::World(const std::string& name) @@ -17,20 +17,20 @@ World::World(const std::string& name) //============================================================================== World::~World() { - World::worldNameManager.removeName(mName); + World::mWorldNameManager.removeName(mName); } //============================================================================== -WorldPtr World::create(const std::string& name) +std::unique_ptr World::create(const std::string& name) { - WorldPtr world(new World(name)); + std::unique_ptr world(new World(name)); return world; } //============================================================================== -WorldPtr World::clone(const std::string& newName) const +std::unique_ptr World::clone(const std::string& newName) const { - WorldPtr worldClone(new World(newName.empty() ? mName : newName)); + std::unique_ptr worldClone(new World(newName.empty() ? mName : newName)); // Clone and add each Skeleton worldClone->mSkeletons.reserve(mSkeletons.size()); @@ -48,9 +48,9 @@ WorldPtr World::clone(const std::string& newName) const std::string World::setName(const std::string& newName) { if (mName.empty()) - mName = World::worldNameManager.issueNewNameAndAdd(newName, this); + mName = World::mWorldNameManager.issueNewNameAndAdd(newName, this); else - mName = World::worldNameManager.changeObjectName(this, newName); + mName = World::mWorldNameManager.changeObjectName(this, newName); return mName; }