Check that all planning problems only hold ScopedStates. (#569)

* Use scoped state in ConfigurationToConfigurations. * Use scopedStates to store goal states for ConfigurationToConfigurations. * Fix reference return in ConfigurationToConfigurations. * Respond to code review. * Run make format. * Update CHANGELOG.
personalrobotics · May 17, 2020 · 4440cd7 · 4440cd7
1 parent e3ab367
commit 4440cd7
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 9 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -60,6 +60,7 @@
 
 * Planner
 
+  * Check that all planning problems only hold ScopedStates: [#569](https://github.com/personalrobotics/aikido/pull/569)
   * Make all DART planners take MetaSkeleton, and add adapter for turning planners into DART planners: [#437](https://github.com/personalrobotics/aikido/pull/437)
   * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302), [#324](https://github.com/personalrobotics/aikido/pull/324)
   * Fixed step sequence iteration in VFP: [#303](https://github.com/personalrobotics/aikido/pull/303)

diff --git a/include/aikido/planner/ConfigurationToConfigurations.hpp b/include/aikido/planner/ConfigurationToConfigurations.hpp
@@ -18,7 +18,7 @@ namespace planner {
 class ConfigurationToConfigurations : public Problem
 {
 public:
-  using GoalStates = std::unordered_set<const statespace::StateSpace::State*>;
+  using GoalStates = std::vector<const statespace::StateSpace::State*>;
 
   /// Constructor.
   ///
@@ -47,14 +47,14 @@ class ConfigurationToConfigurations : public Problem
   std::size_t getNumGoalStates() const;
 
   /// Returns goal states.
-  const GoalStates& getGoalStates() const;
+  GoalStates getGoalStates() const;
 
 protected:
   /// Start state.
-  const statespace::StateSpace::State* mStartState;
+  statespace::StateSpace::ScopedState mStartState;
 
   /// Goal States.
-  const GoalStates mGoalStates;
+  std::vector<statespace::StateSpace::ScopedState> mGoalStates;
 };
 
 } // namespace planner

diff --git a/src/planner/ConfigurationToConfigurations.cpp b/src/planner/ConfigurationToConfigurations.cpp
@@ -12,10 +12,12 @@ ConfigurationToConfigurations::ConfigurationToConfigurations(
     const GoalStates& goalStates,
     constraint::ConstTestablePtr constraint)
   : Problem(std::move(stateSpace), std::move(constraint))
-  , mStartState(startState)
-  , mGoalStates(goalStates)
+  , mStartState(stateSpace->cloneState(startState))
 {
-  // Do nothing
+  for (const statespace::StateSpace::State* goal : goalStates)
+  {
+    mGoalStates.push_back(stateSpace->cloneState(goal));
+  }
 }
 
 //==============================================================================
@@ -45,10 +47,15 @@ std::size_t ConfigurationToConfigurations::getNumGoalStates() const
 }
 
 //==============================================================================
-const ConfigurationToConfigurations::GoalStates&
+ConfigurationToConfigurations::GoalStates
 ConfigurationToConfigurations::getGoalStates() const
 {
-  return mGoalStates;
+  std::vector<const statespace::StateSpace::State*> pointerStates;
+  for (const auto& goal : mGoalStates)
+  {
+    pointerStates.push_back(goal.getState());
+  }
+  return pointerStates;
 }
 
 } // namespace planner