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

#include <diff_drive_pose_controller.hpp>

Inheritance diagram for yocs::DiffDrivePoseController:

Public Member Functions
	DiffDrivePoseController (std::string name, double v_max, double w_max, double dist_thres=0.01, double orient_thres=0.02, double dist_eps=0.01 0.2, double orient_eps=0.02 0.2, double orientation_gain=0.3, double k_1=1.0, double k_2=3.0, double beta=0.4, double lambda=2.0, double v_min=0.01, double v_min_movement=0.01, double w_min_movement=0.01)

virtual void	getControlOutput (double &v, double &w)
	Get controller result / output after spinning. More...

virtual bool	init ()
	unused, overwrite if inherited (and needed) More...

void	setCurrentLimits (double v_min, double w_min, double v_max, double w_max)

virtual void	setInput (double distance_to_goal, double delta, double theta)
	Set input of controller. Should be called before each spinOnce. More...

void	setVerbosity (const bool &verbose)

virtual bool	step ()
	Execute one controller step. More...

virtual	~DiffDrivePoseController ()

Public Member Functions inherited from yocs::Controller
	Controller ()

bool	disable ()

bool	enable ()

bool	getState ()

virtual void	spin ()

virtual	~Controller ()

Protected Member Functions
virtual void	calculateControls ()
	Calculates the controls with the set variables (speed, goal etc.) More...

virtual void	controlOrientation (double angle_difference)

virtual void	controlPose ()

virtual double	enforceMinMax (double &value, double min, double max)
	Enforce value to be between min and max. More...

virtual double	enforceMinVelocity (double value, double min)
	Enforce the minimum velocity. More...

virtual void	onGoalReached ()
	Gets executed when goal is reached, use in child class. More...

Protected Attributes
double	beta_

double	cur_
	path to goal curvature More...

double	delta_
	current heading of the base [rad] More...

double	dist_eps_
	Error in distance above which pose is considered different. More...

double	dist_thres_
	lower bound for the distance (v = 0) More...

double	k_1_
	constant factor determining the ratio of the rate of change in theta to the rate of change in r More...

double	k_2_
	constant factor applied to the heading error feedback More...

double	lambda_

std::string	name_

double	orient_eps_
	Error in orientation above which pose is considered different. More...

double	orient_thres_
	lower bound for the orientation (w = 0) More...

double	orientation_gain_
	p gain for angle controller More...

bool	pose_reached_
	True, if pose has been reached (v == 0, w == 0) More...

double	r_
	distance to pose goal [m] More...

double	theta_
	direction of the pose goal [rad] More...

double	v_
	linear base velocity [m/s] More...

double	v_max_
	maximum linear base velocity [m/s] More...

double	v_min_
	(current) minimum linear base velocity [m/s] More...

double	v_min_movement_
	minimum linear base velocity at which we still move [m/s] More...

bool	verbose_
	Enable or disable ros messages. More...

double	w_
	angular base velocity [rad/s] More...

double	w_max_
	maximum angular base velocity [rad/s] More...

double	w_min_
	(current) minimum angular base velocity [rad/s] More...

double	w_min_movement_
	minimum angular base velocity at which we still move [rad/s] More...

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 35 of file diff_drive_pose_controller.hpp.

Constructor & Destructor Documentation

yocs::DiffDrivePoseController::DiffDrivePoseController	(	std::string	name,
		double	v_max,
		double	w_max,
		double	dist_thres = `0.01`,
		double	orient_thres = `0.02`,
		double	dist_eps = `0.01 * 0.2`,
		double	orient_eps = `0.02 * 0.2`,
		double	orientation_gain = `0.3`,
		double	k_1 = `1.0`,
		double	k_2 = `3.0`,
		double	beta = `0.4`,
		double	lambda = `2.0`,
		double	v_min = `0.01`,
		double	v_min_movement = `0.01`,
		double	w_min_movement = `0.01`
	)

Definition at line 10 of file diff_drive_pose_controller.cpp.

virtual yocs::DiffDrivePoseController::~DiffDrivePoseController ( )

inlinevirtual

Definition at line 44 of file diff_drive_pose_controller.hpp.

Member Function Documentation

void yocs::DiffDrivePoseController::calculateControls ( )

protectedvirtual

Calculates the controls with the set variables (speed, goal etc.)

Definition at line 63 of file diff_drive_pose_controller.cpp.

void yocs::DiffDrivePoseController::controlOrientation ( double angle_difference )

protectedvirtual

Definition at line 127 of file diff_drive_pose_controller.cpp.

void yocs::DiffDrivePoseController::controlPose ( )

protectedvirtual

Definition at line 102 of file diff_drive_pose_controller.cpp.

double yocs::DiffDrivePoseController::enforceMinMax	(	double &	value,
		double	min,
		double	max
	)

protectedvirtual

Enforce value to be between min and max.

Parameters

v	value
min	minimum speed
max	maximum speed

Returns: bounded velocity

Definition at line 137 of file diff_drive_pose_controller.cpp.

double yocs::DiffDrivePoseController::enforceMinVelocity	(	double	value,
		double	min
	)

protectedvirtual

Enforce the minimum velocity.

Parameters

v	velocity
min	minimum velocity

Returns: bounded velocity

Definition at line 142 of file diff_drive_pose_controller.cpp.

void yocs::DiffDrivePoseController::getControlOutput	(	double &	v,
		double &	w
	)

virtual

Get controller result / output after spinning.

Parameters

v	linear velocity out variable
w	angular velocity out variable

Definition at line 162 of file diff_drive_pose_controller.cpp.

virtual bool yocs::DiffDrivePoseController::init ( )

inlinevirtual

unused, overwrite if inherited (and needed)

Returns: true

Implements yocs::Controller.

Reimplemented in yocs::DiffDrivePoseControllerROS.

Definition at line 54 of file diff_drive_pose_controller.hpp.

virtual void yocs::DiffDrivePoseController::onGoalReached ( )

inlineprotectedvirtual

Gets executed when goal is reached, use in child class.

Reimplemented in yocs::DiffDrivePoseControllerROS.

Definition at line 112 of file diff_drive_pose_controller.hpp.

void yocs::DiffDrivePoseController::setCurrentLimits	(	double	v_min,
		double	w_min,
		double	v_max,
		double	w_max
	)

Definition at line 49 of file diff_drive_pose_controller.cpp.

void yocs::DiffDrivePoseController::setInput	(	double	distance_to_goal,
		double	delta,
		double	theta
	)

virtual

Set input of controller. Should be called before each spinOnce.

Parameters

distance_to_goal	distance to goal [m]
delta	heading of the robot [rad]
theta	angle difference between heading and goal [m]

Definition at line 42 of file diff_drive_pose_controller.cpp.

void yocs::DiffDrivePoseController::setVerbosity ( const bool & verbose )

inline

Definition at line 48 of file diff_drive_pose_controller.hpp.

bool yocs::DiffDrivePoseController::step ( )

virtual

Execute one controller step.

Returns: true, if goal pose is reached

Definition at line 57 of file diff_drive_pose_controller.cpp.

Member Data Documentation

double yocs::DiffDrivePoseController::beta_

protected

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

Definition at line 152 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::cur_

protected

path to goal curvature

Definition at line 143 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::delta_

protected

current heading of the base [rad]

Definition at line 122 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::dist_eps_

protected

Error in distance above which pose is considered different.

Definition at line 169 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::dist_thres_

protected

lower bound for the distance (v = 0)

Definition at line 163 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::k_1_

protected

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

Definition at line 145 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::k_2_

protected

constant factor applied to the heading error feedback

Definition at line 147 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::lambda_

protected

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 157 of file diff_drive_pose_controller.hpp.

std::string yocs::DiffDrivePoseController::name_

protected

Definition at line 118 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::orient_eps_

protected

Error in orientation above which pose is considered different.

Definition at line 171 of file diff_drive_pose_controller.hpp.

double yocs::DiffDrivePoseController::orient_thres_

protected

lower bound for the orientation (w = 0)

Definition at line 165 of file diff_drive_pose_controller.hpp.