From 08efe56166b527dfe1f5c09e4fbddbfe4d3e6c96 Mon Sep 17 00:00:00 2001 From: abrzozowski Date: Fri, 26 Jun 2020 00:02:32 +0200 Subject: [PATCH] Return time which was taken to compute the motion plan or execute recovery behaviour --- nav2_msgs/action/BackUp.action | 2 +- nav2_msgs/action/ComputePathToPose.action | 1 + nav2_msgs/action/DummyRecovery.action | 2 +- nav2_msgs/action/Spin.action | 2 +- nav2_msgs/action/Wait.action | 2 +- .../include/nav2_planner/planner_server.hpp | 3 +++ nav2_planner/src/planner_server.cpp | 12 ++++++----- .../include/nav2_recoveries/recovery.hpp | 20 ++++++++++++++---- nav2_recoveries/test/test_recoveries.cpp | 21 +++++++++++++++++++ 9 files changed, 52 insertions(+), 13 deletions(-) diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 906bdd9fffd..996eb838645 100644 --- a/nav2_msgs/action/BackUp.action +++ b/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 diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index a9abf48ba9d..add60a824dc 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -4,5 +4,6 @@ string planner_id --- #result definition nav_msgs/Path path +builtin_interfaces/Duration planning_time --- #feedback diff --git a/nav2_msgs/action/DummyRecovery.action b/nav2_msgs/action/DummyRecovery.action index 7220178c375..78fabe0b81e 100644 --- a/nav2_msgs/action/DummyRecovery.action +++ b/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 diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index d6c926695e3..2f498edcebe 100644 --- a/nav2_msgs/action/Spin.action +++ b/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 diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 63dd38d4114..ed651226c61 100644 --- a/nav2_msgs/action/Wait.action +++ b/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 diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 813c509d1be..930ad01a5d2 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -119,6 +119,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 tf_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 484faa0f868..8e1b74a3c52 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/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( diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 5583a19d4dc..04aed1b02a1 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/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(); + 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: diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index 137e3500e41..a838803f87b 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/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);