Rotate to rough path heading controller shim plugin. More...

#include <nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp>

Inheritance diagram for nav2_rotation_shim_controller::RotationShimController:

Collaboration diagram for nav2_rotation_shim_controller::RotationShimController:

[legend]

Public Member Functions
	RotationShimController ()
	Constructor for nav2_rotation_shim_controller::RotationShimController.

	~RotationShimController () override=default
	Destrructor for nav2_rotation_shim_controller::RotationShimController.

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

void	reset () override
	Reset the state of the controller.

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

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

Protected Member Functions
geometry_msgs::msg::PoseStamped	getSampledPathPt (const geometry_msgs::msg::PoseStamped &global_goal)
	Finds the point on the path that is roughly the sampling point distance away from the robot for use. May throw exception if a point at least that far away cannot be found. More...

geometry_msgs::msg::Pose	transformPoseToBaseFrame (const geometry_msgs::msg::PoseStamped &pt)
	Uses TF to find the location of the sampled path point in base frame. More...

geometry_msgs::msg::TwistStamped	computeRotateToHeadingCommand (const double &angular_distance, const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity)
	Rotates the robot to the rough heading. More...

void	isCollisionFree (const geometry_msgs::msg::TwistStamped &cmd_vel, const double &angular_distance_to_heading, const geometry_msgs::msg::PoseStamped &pose)
	Checks if rotation is safe. More...

bool	isGoalChanged (const geometry_msgs::msg::PoseStamped &goal)
	Checks if the goal has changed based on the given path. More...

Protected Attributes
nav2::LifecycleNode::WeakPtr	node_

nav2::TransformBuffer::SharedPtr	tf_

std::string	plugin_name_

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

rclcpp::Clock::SharedPtr	clock_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

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

pluginlib::ClassLoader< nav2_core::Controller >	lp_loader_

nav2_core::Controller::Ptr	primary_controller_

bool	path_updated_

nav_msgs::msg::Path	current_path_

geometry_msgs::msg::PoseStamped	current_goal_

Parameters *	params_

bool	in_rotation_

double	last_angular_vel_ = std::numeric_limits<double>::max()

std::unique_ptr< nav2_rotation_shim_controller::ParameterHandler >	param_handler_

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

Detailed Description

Rotate to rough path heading controller shim plugin.

Definition at line 41 of file nav2_rotation_shim_controller.hpp.

Member Function Documentation

◆ computeRotateToHeadingCommand()

geometry_msgs::msg::TwistStamped nav2_rotation_shim_controller::RotationShimController::computeRotateToHeadingCommand	(	const double &	angular_distance,
		const geometry_msgs::msg::PoseStamped &	pose,
		const geometry_msgs::msg::Twist &	velocity
	)

protected

Rotates the robot to the rough heading.

Parameters

angular_distance	Angular distance to the goal remaining
pose	Starting pose of robot
velocity	Starting velocity of robot

Returns: Twist command for rotation to rough heading

Definition at line 257 of file nav2_rotation_shim_controller.cpp.

References isCollisionFree().

Referenced by computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped nav2_rotation_shim_controller::RotationShimController::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 nav2_rotation_shim_controller.cpp.

References computeRotateToHeadingCommand(), getSampledPathPt(), isGoalChanged(), nav2_core::GoalChecker::isGoalXYReached(), and transformPoseToBaseFrame().

Here is the call graph for this function:

◆ configure()

void nav2_rotation_shim_controller::RotationShimController::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 42 of file nav2_rotation_shim_controller.cpp.

◆ getSampledPathPt()

geometry_msgs::msg::PoseStamped nav2_rotation_shim_controller::RotationShimController::getSampledPathPt ( const geometry_msgs::msg::PoseStamped & global_goal )

protected

Finds the point on the path that is roughly the sampling point distance away from the robot for use. May throw exception if a point at least that far away cannot be found.

Returns: pt location of the output point

Definition at line 208 of file nav2_rotation_shim_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ isCollisionFree()

void nav2_rotation_shim_controller::RotationShimController::isCollisionFree	(	const geometry_msgs::msg::TwistStamped &	cmd_vel,
		const double &	angular_distance_to_heading,
		const geometry_msgs::msg::PoseStamped &	pose
	)

protected

Checks if rotation is safe.

Parameters

cmd_vel	Velocity to check over
angular_distance_to_heading	Angular distance to heading requested
pose	Starting pose of robot

Definition at line 288 of file nav2_rotation_shim_controller.cpp.

Referenced by computeRotateToHeadingCommand().

Here is the caller graph for this function:

◆ isGoalChanged()

bool nav2_rotation_shim_controller::RotationShimController::isGoalChanged ( const geometry_msgs::msg::PoseStamped & goal )

protected

Checks if the goal has changed based on the given path.

Parameters

goal	The goal to compare with the last goal.

Returns: True if the goal has changed, false otherwise.

Definition at line 328 of file nav2_rotation_shim_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ newPathReceived()

void nav2_rotation_shim_controller::RotationShimController::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 339 of file nav2_rotation_shim_controller.cpp.

◆ setSpeedLimit()

void nav2_rotation_shim_controller::RotationShimController::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 344 of file nav2_rotation_shim_controller.cpp.

◆ transformPoseToBaseFrame()

geometry_msgs::msg::Pose nav2_rotation_shim_controller::RotationShimController::transformPoseToBaseFrame ( const geometry_msgs::msg::PoseStamped & pt )

protected

Uses TF to find the location of the sampled path point in base frame.

Parameters

pt	location of the sampled path point

Returns: location of the pose in base frame

Definition at line 245 of file nav2_rotation_shim_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

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

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ computeRotateToHeadingCommand()

◆ computeVelocityCommands()

◆ configure()

◆ getSampledPathPt()

◆ isCollisionFree()

◆ isGoalChanged()

◆ newPathReceived()

◆ setSpeedLimit()

◆ transformPoseToBaseFrame()