The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread running the plugin in a cycle to move the robot. An internal state is saved and will be pulled by server, to monitor controller execution. Due to a state change it wakes up all threads connected to the condition variable. More...

#include <abstract_controller_execution.h>

Inheritance diagram for mbf_abstract_nav::AbstractControllerExecution:

[legend]

List of all members.

Public Types
enum	ControllerState { INITIALIZED, STARTED, PLANNING, NO_PLAN, MAX_RETRIES, PAT_EXCEEDED, EMPTY_PLAN, INVALID_PLAN, NO_LOCAL_CMD, GOT_LOCAL_CMD, ARRIVED_GOAL, CANCELED, STOPPED, INTERNAL_ERROR }
	Internal states. More...
typedef boost::shared_ptr < AbstractControllerExecution >	Ptr
Public Member Functions
	AbstractControllerExecution (const std::string name, const mbf_abstract_core::AbstractController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const MoveBaseFlexConfig &config, boost::function< void()> setup_fn, boost::function< void()> cleanup_fn)
	Constructor.
virtual bool	cancel ()
	Cancel the planner execution. This calls the cancel method of the planner plugin. This could be useful if the computation takes too much time.
ros::Time	getLastPluginCallTime ()
	Returns the time of the last plugin call.
ControllerState	getState ()
	Return the current state of the controller execution. Thread communication safe.
geometry_msgs::TwistStamped	getVelocityCmd ()
	Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method. Note that it doesn't need to be a valid command sent to the robot, as we report also failed calls to the plugin on controller action feedback.
bool	isMoving ()
	Returns whether the robot should normally move or not. True if the controller seems to work properly.
bool	isPatienceExceeded ()
	Checks whether the patience duration time has been exceeded, ot not.
void	reconfigure (const MoveBaseFlexConfig &config)
	Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconfigure to reconfigure the current state.
bool	setControllerFrequency (double frequency)
	Sets the controller frequency.
void	setNewPlan (const std::vector< geometry_msgs::PoseStamped > &plan)
	Sets a new plan to the controller execution.
virtual bool	start ()
	Starts the controller, a valid plan should be given in advance.
virtual	~AbstractControllerExecution ()
	Destructor.
Static Public Attributes
static const double	DEFAULT_CONTROLLER_FREQUENCY = 100.0
Protected Member Functions
virtual uint32_t	computeVelocityCmd (const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, geometry_msgs::TwistStamped &vel_cmd, std::string &message)
	Request plugin for a new velocity command, given the current position, orientation, and velocity of the robot. We use this virtual method to give concrete implementations as move_base the chance to override it and do additional stuff, for example locking the costmap.
virtual void	run ()
	The main run method, a thread will execute this method. It contains the main controller execution loop.
void	setVelocityCmd (const geometry_msgs::TwistStamped &vel_cmd_stamped)
	Sets the velocity command, to make it available for another thread.
Protected Attributes
mbf_abstract_core::AbstractController::Ptr	controller_
	the local planer to calculate the velocity command
ros::Time	last_call_time_
	The current cycle start time of the last cycle run. Will by updated each cycle.
int	max_retries_
	The maximum number of retries.
ros::Duration	patience_
	The time / duration of patience, before changing the state.
std::string	plugin_name_
	the name of the loaded plugin
ros::Time	start_time_
	The time the controller has been started.
const TFPtr &	tf_listener_ptr
	shared pointer to the shared tf listener
Private Member Functions
bool	computeRobotPose ()
	Computes the robot pose;.
std::vector < geometry_msgs::PoseStamped >	getNewPlan ()
	Gets the new available plan. This method is thread safe.
bool	hasNewPlan ()
	Returns true if a new plan is available, false otherwise! A new plan is set by another thread!
void	publishZeroVelocity ()
bool	reachedGoalCheck ()
	Checks whether the goal has been reached in the range of tolerance or not.
void	setState (ControllerState state)
	Sets the controller state. This method makes the communication of the state thread safe.
Private Attributes
double	angle_tolerance_
	angle tolerance to the given goal pose
boost::chrono::microseconds	calling_duration_
	the duration which corresponds with the controller frequency.
boost::mutex	configuration_mutex_
	dynamic reconfigure config mutex, thread safe param reading and writing
ros::Publisher	current_goal_pub_
	publisher for the current goal
double	dist_tolerance_
	distance tolerance to the given goal pose
std::string	global_frame_
	the global frame the robot is controlling in.
boost::mutex	lct_mtx_
	mutex to handle safe thread communication for the last plugin call time
