Revive planSnap() not to break downstream (#414)

CHANGELOG.md

@@ -33,7 +33,7 @@
 
   * 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 VPF: [#303](https://github.com/personalrobotics/aikido/pull/303)
-  * Refactored planning API: [#314](https://github.com/personalrobotics/aikido/pull/314)
+  * Refactored planning API: [#314](https://github.com/personalrobotics/aikido/pull/314), [#414](https://github.com/personalrobotics/aikido/pull/414)
   * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
   * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)
   * Planning calls with InverseKinematicsSampleable constraints explicitly set MetaSkeleton to solve IK with: [#379](https://github.com/personalrobotics/aikido/pull/379)

include/aikido/common/deprecated.hpp

@@ -0,0 +1,56 @@
+#ifndef AIKIDO_COMMON_DEPRECATED_HPP_
+#define AIKIDO_COMMON_DEPRECATED_HPP_
+
+#include <dart/config.hpp>
+
+// NOTE: Deprecated macros are used for backward compatibility between different
+// minor versions of AIKIDO. Deprecated API can be removed in every major
+// version up.
+
+#if defined(__GNUC__) || defined(__clang__)
+#define AIKIDO_DEPRECATED(version) __attribute__((deprecated))
+#elif defined(_MSC_VER)
+#define AIKIDO_DEPRECATED(version) __declspec(deprecated)
+#else
+#define AIKIDO_DEPRECATED(version) ()
+#endif
+
+// We define two convenient macros that can be used to suppress
+// deprecated-warnings for a specific code block rather than a whole project.
+// This macros would be useful when you need to call deprecated function for
+// some reasons (e.g., for backward compatibility) but don't want warnings.
+//
+// Example code:
+//
+// deprecated_function()  // warning
+//
+// AIKIDO_SUPPRESS_DEPRECATED_BEGIN
+// deprecated_function()  // okay, no warning
+// AIKIDO_SUPPRESS_DEPRECATED_END
+//
+#if defined(__GNUC__) || defined(__GNUG__)
+
+#define AIKIDO_SUPPRESS_DEPRECATED_BEGIN                                       \
+  _Pragma("GCC diagnostic push")                                               \
+      _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+
+#define AIKIDO_SUPPRESS_DEPRECATED_END _Pragma("GCC diagnostic pop")
+
+#elif defined(__clang__)
+
+#define AIKIDO_SUPPRESS_DEPRECATED_BEGIN                                       \
+  _Pragma("clang diagnostic push")                                             \
+      _Pragma("clang diagnostic ignored \"-Wdeprecated-declarations\"")
+
+#define AIKIDO_SUPPRESS_DEPRECATED_END _Pragma("clang diagnostic pop")
+
+#else
+
+#warning "AIKIDO is being built by unsupported compiler."
+
+#define AIKIDO_SUPPRESS_DEPRECATED_BEGIN
+#define AIKIDO_SUPPRESS_DEPRECATED_END
+
+#endif
+
+#endif // AIKIDO_COMMON_DEPRECATED_HPP_

include/aikido/planner/SnapPlanner.hpp

@@ -0,0 +1,43 @@
+#ifndef AIKIDO_PLANNER_SNAPPLANNER_HPP_
+#define AIKIDO_PLANNER_SNAPPLANNER_HPP_
+
+#include <memory>
+#include "aikido/common/deprecated.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/PlanningResult.hpp"
+#include "aikido/statespace/Interpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Plan a trajectory from \c startState to \c goalState by using
+/// \c interpolator to interpolate between them. The planner returns success if
+/// the resulting trajectory satisfies \c constraint at some resolution and
+/// failure (returning \c nullptr) otherwise. The reason for the failure is
+/// stored in the \c planningResult output parameter.
+///
+/// \param stateSpace state space
+/// \param startState start state
+/// \param goalState goal state
+/// \param interpolator interpolator used to produce the output trajectory
+/// \param constraint trajectory-wide constraint that must be satisfied
+/// \param[out] planningResult information about success or failure
+/// \return trajectory or \c nullptr if planning failed
+///
+/// \deprecated Deprecated in 0.3. Please use
+/// SnapConfigurationToConfigurationPlanner instead.
+AIKIDO_DEPRECATED(0.3)
+trajectory::InterpolatedPtr planSnap(
+    const statespace::ConstStateSpacePtr& stateSpace,
+    const statespace::StateSpace::State* startState,
+    const statespace::StateSpace::State* goalState,
+    const std::shared_ptr<statespace::Interpolator>& interpolator,
+    const std::shared_ptr<constraint::Testable>& constraint,
+    planner::PlanningResult& planningResult);
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_SNAPPLANNER_HPP_

src/planner/CMakeLists.txt

@@ -12,6 +12,7 @@ set(sources
   Problem.cpp
   RankedMetaPlanner.cpp
   SnapConfigurationToConfigurationPlanner.cpp
+  SnapPlanner.cpp
   SequenceMetaPlanner.cpp
   World.cpp
   WorldStateSaver.cpp

src/planner/SnapPlanner.cpp

@@ -0,0 +1,33 @@
+#include "aikido/planner/SnapPlanner.hpp"
+
+#include "aikido/planner/ConfigurationToConfiguration.hpp"
+#include "aikido/planner/PlanningResult.hpp"
+#include "aikido/planner/SnapConfigurationToConfigurationPlanner.hpp"
+
+namespace aikido {
+namespace planner {
+
+trajectory::InterpolatedPtr planSnap(
+    const statespace::ConstStateSpacePtr& stateSpace,
+    const aikido::statespace::StateSpace::State* startState,
+    const aikido::statespace::StateSpace::State* goalState,
+    const std::shared_ptr<aikido::statespace::Interpolator>& interpolator,
+    const std::shared_ptr<aikido::constraint::Testable>& constraint,
+    aikido::planner::PlanningResult& planningResult)
+{
+  auto problem = ConfigurationToConfiguration(
+      stateSpace, startState, goalState, constraint);
+
+  auto planner = std::make_shared<SnapConfigurationToConfigurationPlanner>(
+      stateSpace, interpolator);
+
+  SnapConfigurationToConfigurationPlanner::Result result;
+  auto trj = planner->plan(problem, &result);
+  planningResult.setMessage(result.getMessage());
+
+  assert(std::dynamic_pointer_cast<trajectory::Interpolated>(trj));
+  return std::static_pointer_cast<trajectory::Interpolated>(trj);
+}
+
+} // namespace planner
+} // namespace aikido