Manager of incoming reference paths for transformation and processing. More...

#include <nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp>

Collaboration diagram for mppi::PathHandler:

[legend]

Public Member Functions
	PathHandler ()=default
	Constructor for mppi::PathHandler.

	~PathHandler ()=default
	Destructor for mppi::PathHandler.

void	initialize (rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS >, std::shared_ptr< tf2_ros::Buffer >, ParametersHandler *)
	Initialize path handler on bringup. More...

void	setPath (const nav_msgs::msg::Path &plan)
	Set new reference path. More...

nav_msgs::msg::Path &	getPath ()
	Get reference path. More...

nav_msgs::msg::Path	transformPath (const geometry_msgs::msg::PoseStamped &robot_pose)
	transform global plan to local applying constraints, then prune global plan More...

geometry_msgs::msg::PoseStamped	getTransformedGoal (const builtin_interfaces::msg::Time &stamp)
	Get the global goal pose transformed to the local frame. More...

Protected Member Functions
bool	transformPose (const std::string &frame, const geometry_msgs::msg::PoseStamped &in_pose, geometry_msgs::msg::PoseStamped &out_pose) const
	Transform a pose to another frame. More...

double	getMaxCostmapDist ()
	Get largest dimension of costmap (radially) More...

geometry_msgs::msg::PoseStamped	transformToGlobalPlanFrame (const geometry_msgs::msg::PoseStamped &pose)
	Transform a pose to the global reference frame. More...

std::pair< nav_msgs::msg::Path, PathIterator >	getGlobalPlanConsideringBoundsInCostmapFrame (const geometry_msgs::msg::PoseStamped &global_pose)
	Get global plan within window of the local costmap size. More...

void	prunePlan (nav_msgs::msg::Path &plan, const PathIterator end)
	Prune a path to only interesting portions. More...

bool	isWithinInversionTolerances (const geometry_msgs::msg::PoseStamped &robot_pose)
	Check if the robot pose is within the set inversion tolerances. More...

Protected Attributes
std::string	name_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_

ParametersHandler *	parameters_handler_

nav_msgs::msg::Path	global_plan_

nav_msgs::msg::Path	global_plan_up_to_inversion_

rclcpp::Logger	logger_ {rclcpp::get_logger("MPPIController")}

double	max_robot_pose_search_dist_ {0}

double	prune_distance_ {0}

double	transform_tolerance_ {0}

float	inversion_xy_tolerance_ {0.2}

float	inversion_yaw_tolerance {0.4}

bool	enforce_path_inversion_ {false}

unsigned int	inversion_locale_ {0u}

Detailed Description

Manager of incoming reference paths for transformation and processing.

Definition at line 47 of file path_handler.hpp.

Member Function Documentation

◆ getGlobalPlanConsideringBoundsInCostmapFrame()

std::pair< nav_msgs::msg::Path, PathIterator > mppi::PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame ( const geometry_msgs::msg::PoseStamped & global_pose )

protected

Get global plan within window of the local costmap size.

Parameters

global_pose Robot pose

Returns: plan transformed in the costmap frame and iterator to the first pose of the global plan (for pruning)

Definition at line 49 of file path_handler.cpp.

References transformPose().

Referenced by transformPath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getMaxCostmapDist()

double mppi::PathHandler::getMaxCostmapDist ( )

protected

Get largest dimension of costmap (radially)

Returns: Max distance from center of costmap to edge

Definition at line 166 of file path_handler.cpp.

Referenced by initialize().

Here is the caller graph for this function:

◆ getPath()

nav_msgs::msg::Path & mppi::PathHandler::getPath ( )

Get reference path.

Returns: Path

Definition at line 182 of file path_handler.cpp.

◆ getTransformedGoal()

geometry_msgs::msg::PoseStamped mppi::PathHandler::getTransformedGoal ( const builtin_interfaces::msg::Time & stamp )

Get the global goal pose transformed to the local frame.

Parameters

stamp Time to get the goal pose at

