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 9 commits into
base: main
Choose a base branch
from
+70 −4
Open

Make planning pipeline respect the allowed_planning_time #2692

Changes from 1 commit
Commits
Show all changes
9 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
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
35 changes: 34 additions & 1 deletion 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,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_)
{
Expand All @@ -288,11 +295,24 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
req_adapter->getDescription().c_str(), status.message.c_str());
break;
}

// check for timeout
Copy link
Contributor

Choose a reason for hiding this comment

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

Add a note here that it is assumed that the request adapters only consume a very small fraction of the allowed planning time and thus they don't get a budget assigned. The timeout check is mainly a sanity check

if (std::chrono::duration<double>(clock::now() - plan_start_time).count() >= allowed_planning_time)
{
throw timeout_error;
sjahr marked this conversation as resolved.
Show resolved Hide resolved
}
}

// 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<double>(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)
Expand Down Expand Up @@ -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<double>(clock::now() - planner_start_time).count() >= max_single_planner_time)
Copy link
Contributor

Choose a reason for hiding this comment

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

Maybe I would even make sense to use the same check here as for the adapters

if (std::chrono::duration<double>(clock::now() - plan_start_time).count() >= allowed_planning_time)

I think we might not care too much about the individual elements exceeding their allocated individual budget (for now) but for the overall planning process to not exceed the allowed_planning_time 🤔

Copy link
Contributor

Choose a reason for hiding this comment

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

Agreed. Especially because some planners will take longer, so splitting the timeout evenly seems risky.

{
throw timeout_error;
}
}

// Call plan response adapter chain
Expand All @@ -342,6 +369,12 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
res_adapter->getDescription().c_str());
break;
}

// check for timeout
if (std::chrono::duration<double>(clock::now() - plan_start_time).count() >= allowed_planning_time)
{
throw timeout_error;
}
}
}
}
Expand Down
Loading