#include <VelocityControl.hpp>

Public Member Functions
void	onInit ()
void	start ()
void	step ()
	VelocityControl ()
Private Types
enum	{ u = 0, v, w, p, q, r }
enum	{ X = 0, Y, Z, K, M, N }
enum	{ alpha = 0, beta, betaa }
enum	{ Kp = 0, Ki, Kd, Kt }
enum	{ disableAxis = 0, manualAxis = 1, controlAxis = 2, identAxis = 3, directAxis = 4 }
enum	{ numcnt = 6 }
Private Member Functions
void	dynrec_cb (navcon_msgs::VelConConfig &config, uint32_t level)
bool	handleEnableControl (navcon_msgs::EnableControl::Request &req, navcon_msgs::EnableControl::Response &resp)
void	handleEstimates (const auv_msgs::NavSts::ConstPtr &estimate)
void	handleExt (const auv_msgs::BodyForceReq::ConstPtr &tau)
void	handleManual (const sensor_msgs::Joy::ConstPtr &joy)
void	handleMeasurement (const auv_msgs::NavSts::ConstPtr &meas)
void	handleModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update)
void	handleReference (const auv_msgs::BodyVelocityReq::ConstPtr &ref)
bool	handleServerConfig (navcon_msgs::ConfigureVelocityController::Request &req, navcon_msgs::ConfigureVelocityController::Response &resp)
void	handleWindup (const auv_msgs::BodyForceReq::ConstPtr &tau)
void	initialize_controller ()
void	safetyTest ()
void	updateDynRecConfig ()
Private Attributes
boost::array< int32_t, r+1 >	axis_control
navcon_msgs::VelConConfig	config
PIDBase	controller [r+1]
bool	doSafetyTest
ros::ServiceServer	enableControl
bool	externalIdent
ros::ServiceServer	highLevelSelect
ros::Subscriber	identExt
double	joy_scale
ros::Time	lastEst
ros::Time	lastMan
ros::Time	lastMeas
ros::Time	lastRef
ros::Subscriber	manualIn
ros::Subscriber	measSub
double	measurement [r+1]
ros::Subscriber	modelUpdate
ros::NodeHandle	nh
ros::NodeHandle	ph
dynamic_reconfigure::Server < navcon_msgs::VelConConfig >	server
boost::recursive_mutex	serverMux
ros::Subscriber	stateHat
bool	suspend_axis [r+1]
ros::Subscriber	tauAch
ros::Publisher	tauAchW
float	tauExt [N+1]
float	tauManual [N+1]
ros::Publisher	tauOut
double	timeout
double	Ts
bool	use_gvel
ros::Subscriber	velocityRef
Static Private Attributes
static const std::string	dofName [r+1]

Detailed Description

The class contains the implementation of the velocity controller, manual control manager and model identification.

Todo:

Create the merge subclass for velocity controller via external TAU

Implement the modes (manual, external(control), disable, tracking)

Switch to vector operation rather than repeated statements

Add model retuning

Separate dynamic reconfigure option to optional subclass

Remove other modes

Remove everything related to identification and replace it via the merger

Deprecate measurements subscription

Reduce the internal tauAch code calculation

Switch velocity controller to async mode ?

Add feed forward for disturbances ?

Definition at line 71 of file VelocityControl.hpp.

Member Enumeration Documentation

anonymous enum [private]

Enumerator:

u
v
w
p
q
r

Definition at line 73 of file VelocityControl.hpp.

anonymous enum [private]

Enumerator:

X
Y
Z
K
M
N

Definition at line 74 of file VelocityControl.hpp.

anonymous enum [private]

Enumerator:

alpha
beta
betaa

Definition at line 75 of file VelocityControl.hpp.

anonymous enum [private]

Enumerator:

Kp
Ki
Kd
Kt

Definition at line 76 of file VelocityControl.hpp.

anonymous enum [private]

Enumerator:

disableAxis
manualAxis
controlAxis
identAxis
directAxis

Definition at line 77 of file VelocityControl.hpp.

anonymous enum [private]

Enumerator:

numcnt

Definition at line 82 of file VelocityControl.hpp.

Constructor & Destructor Documentation

VelocityControl::VelocityControl ( )

Main constructor

Definition at line 58 of file VelocityControl.cpp.

Member Function Documentation

void VelocityControl::dynrec_cb	(	navcon_msgs::VelConConfig &	config,
		uint32_t	level
	)		`[private]`

Dynamic reconfigure callback.

Definition at line 239 of file VelocityControl.cpp.

bool VelocityControl::handleEnableControl	(	navcon_msgs::EnableControl::Request &	req,
		navcon_msgs::EnableControl::Response &	resp
	)		`[private]`

Handle the enable control request.

Definition at line 128 of file VelocityControl.cpp.

void VelocityControl::handleEstimates ( const auv_msgs::NavSts::ConstPtr & estimate ) [private]

Handle incoming estimates message.

Todo:: Check if this is needed ?

Definition at line 298 of file VelocityControl.cpp.

void VelocityControl::handleExt ( const auv_msgs::BodyForceReq::ConstPtr & tau ) [private]

Handle external identification input.

Definition at line 292 of file VelocityControl.cpp.

void VelocityControl::handleManual ( const sensor_msgs::Joy::ConstPtr & joy ) [private]

Handle incoming estimates message.

Definition at line 193 of file VelocityControl.cpp.

void VelocityControl::handleMeasurement ( const auv_msgs::NavSts::ConstPtr & meas ) [private]

Handle incoming estimates message.

Definition at line 334 of file VelocityControl.cpp.

void VelocityControl::handleModelUpdate ( const navcon_msgs::ModelParamsUpdate::ConstPtr & update ) [private]