Returns: Transformed goal pose

Definition at line 189 of file path_handler.cpp.

References transformPose().

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initialize()

void mppi::PathHandler::initialize	(	rclcpp_lifecycle::LifecycleNode::WeakPtr	parent,
		const std::string &	name,
		std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap,
		std::shared_ptr< tf2_ros::Buffer >	buffer,
		ParametersHandler *	param_handler
	)

Initialize path handler on bringup.

Parameters

parent	WeakPtr to node
name	Name of plugin
costmap_ros	Costmap2DROS object of environment
tf	TF buffer for transformations
dynamic_parameter_handler	Parameter handler object

Definition at line 24 of file path_handler.cpp.

References getMaxCostmapDist(), and mppi::ParametersHandler::getParamGetter().

Referenced by nav2_mppi_controller::MPPIController::configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isWithinInversionTolerances()

bool mppi::PathHandler::isWithinInversionTolerances ( const geometry_msgs::msg::PoseStamped & robot_pose )

protected

Check if the robot pose is within the set inversion tolerances.

Parameters

robot_pose Robot's current pose to check

Returns: bool If the robot pose is within the set inversion tolerances

Definition at line 205 of file path_handler.cpp.

Referenced by transformPath().

Here is the caller graph for this function:

◆ prunePlan()

void mppi::PathHandler::prunePlan	(	nav_msgs::msg::Path &	plan,
		const PathIterator	end
	)

protected

Prune a path to only interesting portions.

Parameters

plan	Plan to prune
end	Final path iterator

Definition at line 184 of file path_handler.cpp.

Referenced by transformPath().

Here is the caller graph for this function:

◆ setPath()

void mppi::PathHandler::setPath ( const nav_msgs::msg::Path & plan )

Set new reference path.

Parameters

Plan	Path to use

Definition at line 173 of file path_handler.cpp.

Referenced by nav2_mppi_controller::MPPIController::setPlan().

Here is the caller graph for this function:

◆ transformPath()

nav_msgs::msg::Path mppi::PathHandler::transformPath ( const geometry_msgs::msg::PoseStamped & robot_pose )

transform global plan to local applying constraints, then prune global plan

Parameters

robot_pose Pose of robot

Returns: global plan in local frame

Definition at line 120 of file path_handler.cpp.

References getGlobalPlanConsideringBoundsInCostmapFrame(), isWithinInversionTolerances(), prunePlan(), and transformToGlobalPlanFrame().

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ transformPose()

bool mppi::PathHandler::transformPose	(	const std::string &	frame,
		const geometry_msgs::msg::PoseStamped &	in_pose,
		geometry_msgs::msg::PoseStamped &	out_pose
	)		const

protected

Transform a pose to another frame.

Parameters

frame	Frame to transform to
in_pose	Input pose
out_pose	Output pose

Returns: Bool if successful

Definition at line 145 of file path_handler.cpp.

Referenced by getGlobalPlanConsideringBoundsInCostmapFrame(), getTransformedGoal(), and transformToGlobalPlanFrame().

Here is the caller graph for this function:

◆ transformToGlobalPlanFrame()

geometry_msgs::msg::PoseStamped mppi::PathHandler::transformToGlobalPlanFrame ( const geometry_msgs::msg::PoseStamped & pose )

protected

Transform a pose to the global reference frame.

Parameters

pose	Current pose

Returns: output poose in global reference frame

Definition at line 104 of file path_handler.cpp.

References transformPose().

Referenced by transformPath().

Here is the call graph for this function:

Here is the caller graph for this function:

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

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
nav2_mppi_controller/src/path_handler.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ getGlobalPlanConsideringBoundsInCostmapFrame()

◆ getMaxCostmapDist()

◆ getPath()

◆ getTransformedGoal()

◆ initialize()

◆ isWithinInversionTolerances()

◆ prunePlan()

◆ setPath()

◆ transformPath()

◆ transformPose()

◆ transformToGlobalPlanFrame()