bool	mbf_tolerance_check_
	whether move base flex should check for the goal tolerance or not.
bool	moving_
	main controller loop variable, true if the controller is running, false otherwise
bool	new_plan_
	true, if a new plan is available. See hasNewPlan()!
std::vector < geometry_msgs::PoseStamped >	plan_
	the last set plan which is currently processed by the controller
boost::mutex	plan_mtx_
	mutex to handle safe thread communication for the current plan
std::string	robot_frame_
	the frame of the robot, which will be used to determine its position.
geometry_msgs::PoseStamped	robot_pose_
	current robot pose;
AbstractControllerExecution::ControllerState	state_
	the current controller state
boost::mutex	state_mtx_
	mutex to handle safe thread communication for the current value of the state
double	tf_timeout_
	time before a timeout used for tf requests
boost::mutex	vel_cmd_mtx_
	mutex to handle safe thread communication for the current velocity command
geometry_msgs::TwistStamped	vel_cmd_stamped_
	the last calculated velocity command
ros::Publisher	vel_pub_
	publisher for the current velocity command

Detailed Description

The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread running the plugin in a cycle to move the robot. An internal state is saved and will be pulled by server, to monitor controller execution. Due to a state change it wakes up all threads connected to the condition variable.

Definition at line 76 of file abstract_controller_execution.h.

Member Typedef Documentation

typedef boost::shared_ptr<AbstractControllerExecution > mbf_abstract_nav::AbstractControllerExecution::Ptr

Definition at line 82 of file abstract_controller_execution.h.

Member Enumeration Documentation

enum mbf_abstract_nav::AbstractControllerExecution::ControllerState

Internal states.

Enumerator:

INITIALIZED	Controller has been initialized successfully.
STARTED	Controller has been started.
PLANNING	Executing the plugin.
NO_PLAN	The controller has been started without a plan.
MAX_RETRIES	Exceeded the maximum number of retries without a valid command.
PAT_EXCEEDED	Exceeded the patience time without a valid command.
EMPTY_PLAN	Received an empty plan.
INVALID_PLAN	Received an invalid plan that the controller plugin rejected.
NO_LOCAL_CMD	Received no velocity command by the plugin, in the current cycle.
GOT_LOCAL_CMD	Got a valid velocity command from the plugin.
ARRIVED_GOAL	The robot arrived the goal.
CANCELED	The controller has been canceled.
STOPPED	The controller has been stopped!
INTERNAL_ERROR	An internal error occurred.

Definition at line 127 of file abstract_controller_execution.h.

Constructor & Destructor Documentation

mbf_abstract_nav::AbstractControllerExecution::AbstractControllerExecution	(	const std::string	name,
		const mbf_abstract_core::AbstractController::Ptr &	controller_ptr,
		const ros::Publisher &	vel_pub,
		const ros::Publisher &	goal_pub,
		const TFPtr &	tf_listener_ptr,
		const MoveBaseFlexConfig &	config,
		boost::function< void()>	setup_fn,
		boost::function< void()>	cleanup_fn
	)

Constructor.

Parameters:

condition	Thread sleep condition variable, to wake up connected threads
controller_plugin_type	The plugin type associated with the plugin to load
tf_listener_ptr	Shared pointer to a common tf listener

Definition at line 49 of file abstract_controller_execution.cpp.

mbf_abstract_nav::AbstractControllerExecution::~AbstractControllerExecution ( ) [virtual]

Destructor.

Definition at line 78 of file abstract_controller_execution.cpp.

Member Function Documentation

bool mbf_abstract_nav::AbstractControllerExecution::cancel ( ) [virtual]

Cancel the planner execution. This calls the cancel method of the planner plugin. This could be useful if the computation takes too much time.

Returns:: true, if the planner plugin tries / tried to cancel the planning step.

Implements mbf_abstract_nav::AbstractExecutionBase.

Definition at line 235 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::computeRobotPose ( ) [private]

Computes the robot pose;.

Returns:: true if the robot pose has been computed successfully, false otherwise.

Definition at line 162 of file abstract_controller_execution.cpp.

uint32_t mbf_abstract_nav::AbstractControllerExecution::computeVelocityCmd	(	const geometry_msgs::PoseStamped &	pose,
		const geometry_msgs::TwistStamped &	velocity,
		geometry_msgs::TwistStamped &	vel_cmd,
		std::string &	message
	)		`[protected, virtual]`

Request plugin for a new velocity command, given the current position, orientation, and velocity of the robot. We use this virtual method to give concrete implementations as move_base the chance to override it and do additional stuff, for example locking the costmap.

