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]

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)
	Constructor. More...

virtual bool	cancel ()
	Cancel the controller execution. This calls the cancel method of the controller plugin, sets the cancel_ flag to true, and waits for the control loop to stop. Normally called upon aborting the navigation. More...

ros::Time	getLastPluginCallTime ()
	Returns the time of the last plugin call. More...

ControllerState	getState ()
	Return the current state of the controller execution. Thread communication safe. More...

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. More...

bool	isMoving ()
	Returns whether the robot should normally move or not. True if the controller seems to work properly. More...

bool	isPatienceExceeded ()
	Checks whether the patience duration time has been exceeded, ot not. More...

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. More...

bool	setControllerFrequency (double frequency)
	Sets the controller frequency. More...

void	setNewPlan (const std::vector< geometry_msgs::PoseStamped > &plan, bool tolerance_from_action=false, double action_dist_tolerance=1.0, double action_angle_tolerance=3.1415)
	Sets a new plan to the controller execution. More...

virtual bool	start ()
	Starts the controller, a valid plan should be given in advance. More...

virtual	~AbstractControllerExecution ()
	Destructor. More...

Public Member Functions inherited from mbf_abstract_nav::AbstractExecutionBase
	AbstractExecutionBase (std::string name)

std::string	getMessage ()
	Gets the current plugin execution message. More...

std::string	getName ()
	Returns the name of the corresponding plugin. More...

uint32_t	getOutcome ()
	Gets the current plugin execution outcome. More...

void	join ()

virtual void	postRun ()
	Optional implementation-specific cleanup function, called right after execution. More...

virtual void	preRun ()
	Optional implementation-specific setup function, called right before execution. More...

virtual void	stop ()

boost::cv_status	waitForStateUpdate (boost::chrono::microseconds const &duration)

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. More...

virtual void	run ()
	The main run method, a thread will execute this method. It contains the main controller execution loop. More...

virtual bool	safetyCheck ()
	Check if its safe to drive. This method gets called at every controller cycle, stopping the robot if its not. When overridden by child class, gives a chance to the specific execution implementation to stop the robot if it detects something wrong on the underlying map. More...

void	setVelocityCmd (const geometry_msgs::TwistStamped &vel_cmd_stamped)
	Sets the velocity command, to make it available for another thread. More...

Protected Attributes
mbf_abstract_core::AbstractController::Ptr	controller_
	the local planer to calculate the velocity command More...

ros::Time	last_call_time_
	The current cycle start time of the last cycle run. Will by updated each cycle. More...

ros::Time	last_valid_cmd_time_
	The time the controller responded with a success output (output < 10). More...

int	max_retries_
	The maximum number of retries. More...

ros::Duration	patience_
	The time / duration of patience, before changing the state. More...

std::string	plugin_name_
	the name of the loaded plugin More...

ros::Time	start_time_
	The time the controller has been started. More...

const TFPtr &	tf_listener_ptr
	shared pointer to the shared tf listener More...

Protected Attributes inherited from mbf_abstract_nav::AbstractExecutionBase
bool	cancel_
	flag for canceling controlling More...

boost::condition_variable	condition_
	condition variable to wake up control thread More...

std::string	message_
	the last received plugin execution message More...

std::string	name_
	the plugin name; not the plugin type! More...

uint32_t	outcome_
	the last received plugin execution outcome More...

boost::thread	thread_
	the controlling thread object More...

Private Member Functions
bool	computeRobotPose ()
	Computes the robot pose;. More...

std::vector< geometry_msgs::PoseStamped >	getNewPlan ()
	Gets the new available plan. This method is thread safe. More...

bool	hasNewPlan ()
	Returns true if a new plan is available, false otherwise! A new plan is set by another thread! More...

void	publishZeroVelocity ()
	Publishes a velocity command with zero values to stop the robot. More...

bool	reachedGoalCheck ()
	Checks whether the goal has been reached in the range of tolerance or not. More...

void	setState (ControllerState state)
	Sets the controller state. This method makes the communication of the state thread safe. More...

Private Attributes
double	action_angle_tolerance_
	replaces parameter angle_tolerance_ for the action More...

double	action_dist_tolerance_
	replaces parameter dist_tolerance_ for the action More...

double	angle_tolerance_
	angle tolerance to the given goal pose More...

boost::chrono::microseconds	calling_duration_
	the duration which corresponds with the controller frequency. More...

boost::mutex	configuration_mutex_
	dynamic reconfigure config mutex, thread safe param reading and writing More...

ros::Publisher	current_goal_pub_
	publisher for the current goal More...

double	dist_tolerance_
	distance tolerance to the given goal pose More...

bool	force_stop_at_goal_
	whether move base flex should force the robot to stop once the goal is reached. More...

bool	force_stop_on_cancel_
	whether move base flex should force the robot to stop on canceling navigation. More...

std::string	global_frame_
	the global frame the robot is controlling in. More...

boost::mutex	lct_mtx_
	mutex to handle safe thread communication for the last plugin call time More...

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

bool	moving_
	main controller loop variable, true if the controller is running, false otherwise More...

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

std::vector< geometry_msgs::PoseStamped >	plan_
	the last set plan which is currently processed by the controller More...

boost::mutex	plan_mtx_
	mutex to handle safe thread communication for the current plan More...

std::string	robot_frame_
	the frame of the robot, which will be used to determine its position. More...

geometry_msgs::PoseStamped	robot_pose_
	current robot pose; More...

