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

Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED #2455

Merged
merged 3 commits into from
Oct 17, 2023
Merged

Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED #2455

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1c1c21f
Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED
rhaschke Oct 16, 2023
be55ed9
Add planning time + timeout to warning
rhaschke Oct 17, 2023
05803f8
Remove wrong error code assignment
rhaschke Oct 17, 2023
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
21 changes: 16 additions & 5 deletions moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -860,7 +860,7 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte
RCLCPP_DEBUG(LOGGER, "%s: Solving the planning problem once...", name_.c_str());
ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start);
registerTerminationCondition(ptc);
result.val = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
std::ignore = ompl_simple_setup_->solve(ptc);
last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
unregisterTerminationCondition();
// fill the result status code
Expand Down Expand Up @@ -976,7 +976,7 @@ void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition()
int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup)
{
auto result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus();
const ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus();
switch (ompl::base::PlannerStatus::StatusType(ompl_status))
{
case ompl::base::PlannerStatus::UNKNOWN:
Expand All @@ -996,12 +996,23 @@ int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::Si
result = moveit_msgs::msg::MoveItErrorCodes::UNRECOGNIZED_GOAL_TYPE;
break;
case ompl::base::PlannerStatus::TIMEOUT:
RCLCPP_WARN(LOGGER, "Timed out");
RCLCPP_WARN(LOGGER, "Timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(),
request_.allowed_planning_time);
result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT;
break;
case ompl::base::PlannerStatus::APPROXIMATE_SOLUTION:
RCLCPP_WARN(LOGGER, "Solution is approximate");
result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
// timeout is a common reason for APPROXIMATE_SOLUTION
if (ompl_simple_setup->getLastPlanComputationTime() > request_.allowed_planning_time)
{
RCLCPP_WARN(LOGGER, "Planning timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(),
request_.allowed_planning_time);
result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT;
}
else
{
RCLCPP_WARN(LOGGER, "Solution is approximate");
result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
}
break;
case ompl::base::PlannerStatus::EXACT_SOLUTION:
result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
Expand Down