A Conjugate Gradient 2D path smoother implementation. More...

#include <nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp>

Public Member Functions
	Smoother (const SmootherParams &params)
	A constructor for nav2_smac_planner::Smoother.

	~Smoother ()
	A destructor for nav2_smac_planner::Smoother.

void	initialize (const double &min_turning_radius)
	Initialization of the smoother. More...

bool	smooth (nav_msgs::msg::Path &path, const nav2_costmap_2d::Costmap2D *costmap, const double &max_time)
	Smoother API method. More...

Protected Member 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. More...

double	getFieldByDim (const geometry_msgs::msg::PoseStamped &msg, const unsigned int &dim)
	Get the field value for a given dimension. More...

void	setFieldByDim (geometry_msgs::msg::PoseStamped &msg, const unsigned int dim, const double &value)
	Set the field value for a given dimension. More...

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) More...

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) More...

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) More...

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 `. More...

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. More...

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. More...

void	updateApproximatePathOrientations (nav_msgs::msg::Path &path, bool &reversing_segment)
	For a given path, update the path point orientations based on smoothing. More...

Protected Attributes
double	min_turning_rad_

double	tolerance_

double	data_w_

double	smooth_w_

int	max_its_

int	refinement_ctr_

int	refinement_num_

bool	is_holonomic_

bool	do_refinement_

MotionModel	motion_model_

ompl::base::StateSpacePtr	state_space_

Detailed Description

A Conjugate Gradient 2D path smoother implementation.

A path smoother implementation.

Member Function Documentation

◆ enforceEndBoundaryConditions()

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

protected

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

Definition at line 475 of file smoother.cpp.

◆ enforceStartBoundaryConditions()

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

protected

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

Definition at line 429 of file smoother.cpp.

◆ findBoundaryExpansion()

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

protected

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

Definition at line 332 of file smoother.cpp.

References nav2_costmap_2d::Costmap2D::getCost(), and nav2_costmap_2d::Costmap2D::worldToMap().

Here is the call graph for this function:

◆ findDirectionalPathSegments()

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

protected

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

Definition at line 224 of file smoother.cpp.

◆ findShortestBoundaryExpansionIdx()

unsigned int nav2_smac_planner::Smoother::findShortestBoundaryExpansionIdx ( const BoundaryExpansions & boundary_expansions )

protected

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

Definition at line 310 of file smoother.cpp.

◆ generateBoundaryExpansionPoints()

template<typename IteratorT >

BoundaryExpansions nav2_smac_planner::Smoother::generateBoundaryExpansionPoints	(	IteratorT	start,
		IteratorT	end
	)

protected

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

Definition at line 392 of file smoother.cpp.

◆ getFieldByDim()

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

inlineprotected

Get the field value for a given dimension.

Parameters

msg	Current pose to sample
dim	Dimension ID of interest

Returns: dim value

Definition at line 199 of file smoother.cpp.

◆ initialize()

void nav2_smac_planner::Smoother::initialize ( const double & min_turning_radius )

Initialization of the smoother.

Parameters

min_turning_radius	Minimum turning radius (m)
motion_model	Motion model type

Definition at line 44 of file smoother.cpp.

◆ setFieldByDim()

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

inlineprotected

Set the field value for a given dimension.

Parameters

msg	Current pose to sample
dim	Dimension ID of interest
value	to set the dimension to for the pose

Definition at line 211 of file smoother.cpp.

◆ smooth()

bool nav2_smac_planner::Smoother::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

Definition at line 50 of file smoother.cpp.

◆ smoothImpl()

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

protected

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

Definition at line 105 of file smoother.cpp.

References nav2_costmap_2d::Costmap2D::getCost(), and nav2_costmap_2d::Costmap2D::worldToMap().

Here is the call graph for this function:

◆ updateApproximatePathOrientations()

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

inlineprotected

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

Definition at line 274 of file smoother.cpp.

The documentation for this class was generated from the following files:

nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
nav2_smac_planner/src/smoother.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ enforceEndBoundaryConditions()

◆ enforceStartBoundaryConditions()

◆ findBoundaryExpansion()

◆ findDirectionalPathSegments()

◆ findShortestBoundaryExpansionIdx()

◆ generateBoundaryExpansionPoints()

◆ getFieldByDim()

◆ initialize()

◆ setFieldByDim()

◆ smooth()

◆ smoothImpl()

◆ updateApproximatePathOrientations()