-
Notifications
You must be signed in to change notification settings - Fork 512
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.
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
Make planning pipeline respect the allowed_planning_time #2692
base: main
Are you sure you want to change the base?
Changes from 4 commits
3930be6
e37e314
69b6de8
06377f7
167fb5e
99bf09c
cffe188
af3e30e
f903bd0
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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 | ||
{ | ||
|
@@ -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); | ||
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"; | ||
TomCC7 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
res.error_code = moveit::core::MoveItErrorCode::TIMED_OUT; | ||
} | ||
|
||
// If adapter does not succeed, break chain and return false | ||
if (!res.error_code) | ||
{ | ||
|
@@ -291,9 +307,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 | ||
double max_single_planner_time = std::chrono::duration<double>(clock::now() - plan_start_time).count() / | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I don't understand this time calculation. Can you explain it? Why are we giving the planner the time since the planning started as time budget? Maybe I am missing something. Also can you make this const? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. sorry made a mistake, I changed it to
|
||
(pipeline_parameters_.planning_plugins.size() - i); | ||
mutable_request.allowed_planning_time = max_single_planner_time; | ||
TomCC7 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
||
const auto& planner = planner_map_.at(pipeline_parameters_.planning_plugins[i]); | ||
TomCC7 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
// Update reference trajectory with latest solution (if available) | ||
if (res.trajectory) | ||
{ | ||
|
@@ -317,6 +339,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) | ||
{ | ||
|
@@ -335,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 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 There was a problem hiding this comment. Choose a reason for hiding this commentThe 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? There was a problem hiding this comment. Choose a reason for hiding this commentThe 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? There was a problem hiding this comment. Choose a reason for hiding this commentThe 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. There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 |
||
if (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) | ||
{ | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.