add getters for obtaining values of cost functions for the best trajectory

[rayvburn]

Consider adding these and publish to ROS topic:

Header:

	/**
	 * @addtogroup costgetters Getters for specific components of the cost of the recently planned trajectory
	 * @{
	 */
	double getCostObstacle() const;
	double getCostOscillation() const;
	double getCostPath() const;
	double getCostGoal() const;
	double getCostGoalFront() const;
	double getCostAlignment() const;
	double getCostBackward() const;
	double getCostTtc() const;
	double getCostChc() const;
	double getCostSpeedyGoal() const;
	double getCostVelocitySmoothness() const;
	double getCostContextualized() const;
	double getCostPersonDisturbance() const;
	/// @}

Source:

double HuberoPlanner::getCostObstacle() const {
	auto traj_copy = result_traj_;
	auto costs_copy = obstacle_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostOscillation() const {
	auto traj_copy = result_traj_;
	auto costs_copy = oscillation_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostPath() const {
	auto traj_copy = result_traj_;
	auto costs_copy = path_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostGoal() const {
	auto traj_copy = result_traj_;
	auto costs_copy = goal_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostGoalFront() const {
	auto traj_copy = result_traj_;
	auto costs_copy = goal_front_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostAlignment() const {
	auto traj_copy = result_traj_;
	auto costs_copy = alignment_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostBackward() const {
	auto traj_copy = result_traj_;
	auto costs_copy = backward_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostTtc() const {
	auto traj_copy = result_traj_;
	auto costs_copy = ttc_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostChc() const {
	auto traj_copy = result_traj_;
	auto costs_copy = chc_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostSpeedyGoal() const {
	auto traj_copy = result_traj_;
	auto costs_copy = speedy_goal_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostVelocitySmoothness() const {
	auto traj_copy = result_traj_;
	auto costs_copy = velocity_smoothness_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostContextualized() const {
	auto traj_copy = result_traj_;
	auto costs_copy = contextualized_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}

double HuberoPlanner::getCostPersonDisturbance() const {
	auto traj_copy = result_traj_;
	auto costs_copy = disturbance_costs_;
	return costs_copy.getScale() * costs_copy.scoreTrajectory(traj_copy);
}