From 145bec1ed3b6639d0b937b155e362e063860f4b3 Mon Sep 17 00:00:00 2001 From: Aris Synodinos Date: Tue, 5 Nov 2019 17:38:25 +0100 Subject: [PATCH] Added cost calculation in move_relative (#108) --- core/src/stages/move_relative.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f09d0053..5d77a6fb 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -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;