AbstractControllerExecution::ControllerState	state_
	the current controller state More...

boost::mutex	state_mtx_
	mutex to handle safe thread communication for the current value of the state More...

double	tf_timeout_
	time before a timeout used for tf requests More...

bool	tolerance_from_action_
	whether check for action specific tolerance More...

boost::mutex	vel_cmd_mtx_
	mutex to handle safe thread communication for the current velocity command More...

geometry_msgs::TwistStamped	vel_cmd_stamped_
	the last calculated velocity command More...

ros::Publisher	vel_pub_
	publisher for the current velocity command More...

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 75 of file abstract_controller_execution.h.

Member Typedef Documentation

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

Definition at line 81 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 132 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
	)

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 50 of file abstract_controller_execution.cpp.

mbf_abstract_nav::AbstractControllerExecution::~AbstractControllerExecution ( )

virtual

Destructor.

Definition at line 79 of file abstract_controller_execution.cpp.

Member Function Documentation

bool mbf_abstract_nav::AbstractControllerExecution::cancel ( )

virtual

Cancel the controller execution. This calls the cancel method of the controller plugin, sets the cancel_ flag to true, and waits for the control loop to stop. Normally called upon aborting the navigation.

Returns: true, if the control loop stops within a cycle time.

Implements mbf_abstract_nav::AbstractExecutionBase.

Definition at line 261 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 170 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
	)

protectedvirtual

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 185 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 213 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 162 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 128 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 206 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 155 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 240 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 220 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 463 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 245 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 95 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::run ( )

protectedvirtual

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 280 of file abstract_controller_execution.cpp.

virtual bool mbf_abstract_nav::AbstractControllerExecution::safetyCheck ( )

inlineprotectedvirtual

Check if its safe to drive. This method gets called at every controller cycle, stopping the robot if its not. When overridden by child class, gives a chance to the specific execution implementation to stop the robot if it detects something wrong on the underlying map.

Returns: Always true, unless overridden by child class.

Definition at line 254 of file abstract_controller_execution.h.

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 83 of file abstract_controller_execution.cpp.

void mbf_abstract_nav::AbstractControllerExecution::setNewPlan	(	const std::vector< geometry_msgs::PoseStamped > &	plan,
		bool	tolerance_from_action = `false`,
		double	action_dist_tolerance = `1.0`,
		double	action_angle_tolerance = `3.1415`
	)

Sets a new plan to the controller execution.

Parameters

plan	A vector of stamped poses.
tolerance_from_action	flag that will be set to true when the new plan (action) has tolerance
action_dist_tolerance	distance to goal tolerance specific for this new plan (action)
action_angle_tolerance	angle to goal tolerance specific for this new plan (action)

Definition at line 134 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 120 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 194 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 108 of file abstract_controller_execution.cpp.

Member Data Documentation

double mbf_abstract_nav::AbstractControllerExecution::action_angle_tolerance_

private

replaces parameter angle_tolerance_ for the action

Definition at line 367 of file abstract_controller_execution.h.

double mbf_abstract_nav::AbstractControllerExecution::action_dist_tolerance_

private

replaces parameter dist_tolerance_ for the action

Definition at line 364 of file abstract_controller_execution.h.

double mbf_abstract_nav::AbstractControllerExecution::angle_tolerance_

private

angle tolerance to the given goal pose

Definition at line 355 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 316 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 337 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 222 of file abstract_controller_execution.h.

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::current_goal_pub_

private

publisher for the current goal

Definition at line 328 of file abstract_controller_execution.h.

const double mbf_abstract_nav::AbstractControllerExecution::DEFAULT_CONTROLLER_FREQUENCY = 100.0

static

Definition at line 79 of file abstract_controller_execution.h.

double mbf_abstract_nav::AbstractControllerExecution::dist_tolerance_

private

distance tolerance to the given goal pose

Definition at line 352 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::force_stop_at_goal_

private

whether move base flex should force the robot to stop once the goal is reached.

Definition at line 346 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::force_stop_on_cancel_

private

whether move base flex should force the robot to stop on canceling navigation.

Definition at line 349 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 322 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 228 of file abstract_controller_execution.h.

ros::Time mbf_abstract_nav::AbstractControllerExecution::last_valid_cmd_time_

protected

The time the controller responded with a success output (output < 10).

Definition at line 234 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 292 of file abstract_controller_execution.h.

int mbf_abstract_nav::AbstractControllerExecution::max_retries_

protected

The maximum number of retries.

Definition at line 237 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 343 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 340 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 295 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 240 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 313 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 286 of file abstract_controller_execution.h.

std::string mbf_abstract_nav::AbstractControllerExecution::plugin_name_

protected

the name of the loaded plugin

Definition at line 219 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 319 of file abstract_controller_execution.h.

geometry_msgs::PoseStamped mbf_abstract_nav::AbstractControllerExecution::robot_pose_

private

current robot pose;

Definition at line 358 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 231 of file abstract_controller_execution.h.

AbstractControllerExecution::ControllerState mbf_abstract_nav::AbstractControllerExecution::state_

private

the current controller state

Definition at line 331 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 283 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 225 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 334 of file abstract_controller_execution.h.

bool mbf_abstract_nav::AbstractControllerExecution::tolerance_from_action_

private

whether check for action specific tolerance

Definition at line 361 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 289 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 310 of file abstract_controller_execution.h.

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::vel_pub_

private

publisher for the current velocity command

Definition at line 325 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