Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00043 #ifndef ROBOT_PROCESS_ROBOT_PROCESS_H
00044 #define ROBOT_PROCESS_ROBOT_PROCESS_H
00045
00046 #include <ros/ros.h>
00047 #include <ros/console.h>
00048 #include <ros/callback_queue.h>
00049
00050 #include <std_srvs/Empty.h>
00051 #include <robot_process_msgs/State.h>
00052 #include <robot_process_msgs/Error.h>
00053
00054 #include <robot_process/isolated_async_timer.h>
00055
00056 #include <string>
00057 #include <vector>
00058
00059 namespace robot_process
00060 {
00061
00067 enum class State : std::uint8_t
00068 {
00069 INVALID = robot_process_msgs::State::INVALID,
00070 LAUNCHING = robot_process_msgs::State::LAUNCHING,
00071 UNCONFIGURED = robot_process_msgs::State::UNCONFIGURED,
00072 STOPPED = robot_process_msgs::State::STOPPED,
00073 PAUSED = robot_process_msgs::State::PAUSED,
00074 RUNNING = robot_process_msgs::State::RUNNING,
00075 TERMINATED = robot_process_msgs::State::TERMINATED,
00076 Count = 7
00077 };
00078
00085 std::ostream& operator<<(std::ostream& os, State state);
00086
00091 class RobotProcess
00092 {
00093 public:
00097 RobotProcess() = delete;
00098
00109 RobotProcess(int argc, char* argv[],
00110 const std::string& name_space = std::string(),
00111 const std::string& name = std::string());
00112
00116 virtual ~RobotProcess();
00117
00127 RobotProcess& init(bool autostart = false);
00128
00136 void run(uint8_t threads = 0);
00137
00145 void runAsync(uint8_t threads = 0);
00146
00151 State getState();
00152
00160 std::string getNamespace() const;
00161
00162 protected:
00163 ros::NodeHandlePtr node_handle_;
00164 ros::NodeHandlePtr node_handle_private_;
00165
00174 void notifyError(uint8_t error_type,
00175 const std::string& function,
00176 const std::string& description);
00177
00193 void registerIsolatedTimer(const IsolatedAsyncTimer::LambdaCallback& callback,
00194 const float& frequency,
00195 bool stoppable = true);
00196
00197 private:
00202 std::vector<std::shared_ptr<robot_process::IsolatedAsyncTimer>> process_timers_;
00203
00207 std::shared_ptr<robot_process::IsolatedAsyncTimer> heartbeat_timer_;
00208
00214 std::string node_namespace_;
00215
00219 std::string node_name_;
00220
00227 bool wait_for_supervisor_ = true;
00228
00233 bool autostart_ = false;
00234
00238 bool autostart_after_reconfigure_ = false;
00239
00243 ros::CallbackQueue state_request_callback_queue_;
00244
00248 std::shared_ptr<ros::AsyncSpinner> state_request_spinner_;
00249
00253 ros::ServiceServer terminate_server_;
00254
00258 ros::ServiceServer reconfigure_server_;
00259
00264 ros::ServiceServer restart_server_;
00265
00269 ros::ServiceServer start_server_;
00270
00271
00275 ros::ServiceServer stop_server_;
00276
00280 ros::ServiceServer pause_server_;
00281
00282
00286 ros::Publisher process_state_pub_;
00287
00291 ros::Publisher process_error_pub_;
00292
00293
00297 std::shared_ptr<ros::AsyncSpinner> global_callback_queue_spinner_;
00298
00299
00303 State current_state_ = State::LAUNCHING;
00304
00309 virtual void onCreate() = 0;
00310
00315 virtual void onTerminate() = 0;
00316
00321 virtual void onConfigure() = 0;
00322
00327 virtual void onUnconfigure() = 0;
00328
00333 virtual void onStart() = 0;
00334
00339 virtual void onStop() = 0;
00340
00345 virtual void onPause() = 0;
00346
00351 virtual void onResume() = 0;
00352
00356 void create();
00357
00361 void terminate();
00362
00366 void configure();
00367
00371 void unconfigure();
00372
00376 void start();
00377
00381 void stop();
00382
00386 void resume();
00387
00391 void pause();
00392
00396 void notifyState() const;
00397
00404 void changeState(const State& new_state);
00405
00414 bool transitionToState(const State& new_state);
00415
00424 ros::ServiceServer registerStateChangeRequest(
00425 const std::string& service_name,
00426 const std::vector<State>& states);
00427
00428 typedef void (RobotProcess::*MemberLambdaCallback)();
00429
00430 typedef boost::function < bool(
00431 std_srvs::Empty::Request& req,
00432 std_srvs::Empty::Response& res) > EmptyServiceCallback;
00433 typedef MemberLambdaCallback StateTransitions
00434 [static_cast<uint8_t>(State::Count)]
00435 [static_cast<uint8_t>(State::Count)];
00436
00437 typedef State StateTransitionPaths
00438 [static_cast<uint8_t>(State::Count)]
00439 [static_cast<uint8_t>(State::Count)];
00440
00447 const static StateTransitions STATE_TRANSITIONS;
00448
00452 const static StateTransitionPaths STATE_TRANSITIONS_PATHS;
00453 };
00454
00455 }
00456
00457 #endif // ROBOT_PROCESS_ROBOT_PROCESS_H