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>
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. Normally called upon aborting the navigation. This calls the cancel method of the controller plugin. If the plugins returns true, it becomes responsible of stopping, and we will keep requesting velocity commands until it returns CANCELED. If it returns false (meaning cancel is not implemented, or that the controller defers handling it), MBF will set the cancel_ flag to true, and wait for the control loop to stop. More... | |
ros::Time | getLastPluginCallTime () const |
Returns the time of the last plugin call. More... | |
ControllerState | getState () const |
Return the current state of the controller execution. Thread communication safe. More... | |
geometry_msgs::TwistStamped | getVelocityCmd () const |
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 () const |
Returns whether the robot should normally move or not. True if the controller seems to work properly. More... | |
bool | isPatienceExceeded () const |
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 (const std::string &name) | |
const std::string & | getMessage () const |
Gets the current plugin execution message. More... | |
const std::string & | getName () const |
Returns the name of the corresponding plugin. More... | |
uint32_t | getOutcome () const |
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 | reconfigure (MoveBaseFlexConfig &_cfg) |
Optional implementaiton-specific configuration function. More... | |
virtual void | stop () |
boost::cv_status | waitForStateUpdate (boost::chrono::microseconds const &duration) |
virtual | ~AbstractExecutionBase () |
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... | |
std::string | global_frame_ |
the global frame the robot is controlling in. 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... | |
std::string | robot_frame_ |
the frame of the robot, which will be used to determine its position. 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::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... | |
boost::mutex | lct_mtx_ |
mutex to handle safe thread communication for the last plugin call time More... | |
ros::Rate | loop_rate_ |
the loop_rate which corresponds with the controller frequency. 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... | |
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... | |
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.
typedef boost::shared_ptr<AbstractControllerExecution > mbf_abstract_nav::AbstractControllerExecution::Ptr |
Definition at line 81 of file abstract_controller_execution.h.
Internal states.
Definition at line 134 of file abstract_controller_execution.h.
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.
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 src/abstract_controller_execution.cpp.
|
virtual |
Destructor.
Definition at line 79 of file src/abstract_controller_execution.cpp.
|
virtual |
Cancel the controller execution. Normally called upon aborting the navigation. This calls the cancel method of the controller plugin. If the plugins returns true, it becomes responsible of stopping, and we will keep requesting velocity commands until it returns CANCELED. If it returns false (meaning cancel is not implemented, or that the controller defers handling it), MBF will set the cancel_ flag to true, and wait for the control loop to stop.
Reimplemented from mbf_abstract_nav::AbstractExecutionBase.
Definition at line 255 of file src/abstract_controller_execution.cpp.
|
private |
Computes the robot pose;.
Definition at line 168 of file src/abstract_controller_execution.cpp.
|
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.
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. |
Definition at line 183 of file src/abstract_controller_execution.cpp.
ros::Time mbf_abstract_nav::AbstractControllerExecution::getLastPluginCallTime | ( | ) | const |
Returns the time of the last plugin call.
Definition at line 209 of file src/abstract_controller_execution.cpp.
|
private |
Gets the new available plan. This method is thread safe.
Definition at line 160 of file src/abstract_controller_execution.cpp.
AbstractControllerExecution::ControllerState mbf_abstract_nav::AbstractControllerExecution::getState | ( | ) | const |
Return the current state of the controller execution. Thread communication safe.
Definition at line 126 of file src/abstract_controller_execution.cpp.
geometry_msgs::TwistStamped mbf_abstract_nav::AbstractControllerExecution::getVelocityCmd | ( | ) | const |
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.
Definition at line 203 of file src/abstract_controller_execution.cpp.
|
private |
Returns true if a new plan is available, false otherwise! A new plan is set by another thread!
Definition at line 153 of file src/abstract_controller_execution.cpp.
bool mbf_abstract_nav::AbstractControllerExecution::isMoving | ( | ) | const |
Returns whether the robot should normally move or not. True if the controller seems to work properly.
Definition at line 234 of file src/abstract_controller_execution.cpp.
bool mbf_abstract_nav::AbstractControllerExecution::isPatienceExceeded | ( | ) | const |
Checks whether the patience duration time has been exceeded, ot not.
Definition at line 215 of file src/abstract_controller_execution.cpp.
|
private |
Publishes a velocity command with zero values to stop the robot.
Definition at line 461 of file src/abstract_controller_execution.cpp.
|
private |
Checks whether the goal has been reached in the range of tolerance or not.
Definition at line 239 of file src/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.
config | MoveBaseFlexConfig object |
Definition at line 95 of file src/abstract_controller_execution.cpp.
|
protectedvirtual |
The main run method, a thread will execute this method. It contains the main controller execution loop.
Reimplemented from mbf_abstract_nav::AbstractExecutionBase.
Definition at line 280 of file src/abstract_controller_execution.cpp.
|
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.
Definition at line 262 of file abstract_controller_execution.h.
bool mbf_abstract_nav::AbstractControllerExecution::setControllerFrequency | ( | double | frequency | ) |
Sets the controller frequency.
frequency | The controller frequency |
Definition at line 83 of file src/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.
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 132 of file src/abstract_controller_execution.cpp.
|
private |
Sets the controller state. This method makes the communication of the state thread safe.
state | The current controller state. |
Definition at line 120 of file src/abstract_controller_execution.cpp.
|
protected |
Sets the velocity command, to make it available for another thread.
vel_cmd_stamped | current velocity command |
Definition at line 192 of file src/abstract_controller_execution.cpp.
|
virtual |
Starts the controller, a valid plan should be given in advance.
Reimplemented from mbf_abstract_nav::AbstractExecutionBase.
Definition at line 108 of file src/abstract_controller_execution.cpp.
|
private |
replaces parameter angle_tolerance_ for the action
Definition at line 369 of file abstract_controller_execution.h.
|
private |
replaces parameter dist_tolerance_ for the action
Definition at line 366 of file abstract_controller_execution.h.
|
private |
angle tolerance to the given goal pose
Definition at line 357 of file abstract_controller_execution.h.
|
private |
dynamic reconfigure config mutex, thread safe param reading and writing
Definition at line 339 of file abstract_controller_execution.h.
|
protected |
the local planer to calculate the velocity command
Definition at line 224 of file abstract_controller_execution.h.
|
private |
publisher for the current goal
Definition at line 330 of file abstract_controller_execution.h.
|
static |
Definition at line 79 of file abstract_controller_execution.h.
|
private |
distance tolerance to the given goal pose
Definition at line 354 of file abstract_controller_execution.h.
|
private |
whether move base flex should force the robot to stop once the goal is reached.
Definition at line 348 of file abstract_controller_execution.h.
|
private |
whether move base flex should force the robot to stop on canceling navigation.
Definition at line 351 of file abstract_controller_execution.h.
|
protected |
the global frame the robot is controlling in.
Definition at line 248 of file abstract_controller_execution.h.
|
protected |
The current cycle start time of the last cycle run. Will by updated each cycle.
Definition at line 230 of file abstract_controller_execution.h.
|
protected |
The time the controller responded with a success output (output < 10).
Definition at line 236 of file abstract_controller_execution.h.
|
mutableprivate |
mutex to handle safe thread communication for the last plugin call time
Definition at line 300 of file abstract_controller_execution.h.
|
private |
the loop_rate which corresponds with the controller frequency.
Definition at line 324 of file abstract_controller_execution.h.
|
protected |
The maximum number of retries.
Definition at line 239 of file abstract_controller_execution.h.
|
private |
whether move base flex should check for the goal tolerance or not.
Definition at line 345 of file abstract_controller_execution.h.
|
private |
main controller loop variable, true if the controller is running, false otherwise
Definition at line 342 of file abstract_controller_execution.h.
|
private |
true, if a new plan is available. See hasNewPlan()!
Definition at line 303 of file abstract_controller_execution.h.
|
protected |
The time / duration of patience, before changing the state.
Definition at line 242 of file abstract_controller_execution.h.
|
private |
the last set plan which is currently processed by the controller
Definition at line 321 of file abstract_controller_execution.h.
|
mutableprivate |
mutex to handle safe thread communication for the current plan
Definition at line 294 of file abstract_controller_execution.h.
|
protected |
the name of the loaded plugin
Definition at line 221 of file abstract_controller_execution.h.
|
protected |
the frame of the robot, which will be used to determine its position.
Definition at line 245 of file abstract_controller_execution.h.
|
private |
current robot pose;
Definition at line 360 of file abstract_controller_execution.h.
|
protected |
The time the controller has been started.
Definition at line 233 of file abstract_controller_execution.h.
|
private |
the current controller state
Definition at line 333 of file abstract_controller_execution.h.
|
mutableprivate |
mutex to handle safe thread communication for the current value of the state
Definition at line 291 of file abstract_controller_execution.h.
|
protected |
shared pointer to the shared tf listener
Definition at line 227 of file abstract_controller_execution.h.
|
private |
time before a timeout used for tf requests
Definition at line 336 of file abstract_controller_execution.h.
|
private |
whether check for action specific tolerance
Definition at line 363 of file abstract_controller_execution.h.
|
mutableprivate |
mutex to handle safe thread communication for the current velocity command
Definition at line 297 of file abstract_controller_execution.h.
|
private |
the last calculated velocity command
Definition at line 318 of file abstract_controller_execution.h.
|
private |
publisher for the current velocity command
Definition at line 327 of file abstract_controller_execution.h.