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 <gtest/gtest.h>
00038
00039 #include <ros/ros.h>
00040 #include <robot_process/robot_process.h>
00041 #include <robot_process_msgs/State.h>
00042
00043 #include <std_srvs/Empty.h>
00044
00045 #include <vector>
00046 #include <string>
00047
00048 using robot_process::RobotProcess;
00049 using robot_process::State;
00050 using robot_process::IsolatedAsyncTimer;
00051
00052 class AnyRobotProcess : public RobotProcess
00053 {
00054 public:
00055 using RobotProcess::RobotProcess;
00056 ~AnyRobotProcess() {}
00057 private:
00058 void onCreate() override {};
00059 void onTerminate() override {};
00060
00061 void onConfigure() override {};
00062 void onUnconfigure() override {};
00063
00064 void onStart() override {};
00065 void onStop() override {};
00066
00067 void onResume() override {};
00068 void onPause() override {};
00069 };
00070
00071 class AnyRobotProcessWithTimer : public AnyRobotProcess
00072 {
00073 public:
00074 using AnyRobotProcess::AnyRobotProcess;
00075 int context = 0;
00076 bool stoppable = true;
00077 private:
00078 void onCreate() override
00079 {
00080 IsolatedAsyncTimer::LambdaCallback cb = [this]() { context++; };
00081 registerIsolatedTimer(cb, 1, stoppable);
00082 }
00083 };
00084
00085 TEST(RobotProcessTests, RemappedNameInitializedStateAndNamespace)
00086 {
00087 int argc = 2;
00088 const char* argv[2];
00089 argv[0] = "random_process_name";
00090 argv[1] = "__name:=remapped_name";
00091
00092 AnyRobotProcess test(argc, const_cast<char**>(argv));
00093 EXPECT_EQ(test.getState(), State::LAUNCHING);
00094 EXPECT_EQ(test.getNamespace(), std::string(""));
00095
00096 boost::function<void(const robot_process_msgs::State::ConstPtr&)> empty_cb =
00097 [](const robot_process_msgs::State::ConstPtr &s){};
00098
00099 ros::NodeHandle nh;
00100 auto sub = nh.subscribe("/heartbeat", 1, empty_cb);
00101
00102 test.init();
00103 EXPECT_EQ(test.getState(), State::STOPPED);
00104 EXPECT_EQ(test.getNamespace(), std::string("/remapped_name"));
00105 }
00106
00107 TEST(RobotProcessTests, InitializedNonWaitingStateAndNamespace)
00108 {
00109 int argc = 2;
00110 const char* argv[2];
00111 argv[0] = "random_process_name";
00112 argv[1] = "__name:=remapped_name";
00113
00114 ros::NodeHandle nh;
00115 nh.setParam("/remapped_name/wait_for_supervisor", false);
00116
00117 AnyRobotProcess test(argc, const_cast<char**>(argv));
00118 EXPECT_EQ(test.getState(), State::LAUNCHING);
00119 EXPECT_EQ(test.getNamespace(), std::string(""));
00120 test.init();
00121 EXPECT_EQ(test.getState(), State::STOPPED);
00122 EXPECT_EQ(test.getNamespace(), std::string("/remapped_name"));
00123 }
00124
00125 TEST(RobotProcessTests, AutostartNonWaitingStateAndNamespace)
00126 {
00127 int argc = 2;
00128 const char* argv[2];
00129 argv[0] = "random_process_name";
00130 argv[1] = "__name:=remapped_name";
00131
00132 ros::NodeHandle nh;
00133 nh.setParam("/remapped_name/wait_for_supervisor", false);
00134 nh.setParam("/remapped_name/autostart", true);
00135
00136 AnyRobotProcess test(argc, const_cast<char**>(argv));
00137 EXPECT_EQ(test.getState(), State::LAUNCHING);
00138 EXPECT_EQ(test.getNamespace(), std::string(""));
00139 test.init();
00140 EXPECT_EQ(test.getState(), State::RUNNING);
00141 EXPECT_EQ(test.getNamespace(), std::string("/remapped_name"));
00142 }
00143
00144 TEST(RobotProcessTests, StartStopServiceState)
00145 {
00146 int argc = 2;
00147 const char* argv[2];
00148 argv[0] = "random_process_name";
00149 argv[1] = "__name:=remapped_name";
00150
00151 ros::NodeHandle nh;
00152 nh.setParam("/remapped_name/wait_for_supervisor", false);
00153 nh.setParam("/remapped_name/autostart", false);
00154
00155 AnyRobotProcess test(argc, const_cast<char**>(argv));
00156 EXPECT_EQ(test.getState(), State::LAUNCHING);
00157 test.init();
00158 test.runAsync();
00159 EXPECT_EQ(test.getState(), State::STOPPED);
00160
00161 auto start = nh.serviceClient<std_srvs::Empty>("/remapped_name/start");
00162 std_srvs::Empty start_empty;
00163 EXPECT_EQ(start.call(start_empty), true);
00164 EXPECT_EQ(test.getState(), State::RUNNING);
00165
00166 auto stop = nh.serviceClient<std_srvs::Empty>("/remapped_name/stop");
00167 std_srvs::Empty stop_empty;
00168 EXPECT_EQ(stop.call(stop_empty), true);
00169 EXPECT_EQ(test.getState(), State::STOPPED);
00170 }
00171
00172 TEST(RobotProcessTests, PauseResumeServiceState)
00173 {
00174 int argc = 2;
00175 const char* argv[2];
00176 argv[0] = "random_process_name";
00177 argv[1] = "__name:=remapped_name";
00178
00179 ros::NodeHandle nh;
00180 nh.setParam("/remapped_name/wait_for_supervisor", false);
00181 nh.setParam("/remapped_name/autostart", true);
00182
00183 AnyRobotProcess test(argc, const_cast<char**>(argv));
00184 EXPECT_EQ(test.getState(), State::LAUNCHING);
00185 test.init();
00186 test.runAsync();
00187 EXPECT_EQ(test.getState(), State::RUNNING);
00188
00189 auto pause = nh.serviceClient<std_srvs::Empty>("/remapped_name/pause");
00190 std_srvs::Empty pause_empty;
00191 EXPECT_EQ(pause.call(pause_empty), true);
00192 EXPECT_EQ(test.getState(), State::PAUSED);
00193
00194 auto start = nh.serviceClient<std_srvs::Empty>("/remapped_name/start");
00195 std_srvs::Empty start_empty;
00196 EXPECT_EQ(start.call(start_empty), true);
00197 EXPECT_EQ(test.getState(), State::RUNNING);
00198 }
00199
00200 TEST(RobotProcessTests, RestartServiceState)
00201 {
00202 int argc = 2;
00203 const char* argv[2];
00204 argv[0] = "random_process_name";
00205 argv[1] = "__name:=remapped_name";
00206
00207 ros::NodeHandle nh;
00208 nh.setParam("/remapped_name/wait_for_supervisor", false);
00209 nh.setParam("/remapped_name/autostart", true);
00210
00211 AnyRobotProcess test(argc, const_cast<char**>(argv));
00212 EXPECT_EQ(test.getState(), State::LAUNCHING);
00213 test.init();
00214 test.runAsync();
00215 EXPECT_EQ(test.getState(), State::RUNNING);
00216
00217 int counter = 0;
00218 std::vector<State> expected_transitions
00219 {
00220 State::RUNNING,
00221 State::PAUSED,
00222 State::STOPPED,
00223 State::PAUSED,
00224 State::RUNNING
00225 };
00226
00227 boost::function<void(const robot_process_msgs::State::ConstPtr&)> check_transitions_cb =
00228 [&](const robot_process_msgs::State::ConstPtr &s)
00229 {
00230 EXPECT_EQ(s->state, static_cast<uint8_t>(expected_transitions[counter++]));
00231 };
00232
00233 auto sub = nh.subscribe("/heartbeat", 0, check_transitions_cb);
00234 auto restart = nh.serviceClient<std_srvs::Empty>("/remapped_name/restart");
00235 std_srvs::Empty restart_empty;
00236 EXPECT_EQ(restart.call(restart_empty), true);
00237 EXPECT_EQ(test.getState(), State::RUNNING);
00238 }
00239
00240 TEST(RobotProcessTests, ReconfigureServiceState)
00241 {
00242 int argc = 2;
00243 const char* argv[2];
00244 argv[0] = "random_process_name";
00245 argv[1] = "__name:=remapped_name";
00246
00247 ros::NodeHandle nh;
00248 nh.setParam("/remapped_name/autostart_after_reconfigure", true);
00249 nh.setParam("/remapped_name/wait_for_supervisor", false);
00250 nh.setParam("/remapped_name/autostart", true);
00251
00252 AnyRobotProcess test(argc, const_cast<char**>(argv));
00253 EXPECT_EQ(test.getState(), State::LAUNCHING);
00254 test.init();
00255 test.runAsync();
00256 EXPECT_EQ(test.getState(), State::RUNNING);
00257
00258 int counter = 0;
00259 std::vector<State> expected_transitions = {
00260 State::RUNNING,
00261 State::PAUSED,
00262 State::STOPPED,
00263 State::UNCONFIGURED,
00264 State::STOPPED,
00265 State::PAUSED,
00266 State::RUNNING
00267 };
00268
00269 boost::function<void(const robot_process_msgs::State::ConstPtr&)> check_transitions_cb =
00270 [&](const robot_process_msgs::State::ConstPtr &s)
00271 {
00272 EXPECT_EQ(s->state, static_cast<uint8_t>(expected_transitions[counter++]));
00273 };
00274
00275 auto sub = nh.subscribe("/heartbeat", 0, check_transitions_cb);
00276 auto reconfigure = nh.serviceClient<std_srvs::Empty>("/remapped_name/reconfigure");
00277 std_srvs::Empty reconfigure_empty;
00278 EXPECT_EQ(reconfigure.call(reconfigure_empty), true);
00279 EXPECT_EQ(test.getState(), State::RUNNING);
00280 }
00281
00282 TEST(RobotProcessTests, NoAutostartAfterReconfigureServiceState)
00283 {
00284 int argc = 2;
00285 const char* argv[2];
00286 argv[0] = "random_process_name";
00287 argv[1] = "__name:=remapped_name";
00288
00289 ros::NodeHandle nh;
00290 nh.setParam("/remapped_name/autostart_after_reconfigure", false);
00291 nh.setParam("/remapped_name/wait_for_supervisor", false);
00292 nh.setParam("/remapped_name/autostart", true);
00293
00294 AnyRobotProcess test(argc, const_cast<char**>(argv));
00295 EXPECT_EQ(test.getState(), State::LAUNCHING);
00296 test.init();
00297 test.runAsync();
00298 EXPECT_EQ(test.getState(), State::RUNNING);
00299
00300 auto reconfigure = nh.serviceClient<std_srvs::Empty>("/remapped_name/reconfigure");
00301 std_srvs::Empty reconfigure_empty;
00302 EXPECT_EQ(reconfigure.call(reconfigure_empty), true);
00303 EXPECT_EQ(test.getState(), State::STOPPED);
00304 }
00305
00306 TEST(RobotProcessTests, TerminateServiceState)
00307 {
00308 int argc = 2;
00309 const char* argv[2];
00310 argv[0] = "random_process_name";
00311 argv[1] = "__name:=remapped_name";
00312
00313 ros::NodeHandle nh;
00314 nh.setParam("/remapped_name/wait_for_supervisor", false);
00315 nh.setParam("/remapped_name/autostart", false);
00316
00317 AnyRobotProcess test(argc, const_cast<char**>(argv));
00318 EXPECT_EQ(test.getState(), State::LAUNCHING);
00319 test.init();
00320 test.runAsync();
00321 EXPECT_EQ(test.getState(), State::STOPPED);
00322
00323 auto terminate = nh.serviceClient<std_srvs::Empty>("/remapped_name/terminate");
00324 std_srvs::Empty terminate_empty;
00325 EXPECT_EQ(terminate.call(terminate_empty), true);
00326 EXPECT_EQ(test.getState(), State::TERMINATED);
00327 }
00328
00329 TEST(RobotProcessTests, IsolatedAsyncTimer)
00330 {
00331 int argc = 2;
00332 const char* argv[2];
00333 argv[0] = "random_process_name";
00334 argv[1] = "__name:=remapped_name";
00335
00336 ros::NodeHandle nh;
00337 nh.setParam("/remapped_name/wait_for_supervisor", false);
00338 nh.setParam("/remapped_name/autostart", true);
00339
00340 AnyRobotProcessWithTimer test(argc, const_cast<char**>(argv));
00341 test.init().runAsync();
00342 ros::Duration(2.1).sleep();
00343 EXPECT_EQ(test.context, 2);
00344 }
00345
00346
00347 TEST(RobotProcessTests, StoppableIsolatedAsyncTimer)
00348 {
00349 int argc = 2;
00350 const char* argv[2];
00351 argv[0] = "random_process_name";
00352 argv[1] = "__name:=remapped_name";
00353
00354 ros::NodeHandle nh;
00355 nh.setParam("/remapped_name/wait_for_supervisor", false);
00356 nh.setParam("/remapped_name/autostart", true);
00357
00358 auto start = nh.serviceClient<std_srvs::Empty>("/remapped_name/start");
00359 auto pause = nh.serviceClient<std_srvs::Empty>("/remapped_name/pause");
00360 std_srvs::Empty pause_empty, start_empty;
00361
00362 AnyRobotProcessWithTimer test(argc, const_cast<char**>(argv));
00363 test.init().runAsync();
00364
00365 ros::Duration(1.1).sleep();
00366 EXPECT_EQ(test.context, 1);
00367 EXPECT_EQ(pause.call(pause_empty), true);
00368 ros::Duration(1.0).sleep();
00369 EXPECT_EQ(test.context, 1);
00370 EXPECT_EQ(start.call(start_empty), true);
00371 ros::Duration(1.1).sleep();
00372 EXPECT_EQ(test.context, 2);
00373 }
00374
00375 TEST(RobotProcessTests, NonStoppableIsolatedAsyncTimer)
00376 {
00377 int argc = 2;
00378 const char* argv[2];
00379 argv[0] = "random_process_name";
00380 argv[1] = "__name:=remapped_name";
00381
00382 ros::NodeHandle nh;
00383 nh.setParam("/remapped_name/wait_for_supervisor", false);
00384 nh.setParam("/remapped_name/autostart", true);
00385
00386 auto start = nh.serviceClient<std_srvs::Empty>("/remapped_name/start");
00387 auto stop = nh.serviceClient<std_srvs::Empty>("/remapped_name/stop");
00388 std_srvs::Empty stop_empty, start_empty;
00389
00390 AnyRobotProcessWithTimer test(argc, const_cast<char**>(argv));
00391 test.stoppable = false;
00392 test.init().runAsync();
00393
00394 ros::Duration(1.1).sleep();
00395 EXPECT_EQ(test.context, 1);
00396 EXPECT_EQ(stop.call(stop_empty), true);
00397 ros::Duration(1.1).sleep();
00398 EXPECT_EQ(test.context, 2);
00399 EXPECT_EQ(start.call(start_empty), true);
00400 ros::Duration(1.1).sleep();
00401 EXPECT_EQ(test.context, 3);
00402 }
00403
00404 int main(int argc, char **argv)
00405 {
00406 testing::InitGoogleTest(&argc, argv);
00407 return RUN_ALL_TESTS();
00408 }