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

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

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 392 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 346 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 249 of file smoother.cpp.

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

Here is the call graph for this function:

◆ 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 227 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 309 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 202 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 46 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 214 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 52 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 108 of file smoother.cpp.

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

Here is the call graph for this function:

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()

◆ findShortestBoundaryExpansionIdx()

◆ generateBoundaryExpansionPoints()

◆ getFieldByDim()

◆ initialize()

◆ setFieldByDim()

◆ smooth()

◆ smoothImpl()