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
00037 #include <robot_process/robot_process.h>
00038
00039 #include <string>
00040 #include <vector>
00041
00042 #define PRINT_FUNC_CALL(state) \
00043 ROS_DEBUG_STREAM(#state << "() method called")
00044
00045 namespace robot_process
00046 {
00047
00048 RobotProcess::RobotProcess(int argc, char* argv[],
00049 const std::string& name_space,
00050 const std::string& name)
00051 : node_namespace_(name_space),
00052 node_name_(name),
00053 state_request_callback_queue_()
00054 {
00055 if (ros::isInitialized())
00056 {
00057 node_name_ = ros::this_node::getName();
00058 return;
00059 }
00060
00061 if (node_name_.empty())
00062 {
00063 ros::init(argc, argv, "robot_process", ros::init_options::AnonymousName);
00064 node_name_ = ros::this_node::getName();
00065 }
00066 else
00067 ros::init(argc, argv, name);
00068 }
00069
00070 RobotProcess::~RobotProcess()
00071 {
00072 ROS_DEBUG_STREAM("RobotProcess dtor [" << getNamespace() << "]");
00073 }
00074
00075 RobotProcess& RobotProcess::init(bool autostart)
00076 {
00077 node_handle_ = ros::NodeHandlePtr(new ros::NodeHandle(node_namespace_));
00078 node_handle_private_ = ros::NodeHandlePtr(new ros::NodeHandle("~" + node_namespace_));
00079
00080 ros::param::param<bool>("~wait_for_supervisor", wait_for_supervisor_, true);
00081 ROS_INFO_STREAM("wait_for_supervisor = "
00082 << std::boolalpha << wait_for_supervisor_);
00083
00084 process_state_pub_ = node_handle_private_->advertise<robot_process_msgs::State>("/heartbeat", 0, true);
00085 process_error_pub_ = node_handle_private_->advertise<robot_process_msgs::Error>("/error", 0, true);
00086
00087 if (wait_for_supervisor_)
00088 {
00089 ros::Rate poll_rate(100);
00090 while (process_state_pub_.getNumSubscribers() == 0 && ros::ok())
00091 poll_rate.sleep();
00092 }
00093
00094 notifyState();
00095
00096 ros::param::param<bool>("~autostart_after_reconfigure", autostart_after_reconfigure_, false);
00097 ROS_INFO_STREAM("autostart_after_reconfigure = "
00098 << std::boolalpha << autostart_after_reconfigure_);
00099
00100 terminate_server_ = registerStateChangeRequest("terminate", {State::TERMINATED});
00101 reconfigure_server_ = registerStateChangeRequest("reconfigure",
00102 {
00103 State::UNCONFIGURED,
00104 autostart_after_reconfigure_ ? State::RUNNING : State::STOPPED
00105 });
00106
00107 restart_server_ = registerStateChangeRequest("restart", {State::STOPPED, State::RUNNING});
00108 start_server_ = registerStateChangeRequest("start", {State::RUNNING});
00109 stop_server_ = registerStateChangeRequest("stop", {State::STOPPED});
00110 pause_server_ = registerStateChangeRequest("pause", {State::PAUSED});
00111
00112 float heartbeat_rate;
00113 ros::param::param<float>("~heartbeat_rate", heartbeat_rate, 1.0f);
00114 ROS_INFO("heartbeat_rate = %.3f [Hz]", heartbeat_rate);
00115
00116 IsolatedAsyncTimer::LambdaCallback heartbeat_callback = [this]()
00117 {
00118 notifyState();
00119 };
00120 heartbeat_timer_ = std::make_shared<IsolatedAsyncTimer>(
00121 *node_handle_private_,
00122 heartbeat_callback,
00123 heartbeat_rate,
00124 false);
00125
00126 ros::param::param<bool>("~autostart", autostart_, false);
00127
00128 state_request_spinner_ = std::make_shared<ros::AsyncSpinner>(1, &state_request_callback_queue_);
00129 state_request_spinner_->start();
00130
00131 autostart_ = autostart_ || autostart;
00132 ROS_INFO_STREAM("autostart = " << std::boolalpha << autostart_);
00133
00134 if (autostart_)
00135 transitionToState(State::RUNNING);
00136 else
00137 transitionToState(State::STOPPED);
00138
00139 return *this;
00140 }
00141
00142 void RobotProcess::run(uint8_t threads)
00143 {
00144 runAsync(threads);
00145 ros::waitForShutdown();
00146 }
00147
00148 void RobotProcess::runAsync(uint8_t threads)
00149 {
00150 global_callback_queue_spinner_ = std::make_shared<ros::AsyncSpinner>(threads);
00151 global_callback_queue_spinner_->start();
00152 }
00153
00154 State RobotProcess::getState()
00155 {
00156 return current_state_;
00157 }
00158
00159 void RobotProcess::notifyError(uint8_t error_type,
00160 const std::string& function,
00161 const std::string& description)
00162 {
00163 ROS_DEBUG_STREAM("Publishing error msg with code: "
00164 << error_type << " function: " << function
00165 << " description: " << description);
00166 robot_process_msgs::Error error_msg;
00167 error_msg.header.stamp = ros::Time::now();
00168 error_msg.node_name = getNamespace();
00169 error_msg.error_type = error_type;
00170 error_msg.function = function;
00171 error_msg.description = description;
00172 process_error_pub_.publish(error_msg);
00173 }
00174
00175 void RobotProcess::registerIsolatedTimer(
00176 const IsolatedAsyncTimer::LambdaCallback& callback,
00177 const float& frequency,
00178 bool stoppable)
00179 {
00180 process_timers_.emplace_back(
00181 std::make_shared<IsolatedAsyncTimer>(
00182 *node_handle_private_,
00183 callback,
00184 frequency,
00185 stoppable,
00186 false,
00187 false));
00188 }
00189
00190 std::string RobotProcess::getNamespace() const
00191 {
00192 if (node_handle_private_)
00193 return node_handle_private_->getNamespace();
00194 else
00195 return std::string();
00196 }
00197
00198
00199 void RobotProcess::create()
00200 {
00201 PRINT_FUNC_CALL("create");
00202 onCreate();
00203 }
00204
00205 void RobotProcess::terminate()
00206 {
00207 PRINT_FUNC_CALL("terminate");
00208 onTerminate();
00209
00210
00211 }
00212
00213 void RobotProcess::configure()
00214 {
00215 PRINT_FUNC_CALL("configure");
00216 onConfigure();
00217 }
00218
00219 void RobotProcess::unconfigure()
00220 {
00221 PRINT_FUNC_CALL("unconfigure");
00222 onUnconfigure();
00223 }
00224
00225 void RobotProcess::start()
00226 {
00227 PRINT_FUNC_CALL("start");
00228 for (const auto & timer : process_timers_)
00229 {
00230 ROS_DEBUG("Starting timer");
00231 timer->start();
00232 }
00233 onStart();
00234 }
00235
00236 void RobotProcess::stop()
00237 {
00238 PRINT_FUNC_CALL("stop");
00239 for (const auto & timer : process_timers_)
00240 {
00241 ROS_DEBUG("Stopping timer");
00242 timer->stop();
00243 }
00244 onStop();
00245 }
00246
00247 void RobotProcess::resume()
00248 {
00249 PRINT_FUNC_CALL("resume");
00250 for (const auto & timer : process_timers_)
00251 {
00252 ROS_DEBUG("Resuming timer");
00253 timer->resume();
00254 }
00255 onResume();
00256 }
00257
00258 void RobotProcess::pause()
00259 {
00260 PRINT_FUNC_CALL("pause");
00261 for (const auto & timer : process_timers_)
00262 {
00263 ROS_DEBUG("Pausing timer");
00264 timer->pause();
00265 }
00266 onPause();
00267 }
00268
00269 ros::ServiceServer RobotProcess::registerStateChangeRequest(
00270 const std::string& service_name,
00271 const std::vector<State>& states)
00272 {
00273 ROS_DEBUG_STREAM(
00274 "Registering state transition request for state " << service_name);
00275 using std_srvs::Empty;
00276 EmptyServiceCallback callback = [ = ](Empty::Request & req, Empty::Response & res)
00277 {
00278 bool success = true;
00279 for (const auto & s : states)
00280 {
00281 success = success && transitionToState(s);
00282 }
00283 return success;
00284 };
00285
00286 auto options = ros::AdvertiseServiceOptions::create<Empty>(
00287 service_name,
00288 callback,
00289 ros::VoidConstPtr(),
00290 &state_request_callback_queue_);
00291
00292 return node_handle_private_->advertiseService(options);
00293 }
00294
00295 void RobotProcess::notifyState() const
00296 {
00297 ROS_DEBUG("Heartbeat sent!");
00298 robot_process_msgs::State state_msg;
00299 state_msg.header.stamp = ros::Time::now();
00300 state_msg.node_name = getNamespace();
00301 state_msg.state = static_cast<uint8_t>(current_state_);
00302 process_state_pub_.publish(state_msg);
00303 }
00304
00305 bool RobotProcess::transitionToState(const State& goal_state)
00306 {
00307 if (current_state_ == goal_state)
00308 {
00309 ROS_WARN_STREAM("Node is already at state " << goal_state);
00310 return false;
00311 }
00312
00313 while (current_state_ != goal_state)
00314 {
00315 auto from_state = static_cast<uint8_t>(current_state_);
00316 auto to_state = static_cast<uint8_t>(goal_state);
00317 State next_state = STATE_TRANSITIONS_PATHS[from_state][to_state];
00318 if (next_state == State::INVALID)
00319 {
00320 ROS_WARN_STREAM("There is no transition path from [" << current_state_
00321 << "] to [" << goal_state << "]");
00322 return false;
00323 }
00324 changeState(next_state);
00325 }
00326 return true;
00327 }
00328
00329 void RobotProcess::changeState(const State& new_state)
00330 {
00331 uint8_t from_state = static_cast<uint8_t>(current_state_);
00332 uint8_t to_state = static_cast<uint8_t>(new_state);
00333 MemberLambdaCallback callback = STATE_TRANSITIONS[from_state][to_state];
00334 if (callback == nullptr)
00335 {
00336 ROS_FATAL_STREAM_ONCE(
00337 "Tried changing state from [" << current_state_
00338 << "] to [" << new_state << "]. Transition does NOT exist!");
00339 return;
00340 }
00341 ROS_DEBUG_STREAM(
00342 "Changing state from [" << current_state_ << "] to ["
00343 << new_state << "]");
00344 current_state_ = new_state;
00345
00346 boost::bind(callback, this)();
00347 notifyState();
00348 }
00349
00350 std::ostream& operator<<(std::ostream& os, State state)
00351 {
00352 switch (state)
00353 {
00354 case State::INVALID :
00355 os << "INVALID";
00356 break;
00357 case State::LAUNCHING :
00358 os << "LAUNCHING";
00359 break;
00360 case State::UNCONFIGURED :
00361 os << "UNCONFIGURED";
00362 break;
00363 case State::STOPPED :
00364 os << "STOPPED";
00365 break;
00366 case State::PAUSED :
00367 os << "PAUSED";
00368 break;
00369 case State::RUNNING :
00370 os << "RUNNING";
00371 break;
00372 case State::TERMINATED :
00373 os << "TERMINATED";
00374 break;
00375 default :
00376 os.setstate(std::ios_base::failbit);
00377 }
00378 return os;
00379 }
00380
00381 const RobotProcess::StateTransitions RobotProcess::STATE_TRANSITIONS =
00382 {
00383
00384 {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr},
00385
00386 {nullptr, nullptr, &RobotProcess::create, nullptr, nullptr, nullptr, nullptr},
00387
00388 {nullptr, nullptr, nullptr, &RobotProcess::configure, nullptr, nullptr, &RobotProcess::terminate},
00389
00390 {nullptr, nullptr, &RobotProcess::unconfigure, nullptr, &RobotProcess::start, nullptr, nullptr},
00391
00392 {nullptr, nullptr, nullptr, &RobotProcess::stop, nullptr, &RobotProcess::resume, nullptr},
00393
00394 {nullptr, nullptr, nullptr, nullptr, &RobotProcess::pause, nullptr, nullptr},
00395
00396 {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}
00397 };
00398
00399
00400 const RobotProcess::StateTransitionPaths RobotProcess::STATE_TRANSITIONS_PATHS =
00401 {
00402 {
00403
00404 State::INVALID, State::INVALID, State::INVALID,
00405 State::INVALID, State::INVALID, State::INVALID,
00406 State::INVALID
00407 },
00408 {
00409
00410 State::INVALID, State::LAUNCHING, State::UNCONFIGURED,
00411 State::UNCONFIGURED, State::UNCONFIGURED, State::UNCONFIGURED,
00412 State::UNCONFIGURED
00413 },
00414 {
00415
00416 State::INVALID, State::INVALID, State::UNCONFIGURED,
00417 State::STOPPED, State::STOPPED, State::STOPPED,
00418 State::TERMINATED
00419 },
00420 {
00421
00422 State::INVALID, State::INVALID, State::UNCONFIGURED,
00423 State::STOPPED, State::PAUSED, State::PAUSED,
00424 State::UNCONFIGURED
00425 },
00426 {
00427
00428 State::INVALID, State::INVALID, State::STOPPED,
00429 State::STOPPED, State::PAUSED, State::RUNNING,
00430 State::STOPPED
00431 },
00432 {
00433
00434 State::INVALID, State::INVALID, State::PAUSED,
00435 State::PAUSED, State::PAUSED, State::PAUSED,
00436 State::PAUSED
00437 },
00438 {
00439
00440 State::INVALID, State::INVALID, State::INVALID,
00441 State::INVALID, State::INVALID, State::INVALID,
00442 State::INVALID
00443 }
00444 };
00445
00446 }