chore, fix: formatting and minor fixes for planning pipeline timeout

moveit · Mar 6, 2024 · 167fb5e · 167fb5e
1 parent 06377f7
commit 167fb5e
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 15 deletions.
diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -282,17 +282,17 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
     {
       assert(req_adapter);
       RCLCPP_INFO(node_->get_logger(), "Calling PlanningRequestAdapter '%s'", req_adapter->getDescription().c_str());
-      const auto status = req_adapter->adapt(planning_scene, mutable_request);
+      auto status = req_adapter->adapt(planning_scene, mutable_request);
       res.error_code = status.val;
-      std::string message = status.message;
 
       // Publish progress
       publishPipelineState(mutable_request, res, req_adapter->getDescription());
 
       // check for timeout (mainly for sanity check since adapters are fast)
       if (std::chrono::duration<double>(clock::now() - plan_start_time).count() >= allowed_planning_time)
       {
-        message = "failed to finish within " + std::to_string(allowed_planning_time) + "s";
+        status.message =
+            "Failed to finish planning within allowed planning time ( " + std::to_string(allowed_planning_time) + "s )";
         res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT;
       }
 
@@ -311,11 +311,11 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
     {
       // modify planner request to notice plugins of their corresponding max_allowed_time
       // NOTE: currently just evenly distributing the remaining time among the remaining planners
-      double max_single_planner_time = std::chrono::duration<double>(clock::now() - plan_start_time).count() /
-                                       (pipeline_parameters_.planning_plugins.size() - i);
-      mutable_request.allowed_planning_time = max_single_planner_time;
+      mutable_request.allowed_planning_time =
+          (allowed_planning_time - std::chrono::duration<double>(clock::now() - plan_start_time).count()) /
+          (pipeline_parameters_.planning_plugins.size() - i);
 
-      const auto& planner = planner_map_.at(pipeline_parameters_.planning_plugins[i]);
+      const auto& planner = planner_map_.at(pipeline_parameters_.planning_plugins.at(i));
       // Update reference trajectory with latest solution (if available)
       if (res.trajectory)
       {

diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp
@@ -38,7 +38,6 @@
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_interface/planning_request_adapter.h>
 #include <moveit/planning_interface/planning_response_adapter.h>
-#include <moveit/utils/moveit_error_code.h>
 #include <class_loader/class_loader.hpp>
 
 namespace planning_pipeline_test

diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp
@@ -123,8 +123,6 @@ TEST_F(TestPlanningPipeline, TestTimeout)
   EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared<planning_pipeline::PlanningPipeline>(
                       robot_model_, node_, "", PLANNER_PLUGINS, REQUEST_ADAPTERS, RESPONSE_ADAPTERS));
 
-  // WHEN generatePlan is called
-  // THEN A successful response is created
   planning_interface::MotionPlanResponse motion_plan_response;
   planning_interface::MotionPlanRequest motion_plan_request;
   const auto planning_scene_ptr = std::make_shared<planning_scene::PlanningScene>(robot_model_);
@@ -135,14 +133,14 @@ TEST_F(TestPlanningPipeline, TestTimeout)
   EXPECT_EQ(motion_plan_response.error_code, moveit::core::MoveItErrorCode::TIMED_OUT);
 
   // timeout by planner
-  motion_plan_request.allowed_planning_time = 1;
+  motion_plan_request.allowed_planning_time = 1.0;
   EXPECT_FALSE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response));
   EXPECT_EQ(motion_plan_response.error_code, moveit::core::MoveItErrorCode::TIMED_OUT);
 
-  // passes
-  motion_plan_request.allowed_planning_time = 10;
-  EXPECT_TRUE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response));
-  EXPECT_EQ(motion_plan_response.error_code, moveit::core::MoveItErrorCode::SUCCESS);
+  // timeout by response adapter
+  motion_plan_request.allowed_planning_time = 2.3;
+  EXPECT_FALSE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response));
+  EXPECT_EQ(motion_plan_response.error_code, moveit::core::MoveItErrorCode::TIMED_OUT);
 }
 
 int main(int argc, char** argv)