Return time which was taken to compute the motion plan or execute recovery behaviour (#1829)

Author: abrzozowski

nav2_msgs/action/BackUp.action

@@ -3,6 +3,6 @@ geometry_msgs/Point target
 float32 speed
 ---
 #result definition
-std_msgs/Empty result
+builtin_interfaces/Duration total_elapsed_time
 ---
 float32 distance_traveled

nav2_msgs/action/ComputePathToPose.action

@@ -4,5 +4,6 @@ string planner_id
 ---
 #result definition
 nav_msgs/Path path
+builtin_interfaces/Duration planning_time
 ---
 #feedback

nav2_msgs/action/DummyRecovery.action

@@ -2,6 +2,6 @@
 std_msgs/String command
 ---
 #result definition
-std_msgs/Empty result
+builtin_interfaces/Duration total_elapsed_time
 ---
 #feedback

nav2_msgs/action/Spin.action

@@ -2,6 +2,6 @@
 float32 target_yaw
 ---
 #result definition
-std_msgs/Empty result
+builtin_interfaces/Duration total_elapsed_time
 ---
 float32 angular_distance_traveled

nav2_msgs/action/Wait.action

@@ -2,7 +2,7 @@
 builtin_interfaces/Duration time
 ---
 #result definition
-std_msgs/Empty result
+builtin_interfaces/Duration total_elapsed_time
 ---
 #feedback
 builtin_interfaces/Duration time_left

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -122,6 +122,9 @@ class PlannerServer : public nav2_util::LifecycleNode
   double max_planner_duration_;
   std::string planner_ids_concat_;
 
+  // Clock
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+
   // TF buffer
   std::shared_ptr<tf2_ros::Buffer> tf_;
 

nav2_planner/src/planner_server.cpp

@@ -207,7 +207,7 @@ PlannerServer::on_shutdown(const rclcpp_lifecycle::State &)
 void
 PlannerServer::computePlan()
 {
-  auto start_time = now();
+  auto start_time = steady_clock_.now();
 
   // Initialize the ComputePathToPose goal and result
   auto goal = action_server_->get_current_goal();
@@ -285,16 +285,18 @@ PlannerServer::computePlan()
     RCLCPP_DEBUG(get_logger(), "Publishing the valid path");
     publishPlan(result->path);
 
-    action_server_->succeeded_current(result);
+    auto cycle_duration = steady_clock_.now() - start_time;
+    result->planning_time = cycle_duration;
 
-    auto cycle_duration = (now() - start_time).seconds();
-    if (max_planner_duration_ && cycle_duration > max_planner_duration_) {
+    if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
       RCLCPP_WARN(
         get_logger(),
         "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
-        1 / max_planner_duration_, 1 / cycle_duration);
+        1 / max_planner_duration_, 1 / cycle_duration.seconds());
     }
 
+    action_server_->succeeded_current(result);
+
     return;
   } catch (std::exception & ex) {
     RCLCPP_WARN(

nav2_recoveries/include/nav2_recoveries/recovery.hpp

@@ -147,6 +147,9 @@ class Recovery : public nav2_core::Recovery
   std::string robot_base_frame_;
   double transform_tolerance_;
 
+  // Clock
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
+
   void execute()
   {
     RCLCPP_INFO(node_->get_logger(), "Attempting %s", recovery_name_.c_str());
@@ -167,13 +170,19 @@ class Recovery : public nav2_core::Recovery
       1s,
       [&]() {RCLCPP_INFO(node_->get_logger(), "%s running...", recovery_name_.c_str());});
 
+    auto start_time = steady_clock_.now();
+
+    // Initialize the ActionT result
+    auto result = std::make_shared<typename ActionT::Result>();
+
     rclcpp::Rate loop_rate(cycle_frequency_);
 
     while (rclcpp::ok()) {
       if (action_server_->is_cancel_requested()) {
         RCLCPP_INFO(node_->get_logger(), "Canceling %s", recovery_name_.c_str());
         stopRobot();
-        action_server_->terminate_all();
+        result->total_elapsed_time = steady_clock_.now() - start_time;
+        action_server_->terminate_all(result);
         return;
       }
 
@@ -184,19 +193,22 @@ class Recovery : public nav2_core::Recovery
           " however feature is currently not implemented. Aborting and stopping.",
           recovery_name_.c_str());
         stopRobot();
-        action_server_->terminate_current();
+        result->total_elapsed_time = steady_clock_.now() - start_time;
+        action_server_->terminate_current(result);
         return;
       }
 
       switch (onCycleUpdate()) {
         case Status::SUCCEEDED:
           RCLCPP_INFO(node_->get_logger(), "%s completed successfully", recovery_name_.c_str());
-          action_server_->succeeded_current();
+          result->total_elapsed_time = steady_clock_.now() - start_time;
+          action_server_->succeeded_current(result);
           return;
 
         case Status::FAILED:
           RCLCPP_WARN(node_->get_logger(), "%s failed", recovery_name_.c_str());
-          action_server_->terminate_current();
+          result->total_elapsed_time = steady_clock_.now() - start_time;
+          action_server_->terminate_current(result);
           return;
 
         case Status::RUNNING:

nav2_recoveries/test/test_recoveries.cpp

@@ -232,6 +232,27 @@ TEST_F(RecoveryTest, testingSequentialFailures)
   SUCCEED();
 }
 
+TEST_F(RecoveryTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted)
+{
+  ASSERT_TRUE(sendCommand("Testing success"));
+  EXPECT_GT(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
+TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnInit)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on init"));
+  EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
+TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnRun)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on run"));
+  EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);