Class for adding node lifecycle to ROS processes. More...
#include <robot_process.h>
Public Member Functions | |
std::string | getNamespace () const |
Returns the full private namespace. | |
State | getState () |
Returns the current state. | |
RobotProcess & | init (bool autostart=false) |
Initializes the RobotProcess. | |
RobotProcess () | |
Default constructor is deleted. | |
RobotProcess (int argc, char *argv[], const std::string &name_space=std::string(), const std::string &name=std::string()) | |
Constructor. | |
void | run (uint8_t threads=0) |
Spins an amount of threads to serve the global callback queue. The call is blocking. | |
void | runAsync (uint8_t threads=0) |
Spins an amount of threads to serve the global callback queue. The call is non-blocking. | |
virtual | ~RobotProcess () |
Virtual destructor. | |
Protected Member Functions | |
void | notifyError (uint8_t error_type, const std::string &function, const std::string &description) |
Sends an error message to a global error topic "/error". | |
void | registerIsolatedTimer (const IsolatedAsyncTimer::LambdaCallback &callback, const float &frequency, bool stoppable=true) |
Register an isolated async timer. | |
Protected Attributes | |
ros::NodeHandlePtr | node_handle_ |
ros::NodeHandlePtr | node_handle_private_ |
Private Types | |
typedef boost::function< bool(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) > | EmptyServiceCallback |
typedef void(RobotProcess::* | MemberLambdaCallback )() |
typedef State | StateTransitionPaths [static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)] |
typedef MemberLambdaCallback | StateTransitions [static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)] |
Private Member Functions | |
void | changeState (const State &new_state) |
Changes state from current to new. Direct transition must exist. The appropriate function will be called during transition. | |
void | configure () |
Called automatically, when transition from UNCONFIGURED to STOPPED. | |
void | create () |
Called automatically, when transition from LAUNCHING to UNCONFIGURED. | |
void | notifyState () const |
Sends a heartbeat message with the current state. | |
virtual void | onConfigure ()=0 |
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to STOPPED. | |
virtual void | onCreate ()=0 |
Function to be defined by the user. Called at the end of transition from LAUNCHING to UNCONFIGURED. | |
virtual void | onPause ()=0 |
Function to be defined by the user. Called at the end of transition from RUNNING to PAUSED. | |
virtual void | onResume ()=0 |
Function to be defined by the user. Called at the end of transition from PAUSED to RUNNING. | |
virtual void | onStart ()=0 |
Function to be defined by the user. Called at the end of transition from STOPPED to PAUSED. | |
virtual void | onStop ()=0 |
Function to be defined by the user. Called at the end of transition from PAUSED to STOPPED. | |
virtual void | onTerminate ()=0 |
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to TERMINATED. | |
virtual void | onUnconfigure ()=0 |
Function to be defined by the user. Called at the end of transition from STOPPED to UNCONFIGURED. | |
void | pause () |
Called automatically, when transition from RUNNING to PAUSED. | |
ros::ServiceServer | registerStateChangeRequest (const std::string &service_name, const std::vector< State > &states) |
Registers a ROS service server listening for state change requests. | |
void | resume () |
Called automatically, when transition from PAUSED to RUNNING. | |
void | start () |
Called automatically, when transition from STOPPED to PAUSED. | |
void | stop () |
Called automatically, when transition from PAUSED to STOPPED. | |
void | terminate () |
Called automatically, when transition from UNCONFIGURED to TERMINATED. | |
bool | transitionToState (const State &new_state) |
Transitions to a new state. Path must exists between the current and the new state. All appropriate function will be called when transition to the goal state. | |
void | unconfigure () |
Called automatically, when transition from STOPPED to UNCONFIGURED. | |
Private Attributes | |
bool | autostart_ = false |
Whether to automatically start (transition to RUNNING) at the end of init function if not the node will transition to STOPPED state. | |
bool | autostart_after_reconfigure_ = false |
Whether to autostart after reconfigure service is called. | |
State | current_state_ = State::LAUNCHING |
Current state of the RobotProcess. Initially, LAUNCHING state. | |
std::shared_ptr < ros::AsyncSpinner > | global_callback_queue_spinner_ |
Shared pointer to async spinner that serves the global callback queue. | |
std::shared_ptr < robot_process::IsolatedAsyncTimer > | heartbeat_timer_ |
Shared pointer to the isolated timer that sends heartbeat messages. | |
std::string | node_name_ |
Name of the actual ROS node. | |
std::string | node_namespace_ |
Node's namespace, if empty then the private node handle resolves to ~ otheerwise private node handle resolves to ~node_namespace. | |
ros::ServiceServer | pause_server_ |
ServiceServer for serving pause service. | |
ros::Publisher | process_error_pub_ |
ROS Publisher of error messages. | |
ros::Publisher | process_state_pub_ |
ROS Publisher of heartbeat messages and state changes messages. | |
std::vector< std::shared_ptr < robot_process::IsolatedAsyncTimer > > | process_timers_ |
Vector of shared pointers to isolated timers created by register isolated timer. | |
ros::ServiceServer | reconfigure_server_ |
ServiceServer for serving reconfigure service. | |
ros::ServiceServer | restart_server_ |
ServiceServer for serving restart service, where restart means transition to STOPPED and then RUNNING. | |
ros::ServiceServer | start_server_ |
ServiceServer for serving start service. | |
ros::CallbackQueue | state_request_callback_queue_ |
Callback queue for state change request services. | |
std::shared_ptr < ros::AsyncSpinner > | state_request_spinner_ |
Async spinner for serving state change requests. | |
ros::ServiceServer | stop_server_ |
ServiceServer for serving stop service. | |
ros::ServiceServer | terminate_server_ |
ServiceServer for serving terminate service. | |
bool | wait_for_supervisor_ = true |
Whether to wait for the supervisor during the init function. Waiting means that there has to be at least one subscriber of the hearthbeat topic. | |
Static Private Attributes | |
static const StateTransitions | STATE_TRANSITIONS |
2D array of direct state transitions with values being the corresponding functions to be called during that transition. First index signifies the state we are transitioning from, while the second index signifies the state we are transitioning to. | |
static const StateTransitionPaths | STATE_TRANSITIONS_PATHS |
2D array of paths between states. |
Class for adding node lifecycle to ROS processes.
Definition at line 91 of file robot_process.h.
typedef boost::function< bool( std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) > robot_process::RobotProcess::EmptyServiceCallback [private] |
Definition at line 432 of file robot_process.h.
typedef void(RobotProcess::* robot_process::RobotProcess::MemberLambdaCallback)() [private] |
Definition at line 428 of file robot_process.h.
typedef State robot_process::RobotProcess::StateTransitionPaths[static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)] [private] |
Definition at line 439 of file robot_process.h.
typedef MemberLambdaCallback robot_process::RobotProcess::StateTransitions[static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)] [private] |
Definition at line 435 of file robot_process.h.
Default constructor is deleted.
robot_process::RobotProcess::RobotProcess | ( | int | argc, |
char * | argv[], | ||
const std::string & | name_space = std::string() , |
||
const std::string & | name = std::string() |
||
) |
Constructor.
UNIX process args are passed in due to ros::init being called inside. name_space allows for running multiple instances of the same class or multiple RobotProcess classes in the same UNIX process without name collisions. name argument is the name passed in to ros::init, which is usually remapped in roslaunch. If name is empty and name is not remapped, then anonymous name will be created.
Definition at line 48 of file robot_process.cpp.
robot_process::RobotProcess::~RobotProcess | ( | ) | [virtual] |
Virtual destructor.
Definition at line 70 of file robot_process.cpp.
void robot_process::RobotProcess::changeState | ( | const State & | new_state | ) | [private] |
Changes state from current to new. Direct transition must exist. The appropriate function will be called during transition.
new_state | State to transition to. |
Definition at line 329 of file robot_process.cpp.
void robot_process::RobotProcess::configure | ( | ) | [private] |
Called automatically, when transition from UNCONFIGURED to STOPPED.
Definition at line 213 of file robot_process.cpp.
void robot_process::RobotProcess::create | ( | ) | [private] |
Called automatically, when transition from LAUNCHING to UNCONFIGURED.
Definition at line 199 of file robot_process.cpp.
std::string robot_process::RobotProcess::getNamespace | ( | ) | const |
Returns the full private namespace.
Returns the full private namespace, meaning name of the actual ROS node together with the name_space argument passed during construction.
Definition at line 190 of file robot_process.cpp.
Returns the current state.
Definition at line 154 of file robot_process.cpp.
RobotProcess & robot_process::RobotProcess::init | ( | bool | autostart = false | ) |
Initializes the RobotProcess.
Creates node handles, starts heartbeat thread, advertises state change request services, waits for the supervisor if requested
autostart | If true, transitions immediately to RUNNING state |
Definition at line 75 of file robot_process.cpp.
void robot_process::RobotProcess::notifyError | ( | uint8_t | error_type, |
const std::string & | function, | ||
const std::string & | description | ||
) | [protected] |
Sends an error message to a global error topic "/error".
Ideally the
error_type | Type of error |
function | Function, where the error was caused |
description | Detailed description of the error |
Definition at line 159 of file robot_process.cpp.
void robot_process::RobotProcess::notifyState | ( | ) | const [private] |
Sends a heartbeat message with the current state.
Definition at line 295 of file robot_process.cpp.
virtual void robot_process::RobotProcess::onConfigure | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to STOPPED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onCreate | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from LAUNCHING to UNCONFIGURED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcessWithTimer, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onPause | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from RUNNING to PAUSED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onResume | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from PAUSED to RUNNING.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onStart | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from STOPPED to PAUSED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onStop | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from PAUSED to STOPPED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onTerminate | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to TERMINATED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
virtual void robot_process::RobotProcess::onUnconfigure | ( | ) | [private, pure virtual] |
Function to be defined by the user. Called at the end of transition from STOPPED to UNCONFIGURED.
Implemented in robot_process::ManagedRobotProcess, AnyRobotProcess, and AnyRobotProcess.
void robot_process::RobotProcess::pause | ( | ) | [private] |
Called automatically, when transition from RUNNING to PAUSED.
Definition at line 258 of file robot_process.cpp.
void robot_process::RobotProcess::registerIsolatedTimer | ( | const IsolatedAsyncTimer::LambdaCallback & | callback, |
const float & | frequency, | ||
bool | stoppable = true |
||
) | [protected] |
Register an isolated async timer.
Registers an isolated async timer, which runs on a separate callback queue and is served by a single separate thread. The timer is managed during the lifecycle and transitioning to PAUSED state will pause its execution, meaning that the callback still executes, but returns immediately. When RobotProcess transitions to STOPPED state, the timer is stopped completely and started again during transition from STOPPED to PAUSED
callback | Timer callback to be registered |
frequency | Frequency of the timer in Hz |
stoppable | If true, timer cannot be stopped when transitioning to PAUSED or STOPPED state |
Definition at line 175 of file robot_process.cpp.
ros::ServiceServer robot_process::RobotProcess::registerStateChangeRequest | ( | const std::string & | service_name, |
const std::vector< State > & | states | ||
) | [private] |
Registers a ROS service server listening for state change requests.
service_name | Name of the service |
states | States to transition to in order |
Definition at line 269 of file robot_process.cpp.
void robot_process::RobotProcess::resume | ( | ) | [private] |
Called automatically, when transition from PAUSED to RUNNING.
Definition at line 247 of file robot_process.cpp.
void robot_process::RobotProcess::run | ( | uint8_t | threads = 0 | ) |
Spins an amount of threads to serve the global callback queue. The call is blocking.
threads | Number of threads to use, 0 signifies the amount of CPU cores available to the OS |
Definition at line 142 of file robot_process.cpp.
void robot_process::RobotProcess::runAsync | ( | uint8_t | threads = 0 | ) |
Spins an amount of threads to serve the global callback queue. The call is non-blocking.
threads | Number of threads to use, 0 signifies the amount of CPU cores available to the OS |
Definition at line 148 of file robot_process.cpp.
void robot_process::RobotProcess::start | ( | ) | [private] |
Called automatically, when transition from STOPPED to PAUSED.
Definition at line 225 of file robot_process.cpp.
void robot_process::RobotProcess::stop | ( | ) | [private] |
Called automatically, when transition from PAUSED to STOPPED.
Definition at line 236 of file robot_process.cpp.
void robot_process::RobotProcess::terminate | ( | ) | [private] |
Called automatically, when transition from UNCONFIGURED to TERMINATED.
Definition at line 205 of file robot_process.cpp.
bool robot_process::RobotProcess::transitionToState | ( | const State & | new_state | ) | [private] |
Transitions to a new state. Path must exists between the current and the new state. All appropriate function will be called when transition to the goal state.
new_state | State to transition to. |
Definition at line 305 of file robot_process.cpp.
void robot_process::RobotProcess::unconfigure | ( | ) | [private] |
Called automatically, when transition from STOPPED to UNCONFIGURED.
Definition at line 219 of file robot_process.cpp.
bool robot_process::RobotProcess::autostart_ = false [private] |
Whether to automatically start (transition to RUNNING) at the end of init function if not the node will transition to STOPPED state.
Definition at line 233 of file robot_process.h.
bool robot_process::RobotProcess::autostart_after_reconfigure_ = false [private] |
Whether to autostart after reconfigure service is called.
Definition at line 238 of file robot_process.h.
State robot_process::RobotProcess::current_state_ = State::LAUNCHING [private] |
Current state of the RobotProcess. Initially, LAUNCHING state.
Definition at line 303 of file robot_process.h.
std::shared_ptr<ros::AsyncSpinner> robot_process::RobotProcess::global_callback_queue_spinner_ [private] |
Shared pointer to async spinner that serves the global callback queue.
Definition at line 297 of file robot_process.h.
std::shared_ptr<robot_process::IsolatedAsyncTimer> robot_process::RobotProcess::heartbeat_timer_ [private] |
Shared pointer to the isolated timer that sends heartbeat messages.
Definition at line 207 of file robot_process.h.
Definition at line 163 of file robot_process.h.
Definition at line 164 of file robot_process.h.
std::string robot_process::RobotProcess::node_name_ [private] |
Name of the actual ROS node.
Definition at line 219 of file robot_process.h.
std::string robot_process::RobotProcess::node_namespace_ [private] |
Node's namespace, if empty then the private node handle resolves to ~ otheerwise private node handle resolves to ~node_namespace.
Definition at line 214 of file robot_process.h.
ServiceServer for serving pause service.
Definition at line 280 of file robot_process.h.
ROS Publisher of error messages.
Definition at line 291 of file robot_process.h.
ROS Publisher of heartbeat messages and state changes messages.
Definition at line 286 of file robot_process.h.
std::vector<std::shared_ptr<robot_process::IsolatedAsyncTimer> > robot_process::RobotProcess::process_timers_ [private] |
Vector of shared pointers to isolated timers created by register isolated timer.
Definition at line 202 of file robot_process.h.
ServiceServer for serving reconfigure service.
Definition at line 258 of file robot_process.h.
ServiceServer for serving restart service, where restart means transition to STOPPED and then RUNNING.
Definition at line 264 of file robot_process.h.
ServiceServer for serving start service.
Definition at line 269 of file robot_process.h.
Callback queue for state change request services.
Definition at line 243 of file robot_process.h.
std::shared_ptr<ros::AsyncSpinner> robot_process::RobotProcess::state_request_spinner_ [private] |
Async spinner for serving state change requests.
Definition at line 248 of file robot_process.h.
const RobotProcess::StateTransitions robot_process::RobotProcess::STATE_TRANSITIONS [static, private] |
{ {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}, {nullptr, nullptr, &RobotProcess::create, nullptr, nullptr, nullptr, nullptr}, {nullptr, nullptr, nullptr, &RobotProcess::configure, nullptr, nullptr, &RobotProcess::terminate}, {nullptr, nullptr, &RobotProcess::unconfigure, nullptr, &RobotProcess::start, nullptr, nullptr}, {nullptr, nullptr, nullptr, &RobotProcess::stop, nullptr, &RobotProcess::resume, nullptr}, {nullptr, nullptr, nullptr, nullptr, &RobotProcess::pause, nullptr, nullptr}, {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr} }
2D array of direct state transitions with values being the corresponding functions to be called during that transition. First index signifies the state we are transitioning from, while the second index signifies the state we are transitioning to.
Definition at line 447 of file robot_process.h.
const RobotProcess::StateTransitionPaths robot_process::RobotProcess::STATE_TRANSITIONS_PATHS [static, private] |
2D array of paths between states.
Definition at line 452 of file robot_process.h.
ServiceServer for serving stop service.
Definition at line 275 of file robot_process.h.
ServiceServer for serving terminate service.
Definition at line 253 of file robot_process.h.
bool robot_process::RobotProcess::wait_for_supervisor_ = true [private] |
Whether to wait for the supervisor during the init function. Waiting means that there has to be at least one subscriber of the hearthbeat topic.
Definition at line 227 of file robot_process.h.