Class Smoother

Class Documentation

class Smoother

A path smoother implementation.

Public Functions

explicit Smoother(const SmootherParams &params)

A constructor for nav2_smac_planner::Smoother.

inline ~Smoother()

A destructor for nav2_smac_planner::Smoother.

void initialize(const double &min_turning_radius)

Initialization of the smoother.

Parameters:

  • min_turning_radius – Minimum turning radius (m)

  • motion_model – Motion model type

bool smooth(nav_msgs::msg::Path &path, const nav2_costmap_2d::Costmap2D *costmap, const double &max_time)

Smoother API method.

Parameters:

  • path – Reference to path

  • costmap – Pointer to minimal costmap

  • max_time – Maximum time to compute, stop early if over limit

Returns:

If smoothing was successful

Protected Functions

bool smoothImpl(nav_msgs::msg::Path &path, bool &reversing_segment, const nav2_costmap_2d::Costmap2D *costmap, const double &max_time)

Smoother method - does the smoothing on a segment.

Parameters:

  • path – Reference to path

  • reversing_segment – Return if this is a reversing segment

  • costmap – Pointer to minimal costmap

  • max_time – Maximum time to compute, stop early if over limit

Returns:

If smoothing was successful

inline double getFieldByDim(const geometry_msgs::msg::PoseStamped &msg, const unsigned int &dim)

Get the field value for a given dimension.

Parameters:

  • msg – Current pose to sample

  • dim – Dimension ID of interest

Returns:

dim value

inline void setFieldByDim(geometry_msgs::msg::PoseStamped &msg, const unsigned int dim, const double &value)

Set the field value for a given dimension.

Parameters:

  • msg – Current pose to sample

  • dim – Dimension ID of interest

  • value – to set the dimention to for the pose

std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path &path)

Finds the starting and end indices of path segments where the robot is traveling in the same direction (e.g. forward vs reverse)

Parameters:

path – Path in which to look for cusps

Returns:

Set of index pairs for each segment of the path in a given direction

void enforceStartBoundaryConditions(const geometry_msgs::msg::Pose &start_pose, nav_msgs::msg::Path &path, const nav2_costmap_2d::Costmap2D *costmap, const bool &reversing_segment)

Enforced minimum curvature boundary conditions on plan output the robot is traveling in the same direction (e.g. forward vs reverse)

Parameters:

  • start_pose – Start pose of the feasible path to maintain

  • path – Path to modify for curvature constraints on start / end of path

  • costmap – Costmap to check for collisions

  • reversing_segment – Whether this path segment is in reverse

void enforceEndBoundaryConditions(const geometry_msgs::msg::Pose &end_pose, nav_msgs::msg::Path &path, const nav2_costmap_2d::Costmap2D *costmap, const bool &reversing_segment)

Enforced minimum curvature boundary conditions on plan output the robot is traveling in the same direction (e.g. forward vs reverse)

Parameters:

  • end_pose – End pose of the feasible path to maintain

  • path – Path to modify for curvature constraints on start / end of path

  • costmap – Costmap to check for collisions

  • reversing_segment – Whether this path segment is in reverse

unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions &boundary_expansions)

Given a set of boundary expansion, find the one which is shortest such that it is least likely to contain a loop-de-loop when working with close-by primitive markers. Instead, select a further away marker which generates a shorter `.

Parameters:

boundary_expansions – Set of boundary expansions

Returns:

Idx of the shorest boundary expansion option

void findBoundaryExpansion(const geometry_msgs::msg::Pose &start, const geometry_msgs::msg::Pose &end, BoundaryExpansion &expansion, const nav2_costmap_2d::Costmap2D *costmap)

Populate a motion model expansion from start->end into expansion.

Parameters:

  • start – Start pose of the feasible path to maintain

  • end – End pose of the feasible path to maintain

  • expansion – Expansion object to populate

  • costmap – Costmap to check for collisions

  • reversing_segment – Whether this path segment is in reverse

template<typename IteratorT>
BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end)

Generates boundary expansions with end idx at least strategic distances away, using either Reverse or (Forward) Path Iterators.

Parameters:

  • start – iterator to start search in path for

  • end – iterator to end search for

Returns:

Boundary expansions with end idxs populated

inline void updateApproximatePathOrientations(nav_msgs::msg::Path &path, bool &reversing_segment)

For a given path, update the path point orientations based on smoothing.

Parameters:

  • path – Path to approximate the path orientation in

  • reversing_segment – Return if this is a reversing segment

Protected Attributes

double min_turning_rad_
double tolerance_
double data_w_
double smooth_w_
int max_its_
int refinement_ctr_
bool is_holonomic_
bool do_refinement_
MotionModel motion_model_
ompl::base::StateSpacePtr state_space_