Parameters:

pose	the current pose of the robot.
velocity	the current velocity of the robot.
cmd_vel	Will be filled with the velocity command to be passed to the robot base.
message	Optional more detailed outcome as a string.

Returns:: Result code as described on ExePath action result and plugin's header.

Definition at line 180 of file abstract_controller_execution.cpp.

ros::Time mbf_abstract_nav::AbstractControllerExecution::getLastPluginCallTime ( )

Returns the time of the last plugin call.

Returns:: Time of the last plugin call

Definition at line 208 of file abstract_controller_execution.cpp.

std::vector< geometry_msgs::PoseStamped > mbf_abstract_nav::AbstractControllerExecution::getNewPlan ( ) [private]

Gets the new available plan. This method is thread safe.

Returns:: The plan

Definition at line 154 of file abstract_controller_execution.cpp.

AbstractControllerExecution::ControllerState mbf_abstract_nav::AbstractControllerExecution::getState ( )

Return the current state of the controller execution. Thread communication safe.

Returns:: current state, enum value of ControllerState

Definition at line 127 of file abstract_controller_execution.cpp.

geometry_msgs::TwistStamped mbf_abstract_nav::AbstractControllerExecution::getVelocityCmd ( )

Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method. Note that it doesn't need to be a valid command sent to the robot, as we report also failed calls to the plugin on controller action feedback.

Returns:: The last valid velocity command.

Definition at line 201 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::hasNewPlan ( ) [private]

Returns true if a new plan is available, false otherwise! A new plan is set by another thread!

Returns:: true, if a new plan has been set, false otherwise.

Definition at line 147 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::isMoving ( )

Returns whether the robot should normally move or not. True if the controller seems to work properly.

Returns:: true, if the robot should normally move, false otherwise

Definition at line 222 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::isPatienceExceeded ( )

Checks whether the patience duration time has been exceeded, ot not.

Returns:: true, if the patience has been exceeded.

Definition at line 215 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::publishZeroVelocity ( ) [private]

publishes a velocity command with zero values to stop the robot.

Definition at line 409 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::reachedGoalCheck ( ) [private]

Checks whether the goal has been reached in the range of tolerance or not.

Returns:: true if the goal has been reached, false otherwise

Definition at line 227 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::reconfigure ( const MoveBaseFlexConfig & config )

Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconfigure to reconfigure the current state.

Parameters:

config MoveBaseFlexConfig object

Definition at line 94 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::run ( ) [protected, virtual]

The main run method, a thread will execute this method. It contains the main controller execution loop.

Implements mbf_abstract_nav::AbstractExecutionBase.

Definition at line 249 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::setControllerFrequency ( double frequency )

Sets the controller frequency.

Parameters:

frequency The controller frequency

Returns:: true, if the controller frequency has been changed / set succesfully, false otherwise

Definition at line 82 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::setNewPlan ( const std::vector< geometry_msgs::PoseStamped > & plan )

Sets a new plan to the controller execution.

Parameters:

plan	A vector of stamped poses.

Definition at line 133 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::setState ( ControllerState state ) [private]

Sets the controller state. This method makes the communication of the state thread safe.

Parameters:

state The current controller state.

Definition at line 119 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::setVelocityCmd ( const geometry_msgs::TwistStamped & vel_cmd_stamped ) [protected]

Sets the velocity command, to make it available for another thread.

Parameters:

vel_cmd_stamped current velocity command

Definition at line 189 of file abstract_controller_execution.cpp.

bool mbf_abstract_nav::AbstractControllerExecution::start ( ) [virtual]

Starts the controller, a valid plan should be given in advance.

Returns:: false if the thread is already running, true if starting the controller succeeded!

Reimplemented from mbf_abstract_nav::AbstractExecutionBase.

Definition at line 107 of file abstract_controller_execution.cpp.

Member Data Documentation

double mbf_abstract_nav::AbstractControllerExecution::angle_tolerance_ [private]

angle tolerance to the given goal pose

Definition at line 332 of file abstract_controller_execution.h.

boost::chrono::microseconds mbf_abstract_nav::AbstractControllerExecution::calling_duration_ [private]

the duration which corresponds with the controller frequency.

Definition at line 299 of file abstract_controller_execution.h.

boost::mutex mbf_abstract_nav::AbstractControllerExecution::configuration_mutex_ [private]

dynamic reconfigure config mutex, thread safe param reading and writing

Definition at line 320 of file abstract_controller_execution.h.

