Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
mbf_abstract_nav::AbstractControllerExecution Class Reference

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:
Inheritance graph
[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< AbstractControllerExecutionPtr
 

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

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

◆ Ptr

Definition at line 82 of file abstract_controller_execution.h.

Member Enumeration Documentation

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

Constructor & Destructor Documentation

◆ AbstractControllerExecution()

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
conditionThread sleep condition variable, to wake up connected threads
controller_plugin_typeThe plugin type associated with the plugin to load
tf_listener_ptrShared pointer to a common tf listener

Definition at line 49 of file abstract_controller_execution.cpp.

◆ ~AbstractControllerExecution()

mbf_abstract_nav::AbstractControllerExecution::~AbstractControllerExecution ( )
virtual

Destructor.

Definition at line 76 of file abstract_controller_execution.cpp.

Member Function Documentation

◆ cancel()

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

◆ computeRobotPose()

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

◆ computeVelocityCmd()

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
posethe current pose of the robot.
velocitythe current velocity of the robot.
cmd_velWill be filled with the velocity command to be passed to the robot base.
messageOptional more detailed outcome as a string.
Returns
Result code as described on ExePath action result and plugin's header.

Definition at line 178 of file abstract_controller_execution.cpp.

◆ getLastPluginCallTime()

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

◆ getNewPlan()

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

◆ getState()

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

◆ getVelocityCmd()

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

◆ hasNewPlan()

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

◆ isMoving()

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

◆ isPatienceExceeded()

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

◆ publishZeroVelocity()

void mbf_abstract_nav::AbstractControllerExecution::publishZeroVelocity ( )
private

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

Definition at line 416 of file abstract_controller_execution.cpp.

◆ reachedGoalCheck()

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

◆ reconfigure()

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

Definition at line 92 of file abstract_controller_execution.cpp.

◆ run()

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

◆ safetyCheck()

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

◆ setControllerFrequency()

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

Sets the controller frequency.

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

Definition at line 80 of file abstract_controller_execution.cpp.

◆ setNewPlan()

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

Sets a new plan to the controller execution.

Parameters
planA vector of stamped poses.

Definition at line 131 of file abstract_controller_execution.cpp.

◆ setState()

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

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

Parameters
stateThe current controller state.

Definition at line 117 of file abstract_controller_execution.cpp.

◆ setVelocityCmd()

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_stampedcurrent velocity command

Definition at line 187 of file abstract_controller_execution.cpp.

◆ start()

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

Member Data Documentation

◆ angle_tolerance_

double mbf_abstract_nav::AbstractControllerExecution::angle_tolerance_
private

angle tolerance to the given goal pose

Definition at line 339 of file abstract_controller_execution.h.

◆ calling_duration_

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

the duration which corresponds with the controller frequency.

Definition at line 306 of file abstract_controller_execution.h.

◆ configuration_mutex_

boost::mutex mbf_abstract_nav::AbstractControllerExecution::configuration_mutex_
private

dynamic reconfigure config mutex, thread safe param reading and writing

Definition at line 327 of file abstract_controller_execution.h.

◆ controller_

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

the local planer to calculate the velocity command

Definition at line 215 of file abstract_controller_execution.h.

◆ current_goal_pub_

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::current_goal_pub_
private

publisher for the current goal

Definition at line 318 of file abstract_controller_execution.h.

◆ DEFAULT_CONTROLLER_FREQUENCY

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

Definition at line 80 of file abstract_controller_execution.h.

◆ dist_tolerance_

double mbf_abstract_nav::AbstractControllerExecution::dist_tolerance_
private

distance tolerance to the given goal pose

Definition at line 336 of file abstract_controller_execution.h.

◆ global_frame_

std::string mbf_abstract_nav::AbstractControllerExecution::global_frame_
private

the global frame the robot is controlling in.

Definition at line 312 of file abstract_controller_execution.h.

◆ last_call_time_

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

◆ lct_mtx_

boost::mutex mbf_abstract_nav::AbstractControllerExecution::lct_mtx_
private

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

Definition at line 282 of file abstract_controller_execution.h.

◆ max_retries_

int mbf_abstract_nav::AbstractControllerExecution::max_retries_
protected

The maximum number of retries.

Definition at line 227 of file abstract_controller_execution.h.

◆ mbf_tolerance_check_

bool mbf_abstract_nav::AbstractControllerExecution::mbf_tolerance_check_
private

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

Definition at line 333 of file abstract_controller_execution.h.

◆ moving_

bool mbf_abstract_nav::AbstractControllerExecution::moving_
private

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

Definition at line 330 of file abstract_controller_execution.h.

◆ new_plan_

bool mbf_abstract_nav::AbstractControllerExecution::new_plan_
private

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

Definition at line 285 of file abstract_controller_execution.h.

◆ patience_

ros::Duration mbf_abstract_nav::AbstractControllerExecution::patience_
protected

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

Definition at line 230 of file abstract_controller_execution.h.

◆ plan_

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

◆ plan_mtx_

boost::mutex mbf_abstract_nav::AbstractControllerExecution::plan_mtx_
private

mutex to handle safe thread communication for the current plan

Definition at line 276 of file abstract_controller_execution.h.

◆ plugin_name_

std::string mbf_abstract_nav::AbstractControllerExecution::plugin_name_
protected

the name of the loaded plugin

Definition at line 212 of file abstract_controller_execution.h.

◆ robot_frame_

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

◆ robot_pose_

geometry_msgs::PoseStamped mbf_abstract_nav::AbstractControllerExecution::robot_pose_
private

current robot pose;

Definition at line 342 of file abstract_controller_execution.h.

◆ start_time_

ros::Time mbf_abstract_nav::AbstractControllerExecution::start_time_
protected

The time the controller has been started.

Definition at line 224 of file abstract_controller_execution.h.

◆ state_

AbstractControllerExecution::ControllerState mbf_abstract_nav::AbstractControllerExecution::state_
private

the current controller state

Definition at line 321 of file abstract_controller_execution.h.

◆ state_mtx_

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

◆ tf_listener_ptr

const TFPtr& mbf_abstract_nav::AbstractControllerExecution::tf_listener_ptr
protected

shared pointer to the shared tf listener

Definition at line 218 of file abstract_controller_execution.h.

◆ tf_timeout_

double mbf_abstract_nav::AbstractControllerExecution::tf_timeout_
private

time before a timeout used for tf requests

Definition at line 324 of file abstract_controller_execution.h.

◆ vel_cmd_mtx_

boost::mutex mbf_abstract_nav::AbstractControllerExecution::vel_cmd_mtx_
private

mutex to handle safe thread communication for the current velocity command

Definition at line 279 of file abstract_controller_execution.h.

◆ vel_cmd_stamped_

geometry_msgs::TwistStamped mbf_abstract_nav::AbstractControllerExecution::vel_cmd_stamped_
private

the last calculated velocity command

Definition at line 300 of file abstract_controller_execution.h.

◆ vel_pub_

ros::Publisher mbf_abstract_nav::AbstractControllerExecution::vel_pub_
private

publisher for the current velocity command

Definition at line 315 of file abstract_controller_execution.h.


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


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Sat Oct 12 2019 04:02:25