Handle incoming estimates message.

Definition at line 220 of file VelocityControl.cpp.

void VelocityControl::handleReference ( const auv_msgs::BodyVelocityReq::ConstPtr & ref ) [private]

Handle incoming reference message.

Definition at line 172 of file VelocityControl.cpp.

bool VelocityControl::handleServerConfig	(	navcon_msgs::ConfigureVelocityController::Request &	req,
		navcon_msgs::ConfigureVelocityController::Response &	resp
	)		`[private]`

Handle server request for configuration.

Definition at line 156 of file VelocityControl.cpp.

void VelocityControl::handleWindup ( const auv_msgs::BodyForceReq::ConstPtr & tau ) [private]

Handle incoming estimates message.

Definition at line 252 of file VelocityControl.cpp.

void VelocityControl::initialize_controller ( ) [private]

Initialize the controller parameters etc.

Definition at line 488 of file VelocityControl.cpp.

void VelocityControl::onInit ( )

Initialize and setup controller.

Definition at line 74 of file VelocityControl.cpp.

void VelocityControl::safetyTest ( ) [private]

The safety test.

Definition at line 351 of file VelocityControl.cpp.

void VelocityControl::start ( )

Start the controller loop.

Definition at line 476 of file VelocityControl.cpp.

void VelocityControl::step ( )

Performs one iteration.

Definition at line 375 of file VelocityControl.cpp.

void VelocityControl::updateDynRecConfig ( ) [private]

Update the dynamic reconfiguration settings.

Definition at line 140 of file VelocityControl.cpp.

Member Data Documentation

boost::array<int32_t,r+1> labust::control::VelocityControl::axis_control [private]

Enable/disable controllers, external windup flag.

Definition at line 189 of file VelocityControl.hpp.

navcon_msgs::VelConConfig labust::control::VelocityControl::config [private]

The dynamic reconfigure parameters.

Definition at line 214 of file VelocityControl.hpp.

PIDBase labust::control::VelocityControl::controller[r+1] [private]

The velocity controllers.

Definition at line 173 of file VelocityControl.hpp.

const std::string VelocityControl::dofName [static, private]

Initial value:

{"Surge","Sway","Heave","Roll","Pitch","Yaw"}

Variable access helper mass.

Definition at line 222 of file VelocityControl.hpp.

bool labust::control::VelocityControl::doSafetyTest [private]

Safety test flag.

Definition at line 226 of file VelocityControl.hpp.

ros::ServiceServer labust::control::VelocityControl::enableControl [private]

Definition at line 210 of file VelocityControl.hpp.

bool labust::control::VelocityControl::externalIdent [private]

Definition at line 193 of file VelocityControl.hpp.

ros::ServiceServer labust::control::VelocityControl::highLevelSelect [private]

High level controller service.

Definition at line 210 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::identExt [private]

Definition at line 206 of file VelocityControl.hpp.

double labust::control::VelocityControl::joy_scale [private]

Joystick scaling.

Definition at line 185 of file VelocityControl.hpp.

ros::Time labust::control::VelocityControl::lastEst [private]

Definition at line 160 of file VelocityControl.hpp.

ros::Time labust::control::VelocityControl::lastMan [private]

Definition at line 160 of file VelocityControl.hpp.

ros::Time labust::control::VelocityControl::lastMeas [private]

Definition at line 160 of file VelocityControl.hpp.

ros::Time labust::control::VelocityControl::lastRef [private]

Last message times.

Definition at line 160 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::manualIn [private]

Definition at line 206 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::measSub [private]

Definition at line 206 of file VelocityControl.hpp.

double labust::control::VelocityControl::measurement[r+1] [private]

The mesurement needed for identification.

Definition at line 177 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::modelUpdate [private]

Definition at line 206 of file VelocityControl.hpp.

ros::NodeHandle labust::control::VelocityControl::nh [private]

The ROS node handles.

Definition at line 156 of file VelocityControl.hpp.

ros::NodeHandle labust::control::VelocityControl::ph [private]

Definition at line 156 of file VelocityControl.hpp.

dynamic_reconfigure::Server<navcon_msgs::VelConConfig> labust::control::VelocityControl::server [private]

The dynamic reconfigure server.

Definition at line 218 of file VelocityControl.hpp.

boost::recursive_mutex labust::control::VelocityControl::serverMux [private]

Dynamic reconfigure server.

Definition at line 197 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::stateHat [private]

Definition at line 206 of file VelocityControl.hpp.

bool labust::control::VelocityControl::suspend_axis[r+1] [private]

Timeout management.

Definition at line 193 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::tauAch [private]

Definition at line 206 of file VelocityControl.hpp.

ros::Publisher labust::control::VelocityControl::tauAchW [private]

Definition at line 202 of file VelocityControl.hpp.

float labust::control::VelocityControl::tauExt[N+1] [private]

Definition at line 181 of file VelocityControl.hpp.

float labust::control::VelocityControl::tauManual[N+1] [private]

Joystick message.

Definition at line 181 of file VelocityControl.hpp.

ros::Publisher labust::control::VelocityControl::tauOut [private]

The publisher of the TAU message.

Definition at line 202 of file VelocityControl.hpp.

double labust::control::VelocityControl::timeout [private]

Timeout

Definition at line 164 of file VelocityControl.hpp.

double labust::control::VelocityControl::Ts [private]

Definition at line 185 of file VelocityControl.hpp.

bool labust::control::VelocityControl::use_gvel [private]

Definition at line 193 of file VelocityControl.hpp.

ros::Subscriber labust::control::VelocityControl::velocityRef [private]

The subscribed topics.

Definition at line 206 of file VelocityControl.hpp.

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

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation