Fix concurrent transition callbacks

robot_activity/src/robot_activity.cpp

@@ -88,7 +88,14 @@ RobotActivity& RobotActivity::init(bool autostart)
   {
     ros::Rate poll_rate(100);
     while (process_state_pub_.getNumSubscribers() == 0 && ros::ok())
+    {
       poll_rate.sleep();
+    }
+    if (!ros::ok())
+    {
+      /* Return immediately if Ctrl+C was pressed */
+      return *this;
+    }
   }
 
   notifyState();
@@ -131,10 +138,19 @@ RobotActivity& RobotActivity::init(bool autostart)
   autostart_ = autostart_ || autostart;
   ROS_INFO_STREAM("autostart = " << std::boolalpha << autostart_);
 
+  std_srvs::Empty empty;
   if (autostart_)
-    transitionToState(State::RUNNING);
+  {
+    auto start = node_handle_private_->serviceClient<std_srvs::Empty>("robot_activity/start");
+    start.waitForExistence();
+    start.call(empty);
+  }
   else
-    transitionToState(State::STOPPED);
+  {
+    auto stop = node_handle_private_->serviceClient<std_srvs::Empty>("robot_activity/stop");
+    stop.waitForExistence();
+    stop.call(empty);
+  }
 
   return *this;
 }

robot_activity/test/robot_activity_tests.cpp

@@ -42,6 +42,7 @@
 
 #include <std_srvs/Empty.h>
 
+#include <thread>
 #include <vector>
 #include <string>
 
@@ -77,7 +78,11 @@ class AnyRobotActivityWithTimer : public AnyRobotActivity
 private:
   void onCreate() override
   {
-    IsolatedAsyncTimer::LambdaCallback cb = [this]() { context++; };
+    IsolatedAsyncTimer::LambdaCallback cb = [this]()
+    {
+      std::cout << "context: " << context << std::endl;
+      context++;
+    };
     registerIsolatedTimer(cb, 1, stoppable);
   }
 };
@@ -91,6 +96,19 @@ class OnStartCanFailRobotActivity : public AnyRobotActivity
   bool onStart() override { return can_start; }
 };
 
+class LongOnConfigureRobotActivity : public AnyRobotActivity
+{
+public:
+  using AnyRobotActivity::AnyRobotActivity;
+  float sleep_time = 3.0;
+private:
+  bool onConfigure() override
+  {
+    ros::Duration(sleep_time).sleep();
+    return true;
+  }
+};
+
 TEST(RobotActivityTests, RemappedNameInitializedStateAndNamespace)
 {
   int argc = 2;
@@ -349,6 +367,8 @@ TEST(RobotActivityTests, IsolatedAsyncTimer)
 
   AnyRobotActivityWithTimer test(argc, const_cast<char**>(argv));
   test.init().runAsync();
+  EXPECT_EQ(test.getState(), State::RUNNING);
+
   ros::Duration(2.1).sleep();
   EXPECT_EQ(test.context, 2);
 }
@@ -372,6 +392,8 @@ TEST(RobotActivityTests, StoppableIsolatedAsyncTimer)
   AnyRobotActivityWithTimer test(argc, const_cast<char**>(argv));
   test.init().runAsync();
 
+  EXPECT_EQ(test.getState(), State::RUNNING);
+
   ros::Duration(1.1).sleep();
   EXPECT_EQ(test.context, 1);
   EXPECT_EQ(pause.call(pause_empty), true);
@@ -401,6 +423,8 @@ TEST(RobotActivityTests, NonStoppableIsolatedAsyncTimer)
   test.stoppable = false;
   test.init().runAsync();
 
+  EXPECT_EQ(test.getState(), State::RUNNING);
+
   ros::Duration(1.1).sleep();
   EXPECT_EQ(test.context, 1);
   EXPECT_EQ(stop.call(stop_empty), true);
@@ -441,6 +465,33 @@ TEST(RobotActivityTests, OnStartCanFailRobotActivityTest)
   EXPECT_EQ(test.getState(), State::RUNNING);
 }
 
+TEST(RobotActivityTests, LongOnConfigureRobotActivityTest)
+{
+  int argc = 2;
+  const char* argv[2];
+  argv[0] = "random_process_name";
+  argv[1] = "__name:=remapped_name";
+
+  LongOnConfigureRobotActivity test(argc, const_cast<char**>(argv));
+
+  ros::NodeHandle nh;
+  nh.setParam("/remapped_name/wait_for_supervisor", false);
+  nh.setParam("/remapped_name/autostart", false);
+  auto start = nh.serviceClient<std_srvs::Empty>("/remapped_name/robot_activity/start");
+  std_srvs::Empty start_empty;
+  std::thread init_robot_activity([&] { test.init().runAsync(); });
+
+  start.waitForExistence();
+
+  auto before = ros::Time::now();
+  EXPECT_EQ(start.call(start_empty), true);
+  auto after = ros::Time::now();
+  EXPECT_EQ(test.getState(), State::RUNNING);
+  EXPECT_LT(abs((after - before).toSec() - test.sleep_time), 0.5);
+
+  init_robot_activity.join();
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);