From 3930be61801546ceb7717deb1557aa56c1ff14cf Mon Sep 17 00:00:00 2001 From: cc Date: Sun, 18 Feb 2024 06:11:56 +0000 Subject: [PATCH 1/7] Make planning pipeline respect the allowed_planning_time --- .../src/planning_pipeline.cpp | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index cbc3d20356..dd3b389e36 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -37,6 +37,9 @@ #include #include #include +#include +#include +#include namespace { @@ -267,10 +270,14 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // --------------------------------- // Solve the motion planning problem // --------------------------------- - planning_interface::MotionPlanRequest mutable_request = req; try { + using clock = std::chrono::system_clock; + const auto timeout_error = std::runtime_error("allowed_planning_time exceeded"); + const auto plan_start_time = clock::now(); + const double allowed_planning_time = req.allowed_planning_time; + // Call plan request adapter chain for (const auto& req_adapter : planning_request_adapter_vector_) { @@ -288,11 +295,24 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& req_adapter->getDescription().c_str(), status.message.c_str()); break; } + + // check for timeout + if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) + { + throw timeout_error; + } } + // modify planner request to notice plugins of their corresponding max_allowed_time + // NOTE: currently just evenly distributing the remaining time + const double max_single_planner_time = std::chrono::duration(clock::now() - plan_start_time).count() / + pipeline_parameters_.planning_plugins.size(); + mutable_request.allowed_planning_time = max_single_planner_time; + // Call planners for (const auto& planner_name : pipeline_parameters_.planning_plugins) { + auto planner_start_time = clock::now(); const auto& planner = planner_map_.at(planner_name); // Update reference trajectory with latest solution (if available) if (res.trajectory) @@ -323,6 +343,13 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription().c_str()); break; } + + // TODO: should this be optional since the plugins already checked for timeout? + // check for timeout + if (std::chrono::duration(clock::now() - planner_start_time).count() >= max_single_planner_time) + { + throw timeout_error; + } } // Call plan response adapter chain @@ -342,6 +369,12 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& res_adapter->getDescription().c_str()); break; } + + // check for timeout + if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) + { + throw timeout_error; + } } } } From e37e31411ef752cadc21a20e0542e6ff5c7147b3 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 28 Feb 2024 18:37:31 +0000 Subject: [PATCH 2/7] feat: timeout check set error_code instead of throw --- .../src/planning_pipeline.cpp | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index dd3b389e36..924f1ad524 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -274,7 +274,6 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& try { using clock = std::chrono::system_clock; - const auto timeout_error = std::runtime_error("allowed_planning_time exceeded"); const auto plan_start_time = clock::now(); const double allowed_planning_time = req.allowed_planning_time; @@ -285,8 +284,18 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& RCLCPP_INFO(node_->get_logger(), "Calling PlanningRequestAdapter '%s'", req_adapter->getDescription().c_str()); const 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(clock::now() - plan_start_time).count() >= allowed_planning_time) + { + message = "failed to finish within " + std::to_string(allowed_planning_time) + "s"; + res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT; + } + // If adapter does not succeed, break chain and return false if (!res.error_code) { @@ -295,12 +304,6 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& req_adapter->getDescription().c_str(), status.message.c_str()); break; } - - // check for timeout - if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) - { - throw timeout_error; - } } // modify planner request to notice plugins of their corresponding max_allowed_time @@ -312,7 +315,6 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // Call planners for (const auto& planner_name : pipeline_parameters_.planning_plugins) { - auto planner_start_time = clock::now(); const auto& planner = planner_map_.at(planner_name); // Update reference trajectory with latest solution (if available) if (res.trajectory) @@ -337,19 +339,18 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& context->solve(res); publishPipelineState(mutable_request, res, planner->getDescription()); + // check for overall timeout + if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) + { + res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT; + } + // If planner does not succeed, break chain and return false if (!res.error_code) { RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription().c_str()); break; } - - // TODO: should this be optional since the plugins already checked for timeout? - // check for timeout - if (std::chrono::duration(clock::now() - planner_start_time).count() >= max_single_planner_time) - { - throw timeout_error; - } } // Call plan response adapter chain @@ -362,6 +363,13 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& RCLCPP_INFO(node_->get_logger(), "Calling PlanningResponseAdapter '%s'", res_adapter->getDescription().c_str()); res_adapter->adapt(planning_scene, mutable_request, res); publishPipelineState(mutable_request, res, res_adapter->getDescription()); + + // check for timeout + if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) + { + res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT; + } + // If adapter does not succeed, break chain and return false if (!res.error_code) { @@ -369,12 +377,6 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& res_adapter->getDescription().c_str()); break; } - - // check for timeout - if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) - { - throw timeout_error; - } } } } From 69b6de86662422a9c7d301ebddcb7b907a37e3f6 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 28 Feb 2024 18:52:43 +0000 Subject: [PATCH 3/7] feat: evenly distribute the planning time among remaining planners --- .../planning_pipeline/src/planning_pipeline.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 924f1ad524..e411ea7cd9 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -306,16 +306,16 @@ 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 - const double max_single_planner_time = std::chrono::duration(clock::now() - plan_start_time).count() / - pipeline_parameters_.planning_plugins.size(); - mutable_request.allowed_planning_time = max_single_planner_time; - // Call planners - for (const auto& planner_name : pipeline_parameters_.planning_plugins) + for (size_t i = 0; i < pipeline_parameters_.planning_plugins.size(); i++) { - const auto& planner = planner_map_.at(planner_name); + // 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(clock::now() - plan_start_time).count() / + (pipeline_parameters_.planning_plugins.size() - i); + mutable_request.allowed_planning_time = max_single_planner_time; + + const auto& planner = planner_map_.at(pipeline_parameters_.planning_plugins[i]); // Update reference trajectory with latest solution (if available) if (res.trajectory) { From 06377f79a1fdf36e4d01a94c8af1bfd2a9f8a752 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 28 Feb 2024 21:26:01 +0000 Subject: [PATCH 4/7] test: add timeout handling test for planningpipeline --- .../test/planning_pipeline_test_plugins.cpp | 1 + .../test/planning_pipeline_tests.cpp | 30 +++++++++++++++++++ 2 files changed, 31 insertions(+) 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 index 1b5d8986ac..708d461982 100644 --- 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,6 +38,7 @@ #include #include #include +#include #include 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 index 7100a69f09..5614326d59 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -37,6 +37,7 @@ #include #include +#include #include namespace @@ -92,6 +93,7 @@ TEST_F(TestPlanningPipeline, HappyPath) // THEN A successful response is created planning_interface::MotionPlanResponse motion_plan_response; planning_interface::MotionPlanRequest motion_plan_request; + motion_plan_request.allowed_planning_time = 10; const auto planning_scene_ptr = std::make_shared(robot_model_); EXPECT_TRUE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response)); EXPECT_TRUE(motion_plan_response.error_code); @@ -115,6 +117,34 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) std::runtime_error); } +TEST_F(TestPlanningPipeline, TestTimeout) +{ + // construct pipline + EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared( + 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(robot_model_); + + // timeout by request adapter + motion_plan_request.allowed_planning_time = 0.1; + 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); + + // timeout by planner + motion_plan_request.allowed_planning_time = 1; + 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); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); From 167fb5e2894646adb7ce524be947c2d122549b55 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 6 Mar 2024 03:06:57 +0000 Subject: [PATCH 5/7] chore, fix: formatting and minor fixes for planning pipeline timeout --- .../planning_pipeline/src/planning_pipeline.cpp | 14 +++++++------- .../test/planning_pipeline_test_plugins.cpp | 1 - .../test/planning_pipeline_tests.cpp | 12 +++++------- 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index e411ea7cd9..69d47f64de 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -282,9 +282,8 @@ 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()); @@ -292,7 +291,8 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // check for timeout (mainly for sanity check since adapters are fast) if (std::chrono::duration(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(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(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 index 708d461982..1b5d8986ac 100644 --- 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 #include #include -#include #include 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 index 5614326d59..cfc3c95c5c 100644 --- 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( 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(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) From 99bf09c32fa4e7fa76cabb9db32cfa664e66bfa4 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 6 Mar 2024 15:00:40 +0000 Subject: [PATCH 6/7] fix spelling --- .../planning/planning_pipeline/test/planning_pipeline_tests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index cfc3c95c5c..bf2ca36766 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -119,7 +119,7 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) TEST_F(TestPlanningPipeline, TestTimeout) { - // construct pipline + // construct pipeline EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared( robot_model_, node_, "", PLANNER_PLUGINS, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); From cffe18865b015d2d281cce7afa7f6af80cae3bbe Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 28 Mar 2024 15:32:30 -0400 Subject: [PATCH 7/7] feat: allowing extra 10ms for response adapter --- .../planning/planning_pipeline/src/planning_pipeline.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 69d47f64de..ac636c0a69 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -356,6 +356,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // Call plan response adapter chain if (res.error_code) { + auto start_time = clock::now(); // Call plan request adapter chain for (const auto& res_adapter : planning_response_adapter_vector_) { @@ -365,7 +366,9 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& publishPipelineState(mutable_request, res, res_adapter->getDescription()); // check for timeout - if (std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) + // NOTE: reserving 10ms here + if (std::chrono::duration(clock::now() - start_time).count() >= 10.0 * 1e-3 && + std::chrono::duration(clock::now() - plan_start_time).count() >= allowed_planning_time) { res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT; }