From ee49eb5925ecbd765cce5d247ba316f2962182b1 Mon Sep 17 00:00:00 2001 From: Maciej Zurad Date: Mon, 30 Jul 2018 19:31:22 +0200 Subject: [PATCH 1/5] fixing a bug, where a long onConfigure or onCreate would allow someone to call /start, /stop, etc on top messing up state --- robot_activity/src/robot_activity.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/robot_activity/src/robot_activity.cpp b/robot_activity/src/robot_activity.cpp index 9bc8e53..5c75db6 100644 --- a/robot_activity/src/robot_activity.cpp +++ b/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("robot_activity/start"); + auto ret = start.call(empty); + ROS_INFO_STREAM("ret = " << std::boolalpha << ret); + } else - transitionToState(State::STOPPED); + { + auto stop = node_handle_private_->serviceClient("robot_activity/stop"); + auto ret = stop.call(empty); + ROS_INFO_STREAM("ret = " << std::boolalpha << ret); + } return *this; } From 8031c9abdefab2274ab5cb604cbb23b20513e1be Mon Sep 17 00:00:00 2001 From: Maciej Zurad Date: Mon, 30 Jul 2018 20:26:33 +0200 Subject: [PATCH 2/5] adding test --- robot_activity/test/robot_activity_tests.cpp | 41 ++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/robot_activity/test/robot_activity_tests.cpp b/robot_activity/test/robot_activity_tests.cpp index 17793be..70a5117 100644 --- a/robot_activity/test/robot_activity_tests.cpp +++ b/robot_activity/test/robot_activity_tests.cpp @@ -42,6 +42,7 @@ #include +#include #include #include @@ -91,6 +92,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; @@ -441,6 +455,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(argv)); + + ros::NodeHandle nh; + nh.setParam("/remapped_name/wait_for_supervisor", false); + nh.setParam("/remapped_name/autostart", false); + auto start = nh.serviceClient("/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); From fc3a9405b537f0f07bc9ed0d53da862111b68a4b Mon Sep 17 00:00:00 2001 From: Maciej Zurad Date: Mon, 30 Jul 2018 20:41:08 +0200 Subject: [PATCH 3/5] fixing regression --- robot_activity/src/robot_activity.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/robot_activity/src/robot_activity.cpp b/robot_activity/src/robot_activity.cpp index 5c75db6..40c8c7b 100644 --- a/robot_activity/src/robot_activity.cpp +++ b/robot_activity/src/robot_activity.cpp @@ -142,14 +142,14 @@ RobotActivity& RobotActivity::init(bool autostart) if (autostart_) { auto start = node_handle_private_->serviceClient("robot_activity/start"); - auto ret = start.call(empty); - ROS_INFO_STREAM("ret = " << std::boolalpha << ret); + start.waitForExistence(); + start.call(empty); } else { auto stop = node_handle_private_->serviceClient("robot_activity/stop"); - auto ret = stop.call(empty); - ROS_INFO_STREAM("ret = " << std::boolalpha << ret); + stop.waitForExistence(); + stop.call(empty); } return *this; From 5cd32072810874ff336780e31f3573f06e3ab8e9 Mon Sep 17 00:00:00 2001 From: Maciej Zurad Date: Tue, 31 Jul 2018 13:42:28 +0200 Subject: [PATCH 4/5] tests failing on travis, passing locally, debugging --- robot_activity/test/robot_activity_tests.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/robot_activity/test/robot_activity_tests.cpp b/robot_activity/test/robot_activity_tests.cpp index 70a5117..023414e 100644 --- a/robot_activity/test/robot_activity_tests.cpp +++ b/robot_activity/test/robot_activity_tests.cpp @@ -78,7 +78,11 @@ class AnyRobotActivityWithTimer : public AnyRobotActivity private: void onCreate() override { - IsolatedAsyncTimer::LambdaCallback cb = [this]() { context++; }; + IsolatedAsyncTimer::LambdaCallback cb = [this]() + { + ROS_INFO("context: %d", context); + context++; + }; registerIsolatedTimer(cb, 1, stoppable); } }; @@ -363,6 +367,8 @@ TEST(RobotActivityTests, IsolatedAsyncTimer) AnyRobotActivityWithTimer test(argc, const_cast(argv)); test.init().runAsync(); + EXPECT_EQ(test.getState(), State::RUNNING); + ros::Duration(2.1).sleep(); EXPECT_EQ(test.context, 2); } From ce68a6442daf629b29736b53b2121f4d1005ea08 Mon Sep 17 00:00:00 2001 From: Maciej Zurad Date: Tue, 31 Jul 2018 15:03:10 +0200 Subject: [PATCH 5/5] adding more test checks to narrow down the undeterministic error --- robot_activity/test/robot_activity_tests.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/robot_activity/test/robot_activity_tests.cpp b/robot_activity/test/robot_activity_tests.cpp index 023414e..0217426 100644 --- a/robot_activity/test/robot_activity_tests.cpp +++ b/robot_activity/test/robot_activity_tests.cpp @@ -80,7 +80,7 @@ class AnyRobotActivityWithTimer : public AnyRobotActivity { IsolatedAsyncTimer::LambdaCallback cb = [this]() { - ROS_INFO("context: %d", context); + std::cout << "context: " << context << std::endl; context++; }; registerIsolatedTimer(cb, 1, stoppable); @@ -392,6 +392,8 @@ TEST(RobotActivityTests, StoppableIsolatedAsyncTimer) AnyRobotActivityWithTimer test(argc, const_cast(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); @@ -421,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);