Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Make planning pipeline respect the allowed_planning_time #2692

Open
wants to merge 10 commits into
base: main
Choose a base branch
from
Open

Make planning pipeline respect the allowed_planning_time #2692

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3930be6
Make planning pipeline respect the allowed_planning_time
TomCC7 Feb 18, 2024
e37e314
feat: timeout check set error_code instead of throw
TomCC7 Feb 28, 2024
69b6de8
feat: evenly distribute the planning time among remaining planners
TomCC7 Feb 28, 2024
06377f7
test: add timeout handling test for planningpipeline
TomCC7 Feb 28, 2024
167fb5e
chore, fix: formatting and minor fixes for planning pipeline timeout
TomCC7 Mar 6, 2024
99bf09c
fix spelling
TomCC7 Mar 6, 2024
cffe188
feat: allowing extra 10ms for response adapter
TomCC7 Mar 28, 2024
af3e30e
Merge branch 'main' into main
sjahr Apr 24, 2024
f903bd0
Merge branch 'main' into main
sjahr Jun 28, 2024
c2a5184
Merge branch 'main' into main
sjahr Oct 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 42 additions & 4 deletions moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <fmt/format.h>
#include <moveit/utils/logger.hpp>
#include <chrono>
#include <ratio>
#include <stdexcept>

namespace
{
Expand Down Expand Up @@ -267,19 +270,32 @@ 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 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_)
{
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;

// 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)
{
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;
}

// If adapter does not succeed, break chain and return false
if (!res.error_code)
{
Expand All @@ -292,9 +308,15 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
}

// 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
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.at(i));
// Update reference trajectory with latest solution (if available)
if (res.trajectory)
{
Expand All @@ -319,6 +341,12 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
context->solve(res);
publishPipelineState(mutable_request, res, planner->getDescription());

// check for overall timeout
if (std::chrono::duration<double>(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)
{
Expand All @@ -332,13 +360,23 @@ 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_)
{
assert(res_adapter);
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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe I'm missing it, but it looks like we will hit a timeout here if the planners use up the allowed planning time. Optimizing planners usually do that, so I think we should either ignore response adapters or use a separate budget

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I think the ultimate solution to this is to give user more control over the timeout. Maybe let them decide timeout of each section (and each planner) separately? But that would require changing the MotionPlanRequest message so I'm not sure. What's your suggestion on this? Should we make the PR more complex or just ignore the response adapters for now?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we ensure for now that a fraction of the time will be reserved for the planners? Something like 100ms in any case?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But in that case the planner might still use up all the reserved time. Or are you saying that giving the request adapter at least 100ms?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the response adapter should have a reserved time budget which is deducted from the planner's allowed planning time.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hi, I add an extra check to ensure that the response adapter has 10ms left in the latest commit

// NOTE: reserving 10ms here
if (std::chrono::duration<double>(clock::now() - start_time).count() >= 10.0 * 1e-3 &&
std::chrono::duration<double>(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)
{
Expand Down
28 changes: 28 additions & 0 deletions moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <gtest/gtest.h>

#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit/utils/moveit_error_code.h>
#include <moveit/utils/robot_model_test_utils.h>

namespace
Expand Down Expand Up @@ -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<planning_scene::PlanningScene>(robot_model_);
EXPECT_TRUE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response));
EXPECT_TRUE(motion_plan_response.error_code);
Expand All @@ -115,6 +117,32 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured)
std::runtime_error);
}

TEST_F(TestPlanningPipeline, TestTimeout)
TomCC7 marked this conversation as resolved.
Show resolved Hide resolved
{
// construct pipeline
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

didn't know that checker also checks for spelling lol

EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared<planning_pipeline::PlanningPipeline>(
robot_model_, node_, "", PLANNER_PLUGINS, REQUEST_ADAPTERS, RESPONSE_ADAPTERS));

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_);
TomCC7 marked this conversation as resolved.
Show resolved Hide resolved

// timeout by request adapter
TomCC7 marked this conversation as resolved.
Show resolved Hide resolved
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.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);

// 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)
{
rclcpp::init(argc, argv);
Expand Down
Loading