Default control law for approaching a dock target. More...

#include <nav2_docking/opennav_docking/include/opennav_docking/controller.hpp>

Collaboration diagram for opennav_docking::Controller:

Public Member Functions
	Controller (const nav2::LifecycleNode::SharedPtr &node, nav2::TransformBuffer::SharedPtr tf, std::string fixed_frame, std::string base_frame)
	Create a controller instance. Configure ROS 2 parameters. More...

	~Controller ()
	A destructor for opennav_docking::Controller.

bool	computeVelocityCommand (const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Twist &cmd, bool is_docking, bool backward=false)
	Compute a velocity command using control law. More...

geometry_msgs::msg::Twist	computeRotateToHeadingCommand (const double &angular_distance_to_heading, const geometry_msgs::msg::Twist &current_velocity, const double &dt)
	Perform a command for in-place rotation. More...

Protected Member Functions
bool	isTrajectoryCollisionFree (const geometry_msgs::msg::Pose &target_pose, bool is_docking, bool backward=false)
	Check if a trajectory is collision free. More...

rcl_interfaces::msg::SetParametersResult	validateParameterUpdatesCallback (const std::vector< rclcpp::Parameter > &parameters)
	Validate incoming parameter updates before applying them. This callback is triggered when one or more parameters are about to be updated. It checks the validity of parameter values and rejects updates that would lead to invalid or inconsistent configurations. More...

void	updateParametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Apply parameter updates after validation This callback is executed when parameters have been successfully updated. It updates the internal configuration of the node with the new parameter values. More...

void	configureCollisionChecker (const nav2::LifecycleNode::SharedPtr &node, std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
	Configure the collision checker. More...

Protected Attributes
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr	post_set_params_handler_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	on_set_params_handler_

std::mutex	dynamic_params_lock_

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

rclcpp::Clock::SharedPtr	clock_

std::unique_ptr< nav2_graceful_controller::SmoothControlLaw >	control_law_

double	k_phi_

double	k_delta_

double	beta_

double	lambda_

double	slowdown_radius_

double	deceleration_max_

double	v_linear_min_

double	v_linear_max_

double	v_angular_max_

double	rotate_to_heading_angular_vel_

double	rotate_to_heading_max_angular_accel_

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

bool	use_collision_detection_

double	projection_time_

double	simulation_time_step_

double	dock_collision_threshold_

double	transform_tolerance_

nav2::TransformBuffer::SharedPtr	tf2_buffer_

std::unique_ptr< nav2_costmap_2d::CostmapSubscriber >	costmap_sub_

std::unique_ptr< nav2_costmap_2d::FootprintSubscriber >	footprint_sub_

std::shared_ptr< nav2_costmap_2d::CostmapTopicCollisionChecker >	collision_checker_

std::string	fixed_frame_

std::string	base_frame_

Detailed Description

Default control law for approaching a dock target.

Definition at line 39 of file controller.hpp.

Constructor & Destructor Documentation

◆ Controller()

opennav_docking::Controller::Controller	(	const nav2::LifecycleNode::SharedPtr &	node,
		nav2::TransformBuffer::SharedPtr	tf,
		std::string	fixed_frame,
		std::string	base_frame
	)

Create a controller instance. Configure ROS 2 parameters.

Parameters

node	Lifecycle node
tf	tf2_ros TF buffer
fixed_frame	Fixed frame
base_frame	Robot base frame

Definition at line 30 of file controller.cpp.

References configureCollisionChecker(), updateParametersCallback(), and validateParameterUpdatesCallback().

Here is the call graph for this function:

Member Function Documentation

◆ computeRotateToHeadingCommand()

geometry_msgs::msg::Twist opennav_docking::Controller::computeRotateToHeadingCommand	(	const double &	angular_distance_to_heading,
		const geometry_msgs::msg::Twist &	current_velocity,
		const double &	dt
	)

Perform a command for in-place rotation.

Parameters

angular_distance_to_heading	Angular distance to goal.
current_velocity	Current angular velocity.
dt	Control loop duration [s].

Returns: TwistStamped command for in-place rotation.

Definition at line 107 of file controller.cpp.

◆ computeVelocityCommand()

bool opennav_docking::Controller::computeVelocityCommand	(	const geometry_msgs::msg::Pose &	pose,
		geometry_msgs::msg::Twist &	cmd,
		bool	is_docking,
		bool	backward = `false`
	)

Compute a velocity command using control law.

Parameters

pose	Target pose, in robot centric coordinates.
cmd	Command velocity.
is_docking	If true, robot is docking. If false, robot is undocking.
backward	If true, robot will drive backwards to goal.

Returns: True if command is valid, false otherwise.

Definition at line 98 of file controller.cpp.

References isTrajectoryCollisionFree().

Here is the call graph for this function:

◆ configureCollisionChecker()

void opennav_docking::Controller::configureCollisionChecker	(	const nav2::LifecycleNode::SharedPtr &	node,
		std::string	costmap_topic,
		std::string	footprint_topic,
		double	transform_tolerance
	)

protected

Configure the collision checker.

Parameters

node	Lifecycle node
costmap_topic	Costmap topic
footprint_topic	Footprint topic
transform_tolerance	Transform tolerance

Definition at line 205 of file controller.cpp.

Referenced by Controller().

Here is the caller graph for this function:

◆ isTrajectoryCollisionFree()

bool opennav_docking::Controller::isTrajectoryCollisionFree	(	const geometry_msgs::msg::Pose &	target_pose,
		bool	is_docking,
		bool	backward = `false`
	)

protected

Check if a trajectory is collision free.

Parameters

target_pose	Target pose, in robot centric coordinates.
is_docking	If true, robot is docking. If false, robot is undocking.
backward	If true, robot will drive backwards to goal.

Returns: True if trajectory is collision free.

Definition at line 132 of file controller.cpp.

Referenced by computeVelocityCommand().

Here is the caller graph for this function:

◆ updateParametersCallback()

void opennav_docking::Controller::updateParametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

Apply parameter updates after validation This callback is executed when parameters have been successfully updated. It updates the internal configuration of the node with the new parameter values.

Parameters

parameters List of parameters that have been updated.

Definition at line 241 of file controller.cpp.

Referenced by Controller().

Here is the caller graph for this function:

◆ validateParameterUpdatesCallback()

rcl_interfaces::msg::SetParametersResult opennav_docking::Controller::validateParameterUpdatesCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

Validate incoming parameter updates before applying them. This callback is triggered when one or more parameters are about to be updated. It checks the validity of parameter values and rejects updates that would lead to invalid or inconsistent configurations.

Parameters

parameters List of parameters that are being updated.

Returns: rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.

Definition at line 216 of file controller.cpp.

Referenced by Controller().

Here is the caller graph for this function:

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
nav2_docking/opennav_docking/src/controller.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Controller()

Member Function Documentation

◆ computeRotateToHeadingCommand()

◆ computeVelocityCommand()

◆ configureCollisionChecker()

◆ isTrajectoryCollisionFree()

◆ updateParametersCallback()

◆ validateParameterUpdatesCallback()