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

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 ()
	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::PoseStamped	getSampledPathGoal ()
	Find the goal point in path May throw exception if the path is empty. 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 nav_msgs::msg::Path &path)
	Checks if the goal has changed based on the given path. More...

rcl_interfaces::msg::SetParametersResult	dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
	Callback executed when a parameter change is detected. More...

Protected Attributes
nav2::LifecycleNode::WeakPtr	node_

std::shared_ptr< tf2_ros::Buffer >	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_

double	forward_sampling_distance_

double	angular_dist_threshold_

double	angular_disengage_threshold_

double	rotate_to_heading_angular_vel_

double	max_angular_accel_

double	control_duration_

double	simulate_ahead_time_

bool	rotate_to_goal_heading_

bool	in_rotation_

bool	rotate_to_heading_once_

bool	closed_loop_

bool	use_path_orientations_

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

std::mutex	mutex_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

std::unique_ptr< nav2_controller::PositionGoalChecker >	position_goal_checker_

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 40 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 313 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
	)

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 168 of file nav2_rotation_shim_controller.cpp.

References computeRotateToHeadingCommand(), getSampledPathGoal(), getSampledPathPt(), nav2_core::GoalChecker::getTolerances(), 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,
		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 41 of file nav2_rotation_shim_controller.cpp.

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_rotation_shim_controller::RotationShimController::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 415 of file nav2_rotation_shim_controller.cpp.

Referenced by activate().

Here is the caller graph for this function:

◆ getSampledPathGoal()

geometry_msgs::msg::PoseStamped nav2_rotation_shim_controller::RotationShimController::getSampledPathGoal ( )

protected

Find the goal point in path May throw exception if the path is empty.

Returns: pt location of the output point

Definition at line 290 of file nav2_rotation_shim_controller.cpp.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ getSampledPathPt()

geometry_msgs::msg::PoseStamped nav2_rotation_shim_controller::RotationShimController::getSampledPathPt ( )

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 263 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 343 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 nav_msgs::msg::Path & path )

protected

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

Parameters

path	The path to compare with the current goal.

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

Definition at line 383 of file nav2_rotation_shim_controller.cpp.

Referenced by setPlan().

Here is the caller graph for this function:

◆ setPlan()

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

References isGoalChanged().

Here is the call graph for this function:

◆ 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 402 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 303 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()

◆ dynamicParametersCallback()

◆ getSampledPathGoal()

◆ getSampledPathPt()

◆ isCollisionFree()

◆ isGoalChanged()

◆ setPlan()

◆ setSpeedLimit()

◆ transformPoseToBaseFrame()