PipelinePlanner: always forward trajectory

(also in case of failure)
This commit is contained in:
Robert Haschke 2019-07-19 13:42:49 +02:00
parent 6ff1a5162f
commit 4225671ae4

View File

@ -101,11 +101,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
if (!planner_->generatePlan(from, req, res))
return false;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return true;
return success;
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
@ -127,11 +125,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
if (!planner_->generatePlan(from, req, res))
return false;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return true;
return success;
}
}
}