reeds_shepp_state_space.hpp

Go to the documentation of this file.

/*********************************************************************
*  Copyright (c) 2017 - for information on the respective copyright
*  owner see the NOTICE file and/or the repository
*
*      https://github.com/hbanzhaf/steering_functions.git
*
*  Licensed under the Apache License, Version 2.0 (the "License");
*  you may not use this file except in compliance with the License.
*  You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
*  Unless required by applicable law or agreed to in writing, software
*  distributed under the License is distributed on an "AS IS" BASIS,
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
*  implied. See the License for the specific language governing
*  permissions and limitations under the License.

*  This source code is derived from the Open Motion Planning Library
*  (OMPL) V 1.3.1 (https://github.com/ompl/ompl).
*  Copyright (c) 2010, Rice University, licensed under the BSD license,
*  cf. 3rd-party-licenses.txt file in the root directory of this source
*  tree.
**********************************************************************/

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#ifndef __REEDS_SHEPP_STATE_SPACE_HPP_
#define __REEDS_SHEPP_STATE_SPACE_HPP_

#include <cassert>
#include <limits>
#include <vector>

#include "steering_functions/filter/ekf.hpp"
#include "steering_functions/steering_functions.hpp"

namespace steering
{

class Reeds_Shepp_State_Space
{
public:
  enum Reeds_Shepp_Path_Segment_Type
  {
    RS_NOP = 0,
    RS_LEFT = 1,
    RS_STRAIGHT = 2,
    RS_RIGHT = 3
  };

  static const Reeds_Shepp_Path_Segment_Type reeds_shepp_path_type[18][5];

  class Reeds_Shepp_Path
  {
  public:
    Reeds_Shepp_Path(const Reeds_Shepp_Path_Segment_Type *type = reeds_shepp_path_type[0],
                     double t = std::numeric_limits<double>::max(), double u = 0., double v = 0., double w = 0.,
                     double x = 0.);

    double length() const
    {
      return total_length_;
    }

  private:
    const Reeds_Shepp_Path_Segment_Type *type_;

    double length_[5];

    double total_length_;

    friend class Reeds_Shepp_State_Space;
  };

  Reeds_Shepp_State_Space(double kappa, double discretization = 0.1) : kappa_(kappa), discretization_(discretization)
  {
    assert(kappa > 0.0 && discretization > 0.0);
    kappa_inv_ = 1 / kappa;
  }

  void set_filter_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise,
                             const Controller &controller);

  Reeds_Shepp_Path reeds_shepp(const State &state1, const State &state2) const;

  double get_distance(const State &state1, const State &state2) const;

  std::vector<Control> get_controls(const State &state1, const State &state2) const;

  std::vector<State> get_path(const State &state1, const State &state2) const;

  std::vector<State_With_Covariance> get_path_with_covariance(const State_With_Covariance &state1,
                                                              const State &state2) const;

  std::vector<State> integrate(const State &state, const std::vector<Control> &controls) const;

  std::vector<State_With_Covariance> integrate_with_covariance(const State_With_Covariance &state,
                                                               const std::vector<Control> &controls) const;

  State interpolate(const State &state, const std::vector<Control> &controls, double t) const;

  inline State integrate_ODE(const State &state, const Control &control, double integration_step) const;

private:
  double kappa_;

  double kappa_inv_;

  double discretization_;

  EKF ekf_;
};

} // namespace steering

#endif