Graceful controller plugin. More...

#include <nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp>

Inheritance diagram for nav2_graceful_controller::GracefulController:

Collaboration diagram for nav2_graceful_controller::GracefulController:

[legend]

Public Member Functions
	GracefulController ()=default
	Constructor for nav2_graceful_controller::GracefulController.

	~GracefulController () override=default
	Destructor for nav2_graceful_controller::GracefulController.

void	configure (const nav2::LifecycleNode::WeakPtr &parent, std::string name, nav2::TransformBuffer::SharedPtr tf, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) override
	Configure controller state machine. More...

void	cleanup () override
	Cleanup controller state machine.

void	activate () override
	Activate controller state machine.

void	deactivate () override
	Deactivate controller state machine.

geometry_msgs::msg::TwistStamped	computeVelocityCommands (const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker, const nav_msgs::msg::Path &transformed_global_plan, const geometry_msgs::msg::PoseStamped &global_goal) override
	Compute the best command given the current pose and velocity. More...

void	newPathReceived (const nav_msgs::msg::Path &raw_global_path) override
	nav2_core newPathReceived - Receives a new plan from the Planner Server More...

void	setSpeedLimit (const double &speed_limit, const bool &percentage) override
	Limits the maximum linear speed of the robot. More...

Public Member Functions inherited from nav2_core::Controller
virtual	~Controller ()
	Virtual destructor.

virtual bool	cancel ()
	Cancel the current control action. More...

virtual void	reset ()
	Reset the state of the controller if necessary after task is exited.

Protected Member Functions
bool	validateTargetPose (geometry_msgs::msg::PoseStamped &target_pose, double dist_to_target, nav_msgs::msg::Path &trajectory, geometry_msgs::msg::TransformStamped &costmap_transform, geometry_msgs::msg::TwistStamped &cmd_vel)
	Validate a given target pose for calculating command velocity. More...

bool	validateTargetPoseOnApproach (geometry_msgs::msg::PoseStamped &target_pose, double dist_to_target, double dist_to_goal, nav_msgs::msg::Path &trajectory, geometry_msgs::msg::TransformStamped &costmap_transform, geometry_msgs::msg::TwistStamped &cmd_vel)
	Validate a given target pose for calculating command velocity on approach to goal. More...

bool	findBestApproachTrajectory (geometry_msgs::msg::PoseStamped &target_pose, double dist_to_target, geometry_msgs::msg::TransformStamped &costmap_transform, double safety_cost, nav_msgs::msg::Path &best_trajectory, geometry_msgs::msg::TwistStamped &best_cmd_vel)
	Find the best approach trajectory by searching multiple orientations. More...

bool	simulateTrajectory (const geometry_msgs::msg::PoseStamped &motion_target, const geometry_msgs::msg::TransformStamped &costmap_transform, nav_msgs::msg::Path &trajectory, geometry_msgs::msg::TwistStamped &cmd_vel, bool backward)
	Simulate trajectory calculating in every step the new velocity command based on a new curvature value and checking for collisions. More...

geometry_msgs::msg::Twist	rotateToTarget (double angle_to_target)
	Rotate the robot to face the motion target with maximum angular velocity. More...

bool	inCollision (const double &x, const double &y, const double &theta, double inflation_scale=1.0)
	Checks if the robot is in collision. More...

double	getMaxCost (const nav_msgs::msg::Path &path, geometry_msgs::msg::TransformStamped &costmap_transform)
	Get the maximum cost of a path. More...

void	computeDistanceAlongPath (const std::vector< geometry_msgs::msg::PoseStamped > &poses, std::vector< double > &distances)
	Compute the distance to each pose in a path. More...

void	validateOrientations (std::vector< geometry_msgs::msg::PoseStamped > &path)
	Control law requires proper orientations, not all planners provide them. More...

Protected Attributes
nav2::TransformBuffer::SharedPtr	tf_buffer_

std::string	plugin_name_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::unique_ptr< nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * > >	collision_checker_

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

Parameters *	params_

bool	do_initial_rotation_

std::optional< double >	safe_approach_angle_

nav2::Publisher< nav_msgs::msg::Path >::SharedPtr	local_plan_pub_

nav2::Publisher< geometry_msgs::msg::PoseStamped >::SharedPtr	motion_target_pub_

nav2::Publisher< visualization_msgs::msg::Marker >::SharedPtr	slowdown_pub_

std::unique_ptr< nav2_graceful_controller::ParameterHandler >	param_handler_

std::unique_ptr< nav2_graceful_controller::SmoothControlLaw >	control_law_