mbf_abstract_core::AbstractController::Ptr mbf_abstract_nav::AbstractControllerExecution::controller_ [protected]

the local planer to calculate the velocity command

Definition at line 217 of file abstract_controller_execution.h.

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::current_goal_pub_ [private]

publisher for the current goal

Definition at line 311 of file abstract_controller_execution.h.

const double mbf_abstract_nav::AbstractControllerExecution::DEFAULT_CONTROLLER_FREQUENCY = 100.0 [static]

Definition at line 80 of file abstract_controller_execution.h.

double mbf_abstract_nav::AbstractControllerExecution::dist_tolerance_ [private]

distance tolerance to the given goal pose

Definition at line 329 of file abstract_controller_execution.h.

std::string mbf_abstract_nav::AbstractControllerExecution::global_frame_ [private]

the global frame the robot is controlling in.

Definition at line 305 of file abstract_controller_execution.h.

ros::Time mbf_abstract_nav::AbstractControllerExecution::last_call_time_ [protected]

The current cycle start time of the last cycle run. Will by updated each cycle.

Definition at line 223 of file abstract_controller_execution.h.

boost::mutex mbf_abstract_nav::AbstractControllerExecution::lct_mtx_ [private]

mutex to handle safe thread communication for the last plugin call time

Definition at line 275 of file abstract_controller_execution.h.

int mbf_abstract_nav::AbstractControllerExecution::max_retries_ [protected]

The maximum number of retries.

Definition at line 229 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::mbf_tolerance_check_ [private]

whether move base flex should check for the goal tolerance or not.

Definition at line 326 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::moving_ [private]

main controller loop variable, true if the controller is running, false otherwise

Definition at line 323 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::new_plan_ [private]

true, if a new plan is available. See hasNewPlan()!

Definition at line 278 of file abstract_controller_execution.h.

ros::Duration mbf_abstract_nav::AbstractControllerExecution::patience_ [protected]

The time / duration of patience, before changing the state.

Definition at line 232 of file abstract_controller_execution.h.

std::vector<geometry_msgs::PoseStamped> mbf_abstract_nav::AbstractControllerExecution::plan_ [private]

the last set plan which is currently processed by the controller

Definition at line 296 of file abstract_controller_execution.h.

boost::mutex mbf_abstract_nav::AbstractControllerExecution::plan_mtx_ [private]

mutex to handle safe thread communication for the current plan

Definition at line 269 of file abstract_controller_execution.h.

std::string mbf_abstract_nav::AbstractControllerExecution::plugin_name_ [protected]

the name of the loaded plugin

Definition at line 214 of file abstract_controller_execution.h.

std::string mbf_abstract_nav::AbstractControllerExecution::robot_frame_ [private]

the frame of the robot, which will be used to determine its position.

Definition at line 302 of file abstract_controller_execution.h.

geometry_msgs::PoseStamped mbf_abstract_nav::AbstractControllerExecution::robot_pose_ [private]

current robot pose;

Definition at line 335 of file abstract_controller_execution.h.

ros::Time mbf_abstract_nav::AbstractControllerExecution::start_time_ [protected]

The time the controller has been started.

Definition at line 226 of file abstract_controller_execution.h.

AbstractControllerExecution::ControllerState mbf_abstract_nav::AbstractControllerExecution::state_ [private]

the current controller state

Definition at line 314 of file abstract_controller_execution.h.

boost::mutex mbf_abstract_nav::AbstractControllerExecution::state_mtx_ [private]

mutex to handle safe thread communication for the current value of the state

Definition at line 266 of file abstract_controller_execution.h.

const TFPtr& mbf_abstract_nav::AbstractControllerExecution::tf_listener_ptr [protected]

shared pointer to the shared tf listener

Definition at line 220 of file abstract_controller_execution.h.

double mbf_abstract_nav::AbstractControllerExecution::tf_timeout_ [private]

time before a timeout used for tf requests

Definition at line 317 of file abstract_controller_execution.h.

boost::mutex mbf_abstract_nav::AbstractControllerExecution::vel_cmd_mtx_ [private]

mutex to handle safe thread communication for the current velocity command

Definition at line 272 of file abstract_controller_execution.h.

geometry_msgs::TwistStamped mbf_abstract_nav::AbstractControllerExecution::vel_cmd_stamped_ [private]

the last calculated velocity command

Definition at line 293 of file abstract_controller_execution.h.

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::vel_pub_ [private]

publisher for the current velocity command

Definition at line 308 of file abstract_controller_execution.h.

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

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation