A controller for driving a differential drive base to a pose goal or along a path specified by multiple poses. More...

#include <controller.hpp>

Inheritance diagram for yocs::DiffDrivePoseController:

Public Member Functions
	DiffDrivePoseController (ros::NodeHandle &nh, std::string &name)
bool	init ()
	Set-up necessary publishers/subscribers and variables.
void	spinOnce ()
	Calculates velocity commands to move the diff-drive base to the (next) pose goal.
virtual	~DiffDrivePoseController ()
Private Member Functions
double	boundRange (double v, double min, double max)
	Bounding range of velocity.
void	controlMaxVelCB (const std_msgs::Float32ConstPtr msg)
	Callback for updating the controller's maximum linear control velocity.
void	disableCB (const std_msgs::EmptyConstPtr msg)
	Callback for disabling the controler.
void	enableCB (const std_msgs::StringConstPtr msg)
	Callback for enabling the controler.
void	getControlOutput ()
	Calculates the controller output based on the current pose difference.
bool	getPoseDiff ()
	Determines the pose difference in polar coordinates.
void	setControlOutput ()
	Sends out the new velocity commands for the left and right wheel based on the current controller output.
Private Attributes
std::string	base_frame_name_
	frame name of the base
double	beta_
ros::Publisher	command_velocity_publisher_
	publisher for sending out base velocity commands
ros::Subscriber	control_velocity_subscriber_
	subscriber for setting the controller's linear velocity
double	cur_
	path to goal curvature
double	delta_
	current heading of the base [rad]
ros::Subscriber	disable_controller_subscriber_
	subscriber for disabling the controller
double	dist_eps_
	Error in distance above which pose is considered differen.
double	dist_thres_
	lower bound for the distance (v = 0)
ros::Subscriber	enable_controller_subscriber_
	subscriber for enabling the controller
std::string	goal_frame_name_
	frame name of the goal (pose)
double	k_1_
	constant factor determining the ratio of the rate of change in theta to the rate of change in r
double	k_2_
	constant factor applied to the heading error feedback
double	lambda_
std::string	name_
ros::NodeHandle	nh_
double	orient_eps_
	Error in orientation above which pose is considered differen.
double	orient_thres_
	lower bound for the orientation (w = 0)
bool	pose_reached_
	True, if pose has been reached (v == 0, w == 0)
ros::Publisher	pose_reached_publisher_
	publishes the status of the goal pose tracking
double	r_
	distance to pose goal [m]
tf::StampedTransform	tf_goal_pose_rel_
	transform for the goal pose relative to the base pose
tf::TransformListener	tf_listener_
	tf used to get goal pose relative to the base pose
double	theta_
	direction of the pose goal [rad]
double	v_
	linear base velocity [m/s]
double	v_max_
	maximum linear base velocity [m/s]
double	v_min_
	minimum linear base velocity [m/s]
double	w_
	angular base velocity [rad/s]
double	w_max_
	maximum angular base velocity [rad/s]
double	w_min_
	minimum angular base velocity [rad/s]

Detailed Description

A controller for driving a differential drive base to a pose goal or along a path specified by multiple poses.

This controller implements a control law drives a differental drive base towards a planar pose goal, i.e. 2D position (x,y) + 1D orientation (theta). It also allows path following by specifying multiple pose goals. The control law contains a transition strategy, which insures that the base moves through each pose and transitions smoothly to the next pose goal.

