mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
JointInterpolation: fix timeout handling
The timeout parameter was essentially ignored and the check was always true.
This commit is contained in:
parent
eae0bdc27f
commit
3d3236575d
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user