Fix const correctness of StateHandle

CHANGELOG.md

@@ -24,6 +24,7 @@
   * Refactored JointStateSpace and MetaSkeletonStateSpace: [#278](https://github.com/personalrobotics/aikido/pull/278)
   * Added methods for checking compatibility between DART objects and state spaces: [#315](https://github.com/personalrobotics/aikido/pull/315)
   * Added flags to MetaSkeletonStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
+  * Fixed const correctness of StateHandle::getState(): [#419](https://github.com/personalrobotics/aikido/pull/419)
 
 * Perception
 

include/aikido/statespace/CartesianProduct.hpp

@@ -16,15 +16,7 @@ class CartesianProduct : public std::enable_shared_from_this<CartesianProduct>,
                          public virtual StateSpace
 {
 public:
-  /// A tuple of states where the i-th state is from the i-th subspace.
-  class State : public StateSpace::State
-  {
-  protected:
-    State() = default;
-    ~State() = default;
-
-    friend class CartesianProduct;
-  };
+  class State;
 
   using StateHandle = CompoundStateHandle<State>;
   using StateHandleConst = CompoundStateHandle<const State>;
@@ -157,6 +149,17 @@ class CartesianProduct : public std::enable_shared_from_this<CartesianProduct>,
   std::size_t mSizeInBytes;
 };
 
+/// A tuple of states where the i-th state is from the i-th subspace.
+class CartesianProduct::State : public StateSpace::State
+{
+protected:
+  friend class CartesianProduct;
+
+  State() = default;
+
+  ~State() = default;
+};
+
 } // namespace statespace
 } // namespace aikido
 

include/aikido/statespace/StateHandle.hpp

@@ -1,31 +1,38 @@
 #ifndef AIKIDO_STATESPACE_STATEHANDLE_HPP_
 #define AIKIDO_STATESPACE_STATEHANDLE_HPP_
 
+#include <type_traits>
+
 namespace aikido {
 namespace statespace {
 
 /// Wrap a State with its StateSpace to provide convenient accessor methods.
 /// The template parameter \c _QualifiedState is necessary to support both
 /// \c const and non-<tt>const</tt> states.
 ///
-/// \tparam _StateSpace type of \c StateSpace this state is a member of
-/// \tparam _QualifiedState type of \c State being wrapped
+/// \tparam _StateSpace Type of \c StateSpace this state is a member of
+/// \tparam _QualifiedState Type of \c State being wrapped
 template <class _StateSpace, class _QualifiedState>
 class StateHandle
 {
 public:
   using StateSpace = _StateSpace;
-  using State = typename StateSpace::State;
   using QualifiedState = _QualifiedState;
 
+  using State = typename StateSpace::State;
+  using ConstState =
+      typename std::conditional<std::is_const<QualifiedState>::value,
+                                QualifiedState,
+                                const QualifiedState>::type;
+
   /// Constructs a nullptr handle.
   StateHandle();
 
   /// Wrap state, which must be form the provided StateSpace.
   ///
-  /// \param _space state space that created \c _state
-  /// \param _state state created by \c _space
-  StateHandle(const StateSpace* _space, QualifiedState* _state);
+  /// \param space State space that created \c state.
+  /// \param state State created by \c space.
+  StateHandle(const StateSpace* space, QualifiedState* state);
 
   StateHandle(const StateHandle&) = default;
   StateHandle(StateHandle&&) = default;
@@ -36,27 +43,42 @@ class StateHandle
   /// Implicitly convert to a \c State pointer.
   operator QualifiedState*() const;
 
-  /// Reset StateHandle to nullptr.
+  /// Resets StateHandle to nullptr.
   void reset();
 
-  /// Reset the state, which must be from the provided StateSpace.
+  /// Resets the state, which must be from the provided StateSpace.
   ///
-  /// \param _space state space that created \c _state
-  /// \param _state state created by \c _space
-  void reset(const StateSpace* _space, QualifiedState* _state);
+  /// \param space State space that created \c state.
+  /// \param state State created by \c space.
+  void reset(const StateSpace* space, QualifiedState* state);
 
-  /// Gets the State.
+  /// Returns the State. This function is enabled only if QualifiedState is a
+  /// non-const State type.
   ///
   /// \return state wrapped by this handle
-  QualifiedState* getState() const;
+  template <typename Q = QualifiedState>
+  auto getState() ->
+      typename std::enable_if<!std::is_const<Q>::value, Q*>::type;
+  // Note: We don't define non-const function for const State type because it
+  // violates const-correctness.
 
-  /// Gets the state space that created this state.
+  /// Returns the State.
   ///
-  /// \return state space created this state
+  /// \return State wrapped by this handle
+  template <typename Q = QualifiedState>
+  auto getState() const ->
+      typename std::conditional<std::is_const<Q>::value, Q*, const Q*>::type;
+
+  /// Returns the state space that created this state.
+  ///
+  /// \return State space created this state
   const StateSpace* getStateSpace() const;
 
 protected:
+  /// State space of the sate that is managed by this handler.
   const StateSpace* mSpace;
+
+  /// State managed by this handler. This can be either const or non-const type.
   QualifiedState* mState;
 };
 
@@ -65,4 +87,4 @@ class StateHandle
 
 #include "detail/StateHandle-impl.hpp"
 
-#endif // ifndef AIKIDO_STATESPACE_STATEHANDLE_HPP_
+#endif // AIKIDO_STATESPACE_STATEHANDLE_HPP_

include/aikido/statespace/StateSpace.hpp

@@ -32,17 +32,7 @@ class StateSpace
 {
 public:
   /// Base class for all States.
-  class State
-  {
-  protected:
-    /// This is a base class that should only only be used in derived classes.
-    State() = default;
-
-    /// It is unsafe to call this, since it is a non-virtual destructor.  Having
-    /// any virtual function in this class, including this destructor, adds
-    /// sizeof(pointer) overhead for a vtable.
-    ~State() = default;
-  };
+  class State;
 
   using StateHandle = statespace::StateHandle<StateSpace, State>;
   using StateHandleConst = statespace::StateHandle<StateSpace, const State>;
@@ -169,6 +159,18 @@ class StateSpace
   virtual void print(const State* _state, std::ostream& _os) const = 0;
 };
 
+class StateSpace::State
+{
+protected:
+  /// This is a base class that should only only be used in derived classes.
+  State() = default;
+
+  /// It is unsafe to call this, since it is a non-virtual destructor.  Having
+  /// any virtual function in this class, including this destructor, adds
+  /// sizeof(pointer) overhead for a vtable.
+  ~State() = default;
+};
+
 } // namespace statespace
 } // namespace aikido
 

include/aikido/statespace/detail/CartesianProduct-impl.hpp

@@ -1,3 +1,5 @@
+#include "aikido/statespace/CartesianProduct.hpp"
+
 #include <sstream>
 
 namespace aikido {
@@ -20,6 +22,7 @@ class CompoundStateHandle
   /// Construct and initialize to \c nullptr.
   CompoundStateHandle()
   {
+    // Do nothing
   }
 
   /// Construct a handle for \c _state in \c _space.
@@ -29,6 +32,19 @@ class CompoundStateHandle
   CompoundStateHandle(const StateSpace* _space, State* _state)
     : statespace::StateHandle<CartesianProduct, QualifiedState>(_space, _state)
   {
+    // Do nothing
+  }
+
+  /// Gets state by subspace index.
+  ///
+  /// \tparam Space type of \c StateSpace for subspace \c _index
+  /// \param _index in the range [ 0, \c getNumSubspaces() ]
+  /// \return state at \c _index
+  template <class Space = statespace::StateSpace>
+  typename Space::State* getSubState(std::size_t _index)
+  {
+    return this->getStateSpace()->template getSubState<Space>(
+        this->getState(), _index);
   }
 
   /// Gets state by subspace index.
@@ -37,7 +53,7 @@ class CompoundStateHandle
   /// \param _index in the range [ 0, \c getNumSubspaces() ]
   /// \return state at \c _index
   template <class Space = statespace::StateSpace>
-  typename Space::State* getSubState(std::size_t _index) const
+  const typename Space::State* getSubState(std::size_t _index) const
   {
     return this->getStateSpace()->template getSubState<Space>(
         this->getState(), _index);
@@ -50,12 +66,26 @@ class CompoundStateHandle
   /// \param _index in the range [ 0, \c getNumSubspaces() ]
   /// \return state at \c _index
   template <class Space = statespace::StateSpace>
-  typename Space::StateHandle getSubStateHandle(std::size_t _index) const
+  typename Space::StateHandle getSubStateHandle(std::size_t _index)
   {
     return typename Space::StateHandle(
         this->getStateSpace()->template getSubspace<Space>(_index).get(),
         getSubState<Space>(_index));
   }
+
+  /// Gets state by subspace index and wraps it in a \c Space::StateHandle
+  /// helper class.
+  ///
+  /// \tparam Space type of \c StateSpace for subspace \c _index
+  /// \param _index in the range [ 0, \c getNumSubspaces() ]
+  /// \return state at \c _index
+  template <class Space = statespace::StateSpace>
+  typename Space::StateHandleConst getSubStateHandle(std::size_t _index) const
+  {
+    return typename Space::StateHandleConst(
+        this->getStateSpace()->template getSubspace<Space>(_index).get(),
+        getSubState<Space>(_index));
+  }
 };
 
 //==============================================================================

include/aikido/statespace/detail/SE2-impl.hpp

@@ -38,7 +38,7 @@ class SE2StateHandle : public statespace::StateHandle<SE2, _QualifiedState>
   /// Sets value to an Eigen transfomation object.
   ///
   /// \param _transform Eigen transformation
-  void setIsometry(const Eigen::Isometry2d& _transform) const
+  void setIsometry(const Eigen::Isometry2d& _transform)
   {
     return this->getStateSpace()->setIsometry(this->getState(), _transform);
   }

include/aikido/statespace/detail/SE3-impl.hpp

@@ -38,7 +38,7 @@ class SE3StateHandle : public statespace::StateHandle<SE3, _QualifiedState>
   /// Gets value as an Eigen transformation object.
   ///
   /// \return Eigen trasnformation
-  void setIsometry(const Eigen::Isometry3d& _transform) const
+  void setIsometry(const Eigen::Isometry3d& _transform)
   {
     return this->getStateSpace()->setIsometry(this->getState(), _transform);
   }

include/aikido/statespace/detail/ScopedState-impl.hpp

@@ -1,17 +1,21 @@
+#include "aikido/statespace/ScopedState.hpp"
+
 namespace aikido {
 namespace statespace {
 
-template <class _Handle>
-ScopedState<_Handle>::ScopedState(const StateSpace* _space)
+//==============================================================================
+template <class Handle>
+ScopedState<Handle>::ScopedState(const StateSpace* _space)
 {
   this->mSpace = _space;
   mBuffer.reset(new char[_space->getStateSizeInBytes()]);
   this->mState = static_cast<ScopedState::State*>(
       _space->allocateStateInBuffer(mBuffer.get()));
 }
 
-template <class _Handle>
-ScopedState<_Handle>::~ScopedState()
+//==============================================================================
+template <class Handle>
+ScopedState<Handle>::~ScopedState()
 {
   this->mSpace->freeStateInBuffer(this->mState);
 }

include/aikido/statespace/detail/StateHandle-impl.hpp

@@ -1,49 +1,62 @@
+#include "aikido/statespace/StateHandle.hpp"
+
 namespace aikido {
 namespace statespace {
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-StateHandle<_StateSpace, _QualifiedState>::StateHandle()
-  : mSpace(nullptr), mState(nullptr)
+template <class StateSpace, class QualifiedState>
+StateHandle<StateSpace, QualifiedState>::StateHandle()
+  : StateHandle(nullptr, nullptr)
 {
+  // Do nothing
 }
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-StateHandle<_StateSpace, _QualifiedState>::StateHandle(
-    const StateSpace* _space, QualifiedState* _state)
-  : mSpace(_space), mState(_state)
+template <class StateSpace, class QualifiedState>
+StateHandle<StateSpace, QualifiedState>::StateHandle(
+    const StateSpace* space, QualifiedState* state)
+  : mSpace(space), mState(state)
 {
+  // Do nothing
 }
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-StateHandle<_StateSpace, _QualifiedState>::operator QualifiedState*() const
+template <class StateSpace, class QualifiedState>
+StateHandle<StateSpace, QualifiedState>::operator QualifiedState*() const
 {
   return mState;
 }
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-void StateHandle<_StateSpace, _QualifiedState>::reset()
+template <class StateSpace, class QualifiedState>
+void StateHandle<StateSpace, QualifiedState>::reset()
 {
-  mSpace = nullptr;
-  mState = nullptr;
+  reset(nullptr, nullptr);
 }
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-void StateHandle<_StateSpace, _QualifiedState>::reset(
-    const StateSpace* _space, QualifiedState* _state)
+template <class StateSpace, class QualifiedState>
+void StateHandle<StateSpace, QualifiedState>::reset(
+    const StateSpace* space, QualifiedState* state)
 {
-  mSpace = _space;
-  mState = _state;
+  mSpace = space;
+  mState = state;
 }
 
 //==============================================================================
-template <class _StateSpace, class _QualifiedState>
-auto StateHandle<_StateSpace, _QualifiedState>::getState() const
-    -> QualifiedState*
+template <class StateSpace, class QualifiedState>
+template <typename Q>
+auto StateHandle<StateSpace, QualifiedState>::getState() ->
+    typename std::enable_if<!std::is_const<Q>::value, Q*>::type
+{
+  return mState;
+}
+
+//==============================================================================
+template <class StateSpace, class QualifiedState>
+template <typename Q>
+auto StateHandle<StateSpace, QualifiedState>::getState() const ->
+    typename std::conditional<std::is_const<Q>::value, Q*, const Q*>::type
 {
   return mState;
 }

src/planner/ConfigurationToConfiguration.cpp

@@ -12,8 +12,8 @@ ConfigurationToConfiguration::ConfigurationToConfiguration(
     const statespace::StateSpace::State* goalState,
     constraint::ConstTestablePtr constraint)
   : Problem(std::move(stateSpace), std::move(constraint))
-  , mStartState(startState)
-  , mGoalState(goalState)
+  , mStartState(std::move(startState))
+  , mGoalState(std::move(goalState))
 {
   // Do nothing
 }