This controller is an implementation of control law based on the following work: {DBLP:conf/icra/ParkK11, author = {Jong Jin Park and Benjamin Kuipers}, title = {A smooth control law for graceful motion of differential wheeled mobile robots in 2D environment}, booktitle = {ICRA}, year = {2011}, pages = {4896-4902}, ee = {http://dx.doi.org/10.1109/ICRA.2011.5980167}, crossref = {DBLP:conf/icra/2011}, bibsource = {DBLP, http://dblp.uni-trier.de} }

This controller can be enabled/disabled.

Definition at line 80 of file controller.hpp.

Constructor & Destructor Documentation

yocs::DiffDrivePoseController::DiffDrivePoseController	(	ros::NodeHandle &	nh,
		std::string &	name
	)		`[inline]`

Definition at line 83 of file controller.hpp.

virtual yocs::DiffDrivePoseController::~DiffDrivePoseController ( ) [inline, virtual]

Definition at line 86 of file controller.hpp.

Member Function Documentation

double yocs::DiffDrivePoseController::boundRange	(	double	v,
		double	min,
		double	max
	)		`[private]`

Bounding range of velocity.

Parameters:

v	velocity
min	minimum speed
max	maximum speed

Returns:: bounded velocity

Definition at line 416 of file controller.hpp.

void yocs::DiffDrivePoseController::controlMaxVelCB ( const std_msgs::Float32ConstPtr msg ) [private]

Callback for updating the controller's maximum linear control velocity.

Parameters:

msg	maximum linear control velocity

Definition at line 456 of file controller.hpp.

void yocs::DiffDrivePoseController::disableCB ( const std_msgs::EmptyConstPtr msg ) [private]

Callback for disabling the controler.

Parameters:

msg	empty message

Definition at line 475 of file controller.hpp.

void yocs::DiffDrivePoseController::enableCB ( const std_msgs::StringConstPtr msg ) [private]

Callback for enabling the controler.

Parameters:

msg	goal frame name

Definition at line 462 of file controller.hpp.

void yocs::DiffDrivePoseController::getControlOutput ( ) [private]

Calculates the controller output based on the current pose difference.

Definition at line 371 of file controller.hpp.

bool yocs::DiffDrivePoseController::getPoseDiff ( ) [private]

Determines the pose difference in polar coordinates.

Definition at line 343 of file controller.hpp.

bool yocs::DiffDrivePoseController::init ( ) [virtual]

Set-up necessary publishers/subscribers and variables.

Returns:: true, if successful

Implements yocs::Controller.

Definition at line 212 of file controller.hpp.

void yocs::DiffDrivePoseController::setControlOutput ( ) [private]

Sends out the new velocity commands for the left and right wheel based on the current controller output.

Definition at line 445 of file controller.hpp.

void yocs::DiffDrivePoseController::spinOnce ( )

Calculates velocity commands to move the diff-drive base to the (next) pose goal.

Definition at line 316 of file controller.hpp.

Member Data Documentation

std::string yocs::DiffDrivePoseController::base_frame_name_ [private]

frame name of the base

Definition at line 207 of file controller.hpp.

double yocs::DiffDrivePoseController::beta_ [private]

constant factor for the curvature-based velocity rule determines how fast the velocity drops when the curvature increases

Definition at line 185 of file controller.hpp.

ros::Publisher yocs::DiffDrivePoseController::command_velocity_publisher_ [private]

publisher for sending out base velocity commands

Definition at line 152 of file controller.hpp.

ros::Subscriber yocs::DiffDrivePoseController::control_velocity_subscriber_ [private]

subscriber for setting the controller's linear velocity

Definition at line 150 of file controller.hpp.

double yocs::DiffDrivePoseController::cur_ [private]

path to goal curvature

Definition at line 176 of file controller.hpp.

double yocs::DiffDrivePoseController::delta_ [private]

current heading of the base [rad]

Definition at line 162 of file controller.hpp.

ros::Subscriber yocs::DiffDrivePoseController::disable_controller_subscriber_ [private]

subscriber for disabling the controller

Definition at line 148 of file controller.hpp.

double yocs::DiffDrivePoseController::dist_eps_ [private]

Error in distance above which pose is considered differen.

Definition at line 198 of file controller.hpp.

double yocs::DiffDrivePoseController::dist_thres_ [private]

lower bound for the distance (v = 0)

Definition at line 192 of file controller.hpp.

ros::Subscriber yocs::DiffDrivePoseController::enable_controller_subscriber_ [private]

subscriber for enabling the controller

Definition at line 146 of file controller.hpp.

std::string yocs::DiffDrivePoseController::goal_frame_name_ [private]

frame name of the goal (pose)

Definition at line 209 of file controller.hpp.

double yocs::DiffDrivePoseController::k_1_ [private]

constant factor determining the ratio of the rate of change in theta to the rate of change in r

Definition at line 178 of file controller.hpp.

double yocs::DiffDrivePoseController::k_2_ [private]

constant factor applied to the heading error feedback

Definition at line 180 of file controller.hpp.

double yocs::DiffDrivePoseController::lambda_ [private]

constant factor for the curvature-based velocity rule determines the sharpness of the curve: higher lambda -> bigger drop in short term, smaller in the long term

Definition at line 190 of file controller.hpp.

std::string yocs::DiffDrivePoseController::name_ [private]

Definition at line 142 of file controller.hpp.

ros::NodeHandle yocs::DiffDrivePoseController::nh_ [private]

Definition at line 141 of file controller.hpp.

double yocs::DiffDrivePoseController::orient_eps_ [private]

Error in orientation above which pose is considered differen.

Definition at line 200 of file controller.hpp.

double yocs::DiffDrivePoseController::orient_thres_ [private]

lower bound for the orientation (w = 0)

Definition at line 194 of file controller.hpp.

bool yocs::DiffDrivePoseController::pose_reached_ [private]