Program Listing for File smoother.hpp
↰ Return to documentation for file (include/nav2_smac_planner/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_, refinement_num_;
bool is_holonomic_, do_refinement_;
MotionModel motion_model_;
ompl::base::StateSpacePtr state_space_;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__SMOOTHER_HPP_