Added cost calculation in move_relative (#108)

This commit is contained in:
Aris Synodinos 2019-11-05 17:38:25 +01:00 committed by Henning Kayser
parent ff297d2929
commit 145bec1ed3

View File

@ -288,6 +288,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
if (dir == BACKWARD)
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
// set cost
double cost = 0;
for (const double& distance : robot_trajectory->getWayPointDurations()) {
cost += distance;
}
solution.setCost(cost);
if (!success)
solution.markAsFailure();
return true;