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

Protected Member Functions
geometry_msgs::msg::PoseStamped	getMotionTarget (const double &motion_target_dist, const nav_msgs::msg::Path &path)
	Get motion target point. More...

bool	simulateTrajectory (const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::PoseStamped &motion_target, const geometry_msgs::msg::TransformStamped &costmap_transform, nav_msgs::msg::Path &trajectory, const 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 (const 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...

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_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > >	transformed_plan_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > >	local_plan_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PointStamped > >	motion_target_pub_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::Marker > >	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

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

References getMotionTarget(), nav2_core::GoalChecker::getTolerances(), rotateToTarget(), and simulateTrajectory().

Here is the call graph for this function:

◆ configure()

void nav2_graceful_controller::GracefulController::configure	(	const rclcpp_lifecycle::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 23 of file graceful_controller.cpp.

◆ getMotionTarget()

geometry_msgs::msg::PoseStamped nav2_graceful_controller::GracefulController::getMotionTarget	(	const double &	motion_target_dist,
		const nav_msgs::msg::Path &	path
	)

protected

Get motion target point.

Parameters

motion_target_dist	Optimal motion target distance
path	Current global path

Returns: Motion target point

Definition at line 243 of file graceful_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

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

Referenced by simulateTrajectory().

Here is the caller graph for this function:

◆ rotateToTarget()

geometry_msgs::msg::Twist nav2_graceful_controller::GracefulController::rotateToTarget ( const 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 316 of file graceful_controller.cpp.

Referenced by computeVelocityCommands().

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 213 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 219 of file graceful_controller.cpp.

◆ simulateTrajectory()

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

protected

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

Parameters

robot_pose	Robot pose
motion_target	Motion target point
costmap_transform	Transform between global and local costmap
trajectory	Simulated trajectory
backward	Flag to indicate if the robot is moving backward