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, boost::function< void()> setup_fn, boost::function< void()> cleanup_fn)
	Constructor. More...

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. 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)
	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, boost::function< void()> setup_fn, boost::function< void()> cleanup_fn)

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	stop ()

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

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

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 ()

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

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

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

Additional Inherited Members
Public Attributes inherited from mbf_abstract_nav::AbstractExecutionBase
boost::function< void()>	cleanup_fn_
	Implementation-specific cleanup function called right after execution; empty on abstract server. More...

boost::function< void()>	setup_fn_
	Implementation-specific setup function called right before execution; empty on abstract server. 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 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
	)

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 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 ( )

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

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation