smoother.hpp

// Copyright (c) 2021, Samsung Research America
//
// 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. Reserved.
 
#ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_
#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_
 
#include <cmath>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
 
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "angles/angles.h"
#include "tf2/utils.h"
#include "ompl/base/StateSpace.h"
#include "ompl/base/spaces/DubinsStateSpace.h"
 
namespace nav2_smac_planner
{
 
struct PathSegment
{
  unsigned int start;
  unsigned int end;
};
 
struct BoundaryPoints
{
  BoundaryPoints(double & x_in, double & y_in, double & theta_in)
  : x(x_in), y(y_in), theta(theta_in)
  {}
 
  double x;
  double y;
  double theta;
};
 
struct BoundaryExpansion
{
  double path_end_idx{0.0};
  double expansion_path_length{0.0};
  double original_path_length{0.0};
  std::vector<BoundaryPoints> pts;
  bool in_collision{false};
};
 
typedef std::vector<BoundaryExpansion> BoundaryExpansions;
typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
 
class Smoother
{
public:
  explicit Smoother(const SmootherParams & params);
 
  ~Smoother() {}
 
  void initialize(
    const double & min_turning_radius);
 
  bool smooth(
    nav_msgs::msg::Path & path,
    const nav2_costmap_2d::Costmap2D * costmap,
    const double & max_time);
 
protected:
  bool smoothImpl(
    nav_msgs::msg::Path & path,
    bool & reversing_segment,
    const nav2_costmap_2d::Costmap2D * costmap,
    const double & max_time);
 
  inline double getFieldByDim(
    const geometry_msgs::msg::PoseStamped & msg,
    const unsigned int & dim);
 
  inline void setFieldByDim(
    geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
    const double & value);
 
  std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
 
  void enforceStartBoundaryConditions(
    const geometry_msgs::msg::Pose & start_pose,
    nav_msgs::msg::Path & path,
    const nav2_costmap_2d::Costmap2D * costmap,
    const bool & reversing_segment);
 
  void enforceEndBoundaryConditions(
    const geometry_msgs::msg::Pose & end_pose,
    nav_msgs::msg::Path & path,
    const nav2_costmap_2d::Costmap2D * costmap,
    const bool & reversing_segment);
 
  unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions);
 
  void findBoundaryExpansion(
    const geometry_msgs::msg::Pose & start,
    const geometry_msgs::msg::Pose & end,
    BoundaryExpansion & expansion,
    const nav2_costmap_2d::Costmap2D * costmap);
 
  template<typename IteratorT>
  BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end);
 
  inline void updateApproximatePathOrientations(
    nav_msgs::msg::Path & path,
    bool & reversing_segment);
 
  double min_turning_rad_, tolerance_, data_w_, smooth_w_;
  int max_its_, refinement_ctr_;
  bool is_holonomic_, do_refinement_;
  MotionModel motion_model_;
  ompl::base::StateSpacePtr state_space_;
};
 
}  // namespace nav2_smac_planner
 
#endif  // NAV2_SMAC_PLANNER__SMOOTHER_HPP_