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, std::shared_ptr< tf2_ros::Buffer > 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) override
	Compute the best command given the current pose and velocity. More...

void	setPlan (const nav_msgs::msg::Path &path) override
	nav2_core setPlan - Sets the global plan. 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	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)
	Checks if the robot is in collision. 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
std::shared_ptr< tf2_ros::Buffer >	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_

double	goal_dist_tolerance_

bool	goal_reached_

bool	do_initial_rotation_

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

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::PathHandler >	path_handler_

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

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

Returns: Best command

Implements nav2_core::Controller.

Definition at line 113 of file graceful_controller.cpp.

References computeDistanceAlongPath(), nav2_core::GoalChecker::getTolerances(), inCollision(), rotateToTarget(), simulateTrajectory(), and validateOrientations().

Here is the call graph for this function:

◆ configure()

void nav2_graceful_controller::GracefulController::configure	(	const nav2::LifecycleNode::WeakPtr &	parent,
		std::string	name,
		std::shared_ptr< tf2_ros::Buffer >	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 27 of file graceful_controller.cpp.

◆ inCollision()

bool nav2_graceful_controller::GracefulController::inCollision	(	const double &	x,
		const double &	y,
		const double &	theta
	)

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

Returns: Whether in collision

Definition at line 377 of file graceful_controller.cpp.

Referenced by computeVelocityCommands(), and simulateTrajectory().

Here is the caller graph for this function:

◆ 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 367 of file graceful_controller.cpp.

Referenced by computeVelocityCommands(), and simulateTrajectory().

Here is the caller graph for this function:

◆ setPlan()

void nav2_graceful_controller::GracefulController::setPlan ( const nav_msgs::msg::Path & path )

overridevirtual

nav2_core setPlan - Sets the global plan.

Parameters

path	The global plan

Implements nav2_core::Controller.

Definition at line 255 of file graceful_controller.cpp.

◆ 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 262 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