Additional Inherited Members
Public Types inherited from nav2_core::Controller
using	Ptr = std::shared_ptr< nav2_core::Controller >

Detailed Description

Graceful controller plugin.

Definition at line 42 of file graceful_controller.hpp.

Member Function Documentation

◆ computeDistanceAlongPath()

void nav2_graceful_controller::GracefulController::computeDistanceAlongPath	(	const std::vector< geometry_msgs::msg::PoseStamped > &	poses,
		std::vector< double > &	distances
	)

protected

Compute the distance to each pose in a path.

Parameters

poses	Poses to compute distances with
distances	Computed distances

Definition at line 524 of file graceful_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped nav2_graceful_controller::GracefulController::computeVelocityCommands	(	const geometry_msgs::msg::PoseStamped &	pose,
		const geometry_msgs::msg::Twist &	velocity,
		nav2_core::GoalChecker *	goal_checker,
		const nav_msgs::msg::Path &	transformed_global_plan,
		const geometry_msgs::msg::PoseStamped &	global_goal
	)

overridevirtual

Compute the best command given the current pose and velocity.

Parameters

pose	Current robot pose
velocity	Current robot velocity
goal_checker	Ptr to the goal checker for this task in case useful in computing commands
transformed_global_plan	The global plan after being processed by the path handler
global_goal	The last pose of the global plan

Returns: Best command

Implements nav2_core::Controller.

Definition at line 119 of file graceful_controller.cpp.

References computeDistanceAlongPath(), inCollision(), nav2_core::GoalChecker::isGoalXYReached(), rotateToTarget(), validateOrientations(), validateTargetPose(), and validateTargetPoseOnApproach().

Here is the call graph for this function:

◆ configure()

void nav2_graceful_controller::GracefulController::configure	(	const nav2::LifecycleNode::WeakPtr &	parent,
		std::string	name,
		nav2::TransformBuffer::SharedPtr	tf,
		std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros
	)

overridevirtual

Configure controller state machine.

Parameters

parent	WeakPtr to node
name	Name of plugin
tf	TF buffer
costmap_ros	Costmap2DROS object of environment

Implements nav2_core::Controller.

Definition at line 30 of file graceful_controller.cpp.

◆ findBestApproachTrajectory()

bool nav2_graceful_controller::GracefulController::findBestApproachTrajectory	(	geometry_msgs::msg::PoseStamped &	target_pose,
		double	dist_to_target,
		geometry_msgs::msg::TransformStamped &	costmap_transform,
		double	safety_cost,
		nav_msgs::msg::Path &	best_trajectory,
		geometry_msgs::msg::TwistStamped &	best_cmd_vel
	)

protected

Find the best approach trajectory by searching multiple orientations.

Parameters

target_pose	Base target pose (position will be used)
dist_to_target	Distance to target
costmap_transform	Transform
safety_cost	Safety cost threshold
best_trajectory	Reference to store best trajectory
best_cmd_vel	Reference to store best cmd_vel

Returns: true if a valid trajectory was found

Definition at line 564 of file graceful_controller.cpp.

References getMaxCost(), and validateTargetPose().

Referenced by validateTargetPoseOnApproach().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getMaxCost()

double nav2_graceful_controller::GracefulController::getMaxCost	(	const nav_msgs::msg::Path &	path,
		geometry_msgs::msg::TransformStamped &	costmap_transform
	)

protected

Get the maximum cost of a path.

Parameters

path	Path to check
costmap_transform	Transform between global and local costmap

Returns: Maximum cost encountered

Definition at line 456 of file graceful_controller.cpp.

Referenced by findBestApproachTrajectory(), and validateTargetPoseOnApproach().

Here is the caller graph for this function:

◆ inCollision()

bool nav2_graceful_controller::GracefulController::inCollision	(	const double &	x,
		const double &	y,
		const double &	theta,
		double	inflation_scale = `1.0`
	)

protected

Checks if the robot is in collision.

Parameters

x	The x coordinate of the robot in global frame
y	The y coordinate of the robot in global frame
theta	The orientation of the robot in global frame
inflation_scale	Scaling factor for the robot footprint

Returns: Whether in collision

Definition at line 475 of file graceful_controller.cpp.

Referenced by computeVelocityCommands(), and simulateTrajectory().

Here is the caller graph for this function:

◆ newPathReceived()

void nav2_graceful_controller::GracefulController::newPathReceived ( const nav_msgs::msg::Path & raw_global_path )

overridevirtual

nav2_core newPathReceived - Receives a new plan from the Planner Server

Parameters

raw_global_path The global plan from the Planner Server

Implements nav2_core::Controller.

Definition at line 254 of file graceful_controller.cpp.

