JointInterpolation: fix timeout handling

The timeout parameter was essentially ignored and the check was always true.
This commit is contained in:
Robert Haschke 2023-02-17 11:41:32 +01:00
parent eae0bdc27f
commit 3d3236575d

View File

@ -103,7 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
auto to{ from->diff() };
@ -125,8 +125,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
to->getCurrentStateNonConst().update();
timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
if (timeout <= 0.0)
if (std::chrono::steady_clock::now() >= deadline)
return false;
return plan(from, to, jmg, timeout, result, path_constraints);