Introduce and use ROSNotOkException

pilz_control/test/pjtc_test_helper.h

@@ -30,6 +30,8 @@
 
 #include <control_msgs/FollowJointTrajectoryActionGoal.h>
 
+#include <pilz_testutils/ros_not_ok_exception.h>
+
 #include "robot_mock.h"
 
 namespace pilz_joint_trajectory_controller_test
@@ -107,13 +109,13 @@ static GoalType generateSimpleGoal(const ros::Duration& goal_duration = ros::Dur
 }
 
 /**
- * @brief Either return true, when the condition is fulfilled or false, when the timeout has been reached
- * or ROS is shutdown.
+ * @brief Either return true, when the condition is fulfilled or false, when the timeout has been reached.
  * @param is_condition_fulfilled Boolean function, which is expected to return true eventually.
  * @param timeout Timeout [ms] with respect to system time.
  * @param update_func Update function. If non-empty, the following is done periodically:
  * - Make progress in simulated ros::Time,
  * - Invoke update_func.
+ * @throws ROSNotOkException if ros::ok() returned false
  */
 static bool waitFor(const std::function<bool()>& is_condition_fulfilled, const std::chrono::milliseconds& timeout,
                     const UpdateFunc& update_func = UpdateFunc())
@@ -136,6 +138,12 @@ static bool waitFor(const std::function<bool()>& is_condition_fulfilled, const s
     }
     std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_TIME_MSEC));
   } while (ros::ok());
+
+  if (!ros::ok())
+  {
+    throw pilz_testutils::ROSNotOkException("Expected ros::ok() to be true while waiting for some condition.");
+  }
+
   return false;
 }
 

pilz_testutils/include/pilz_testutils/ros_not_ok_exception.h

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2020 Pilz GmbH & Co. KG
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ROS_NOT_OK_EXCEPTION_H
+#define ROS_NOT_OK_EXCEPTION_H
+
+#include <stdexcept>
+#include <string>
+
+namespace pilz_testutils
+{
+// @brief Exception to be thrown if ros::ok() is expected to but true, but it's not
+class ROSNotOkException : public std::runtime_error
+{
+public:
+  ROSNotOkException(const std::string& what_arg) : std::runtime_error(what_arg)
+  {
+  }
+};
+
+}  // namespace pilz_testutils
+
+#endif  // ROS_NOT_OK_EXCEPTION_H