◆ rotateToTarget()

geometry_msgs::msg::Twist nav2_graceful_controller::GracefulController::rotateToTarget ( double angle_to_target )

protected

Rotate the robot to face the motion target with maximum angular velocity.

Parameters

angle_to_target Angle to the motion target

Returns: geometry_msgs::msg::Twist Velocity command

Definition at line 445 of file graceful_controller.cpp.

Referenced by computeVelocityCommands(), and simulateTrajectory().

Here is the caller graph for this function:

◆ setSpeedLimit()

void nav2_graceful_controller::GracefulController::setSpeedLimit	(	const double &	speed_limit,
		const bool &	percentage
	)

overridevirtual

Limits the maximum linear speed of the robot.

Parameters

speed_limit	expressed in absolute value (in m/s) or in percentage from maximum robot speed
percentage	setting speed limit in percentage if true or in absolute values in false case

Implements nav2_core::Controller.

Definition at line 260 of file graceful_controller.cpp.

◆ simulateTrajectory()

bool nav2_graceful_controller::GracefulController::simulateTrajectory	(	const geometry_msgs::msg::PoseStamped &	motion_target,
		const geometry_msgs::msg::TransformStamped &	costmap_transform,
		nav_msgs::msg::Path &	trajectory,
		geometry_msgs::msg::TwistStamped &	cmd_vel,
		bool	backward
	)

protected

Simulate trajectory calculating in every step the new velocity command based on a new curvature value and checking for collisions.

Parameters

motion_target	Motion target point (in costmap local frame?)
costmap_transform	Transform between global and local costmap
trajectory	Simulated trajectory
cmd_vel	Initial command velocity during simulation
backward	Flag to indicate if the robot is moving backward

Returns: true if the trajectory is collision free, false otherwise

Definition at line 352 of file graceful_controller.cpp.

References inCollision(), and rotateToTarget().

Referenced by validateTargetPose().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ validateOrientations()

void nav2_graceful_controller::GracefulController::validateOrientations ( std::vector< geometry_msgs::msg::PoseStamped > & path )

protected

Control law requires proper orientations, not all planners provide them.

Parameters

path	Path to add orientations into, if required

Definition at line 539 of file graceful_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ validateTargetPose()

bool nav2_graceful_controller::GracefulController::validateTargetPose	(	geometry_msgs::msg::PoseStamped &	target_pose,
		double	dist_to_target,
		nav_msgs::msg::Path &	trajectory,
		geometry_msgs::msg::TransformStamped &	costmap_transform,
		geometry_msgs::msg::TwistStamped &	cmd_vel
	)

protected

Validate a given target pose for calculating command velocity.

Parameters

target_pose	Target pose to validate
dist_to_target	Distance to target pose
trajectory	Trajectory to validate in simulation
costmap_transform	Transform between global and local costmap
cmd_vel	Initial command velocity to validate in simulation

Returns: true if target pose is valid, false otherwise

Definition at line 284 of file graceful_controller.cpp.

References simulateTrajectory().

Referenced by computeVelocityCommands(), findBestApproachTrajectory(), and validateTargetPoseOnApproach().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ validateTargetPoseOnApproach()

bool nav2_graceful_controller::GracefulController::validateTargetPoseOnApproach	(	geometry_msgs::msg::PoseStamped &	target_pose,
		double	dist_to_target,
		double	dist_to_goal,
		nav_msgs::msg::Path &	trajectory,
		geometry_msgs::msg::TransformStamped &	costmap_transform,
		geometry_msgs::msg::TwistStamped &	cmd_vel
	)

protected

Validate a given target pose for calculating command velocity on approach to goal.

Parameters

target_pose	Target pose to validate
dist_to_target	Distance to target pose
dist_to_goal	Distance to navigation goal
trajectory	Trajectory to validate in simulation
costmap_transform	Transform between global and local costmap
cmd_vel	Initial command velocity to validate in simulation

Returns: true if target pose is valid, false otherwise

Definition at line 318 of file graceful_controller.cpp.

References findBestApproachTrajectory(), getMaxCost(), and validateTargetPose().

Referenced by computeVelocityCommands().

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_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
nav2_graceful_controller/src/graceful_controller.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ computeDistanceAlongPath()

◆ computeVelocityCommands()

◆ configure()

◆ findBestApproachTrajectory()

◆ getMaxCost()

◆ inCollision()

◆ newPathReceived()

◆ rotateToTarget()

◆ setSpeedLimit()

◆ simulateTrajectory()

◆ validateOrientations()

◆ validateTargetPose()

◆ validateTargetPoseOnApproach()