Add CrossStateOptimization for ReedsShepp

include/hpp/manipulation/steering-method/cross-state-optimization.hh

@@ -35,7 +35,11 @@ namespace hpp {
           static CrossStateOptimizationPtr_t create (const Problem& problem);
 
           /// \warning core::Problem will be casted to Problem
-          static CrossStateOptimizationPtr_t createFromCore
+          static CrossStateOptimizationPtr_t create
+            (const core::Problem& problem);
+
+          template <typename T>
+            static CrossStateOptimizationPtr_t create
             (const core::Problem& problem);
 
           core::SteeringMethodPtr_t copy () const;
@@ -77,6 +81,15 @@ namespace hpp {
           /// Weak pointer to itself
           CrossStateOptimizationWkPtr_t weak_;
       }; // class CrossStateOptimization
+
+      template <typename T>
+        CrossStateOptimizationPtr_t CrossStateOptimization::create
+        (const core::Problem& problem)
+      {
+        CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem);
+        gsm->innerSteeringMethod (T::create (problem));
+        return gsm;
+      }
     } // namespace steeringMethod
   } // namespace manipulation
 } // namespace hpp

src/problem-solver.cc

@@ -154,14 +154,20 @@ namespace hpp {
           steeringMethod::Graph::create <core::steeringMethod::Straight>);
       steeringMethods.add ("Graph-Hermite",
           steeringMethod::Graph::create <core::steeringMethod::Hermite>);
-      steeringMethods.add ("CrossStateOptimization",
-          steeringMethod::CrossStateOptimization::createFromCore);
       steeringMethods.add ("Graph-ReedsShepp",
           createSMWithGuess <steeringMethod::Graph, core::steeringMethod::ReedsShepp>);
       steeringMethods.add ("Graph-Dubins",
           createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>);
       steeringMethods.add ("Graph-Snibud",
           createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>);
+      steeringMethods.add ("CrossStateOptimization-Straight",
+          steeringMethod::CrossStateOptimization::create<core::steeringMethod::Straight>);
+      steeringMethods.add ("CrossStateOptimization-ReedsShepp",
+          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::ReedsShepp>);
+      steeringMethods.add ("CrossStateOptimization-Dubins",
+          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Dubins>);
+      steeringMethods.add ("CrossStateOptimization-Snibud",
+          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Snibud>);
 
       pathOptimizers.add ("KeypointsShortcut",
           pathOptimization::Keypoints::create);

src/steering-method/cross-state-optimization.cc

@@ -59,7 +59,7 @@ namespace hpp {
         return shPtr;
       }
 
-      CrossStateOptimizationPtr_t CrossStateOptimization::createFromCore (
+      CrossStateOptimizationPtr_t CrossStateOptimization::create (
           const core::Problem& problem)
       {
         HPP_STATIC_CAST_REF_CHECK (const Problem, problem);