cfun and planner - human-awareness c. funs extended with the para…

…meters for selecting computations for the whole trajectory vs only the first step [#119]

include/humap_local_planner/fformation_space_intrusion_cost_function.h

@@ -14,6 +14,14 @@ class FformationSpaceIntrusionCostFunction: public base_local_planner::Trajector
 public:
 	FformationSpaceIntrusionCostFunction(const std::vector<Group>& groups);
 
+	/**
+	 * @brief Updates the internal parameters
+	 *
+	 * @param compute_whole_horizon set to true if the cost function should be computed for each entry
+	 * of the trajectory
+	 */
+	void setParameters(bool compute_whole_horizon);
+
 	/**
 	 * @brief General updating of context values if required.
 	 * Subclasses may overwrite. Return false in case there is any error.
@@ -28,6 +36,7 @@ class FformationSpaceIntrusionCostFunction: public base_local_planner::Trajector
 protected:
 	/// A reference to a dataset containing F-formations (groups) detections
 	const std::vector<Group>& groups_;
+	bool compute_whole_horizon_;
 };
 
 } // namespace humap_local_planner

include/humap_local_planner/heading_disturbance_cost_function.h

@@ -17,18 +17,21 @@ class HeadingDisturbanceCostFunction: public base_local_planner::TrajectoryCostF
 	HeadingDisturbanceCostFunction(const std::vector<Person>& people);
 
 	/**
-	 * @brief Set the Parameters object
+	 * @brief Updates the internal parameters
 	 *
 	 * @param fov_person person's field of view (full) that is used in disturbance calculations
 	 * @param person_model_radius radius that defines circle that models physical area occupied by a person
 	 * @param robot_circumradius defines dimensions of the robot, required for cost normalization
 	 * @param max_speed max speed of the robot, required for cost normalization
+	 * @param compute_whole_horizon set to true if the cost function should be computed for each entry
+	 * of the trajectory
 	 */
 	void setParameters(
 		double fov_person,
 		double person_model_radius,
 		double robot_circumradius,
-		double max_speed
+		double max_speed,
+		bool compute_whole_horizon = true
 	);
 
 	/**
@@ -49,6 +52,7 @@ class HeadingDisturbanceCostFunction: public base_local_planner::TrajectoryCostF
 	double person_model_radius_;
 	double robot_circumradius_;
 	double max_speed_;
+	bool compute_whole_horizon_;
 };
 
 } // namespace humap_local_planner

include/humap_local_planner/passing_speed_cost_function.h

@@ -14,6 +14,14 @@ class PassingSpeedCostFunction: public base_local_planner::TrajectoryCostFunctio
 public:
 	PassingSpeedCostFunction(const std::vector<Person>& people);
 
+	/**
+	 * @brief Updates the internal parameters
+	 *
+	 * @param compute_whole_horizon set to true if the cost function should be computed for each entry
+	 * of the trajectory
+	 */
+	void setParameters(bool compute_whole_horizon);
+
 	/**
 	 * @brief General updating of context values if required.
 	 * Subclasses may overwrite. Return false in case there is any error.
@@ -28,6 +36,7 @@ class PassingSpeedCostFunction: public base_local_planner::TrajectoryCostFunctio
 protected:
 	/// A reference to a dataset containing people detections
 	const std::vector<Person>& people_;
+	bool compute_whole_horizon_;
 };
 
 } // namespace humap_local_planner

include/humap_local_planner/personal_space_intrusion_cost_function.h

@@ -14,6 +14,14 @@ class PersonalSpaceIntrusionCostFunction: public base_local_planner::TrajectoryC
 public:
 	PersonalSpaceIntrusionCostFunction(const std::vector<Person>& people);
 
+	/**
+	 * @brief Updates the internal parameters
+	 *
+	 * @param compute_whole_horizon set to true if the cost function should be computed for each entry
+	 * of the trajectory
+	 */
+	void setParameters(bool compute_whole_horizon);
+
 	/**
 	 * @brief General updating of context values if required.
 	 * Subclasses may overwrite. Return false in case there is any error.
@@ -28,6 +36,7 @@ class PersonalSpaceIntrusionCostFunction: public base_local_planner::TrajectoryC
 protected:
 	/// A reference to a dataset containing people detections
 	const std::vector<Person>& people_;
+	bool compute_whole_horizon_;
 };
 
 } // namespace humap_local_planner

src/fformation_space_intrusion_cost_function.cpp

@@ -6,9 +6,14 @@
 namespace humap_local_planner {
 
 FformationSpaceIntrusionCostFunction::FformationSpaceIntrusionCostFunction(const std::vector<Group>& groups):
-	groups_(groups)
+	groups_(groups),
+	compute_whole_horizon_(true)
 {}
 
+void FformationSpaceIntrusionCostFunction::setParameters(bool compute_whole_horizon) {
+	compute_whole_horizon_ = compute_whole_horizon;
+}
+
 bool FformationSpaceIntrusionCostFunction::prepare() {
 	return true;
 }
@@ -26,14 +31,24 @@ double FformationSpaceIntrusionCostFunction::scoreTrajectory(base_local_planner:
 
 	// compare group poses against each pose of robot trajectory
 	Trajectory robot_traj(traj);
+
+	// select the ending iteration number when not the whole horizon is meant to be evaluated;
+	// prepare for the case when `robot_traj.getSteps()` is 0
+	// NOTE: velocities are not considered below, therefore getSteps() can be considered instead
+	// of getVelocitiesNum()
+	unsigned int i_end_whole = static_cast<unsigned int>(robot_traj.getSteps());
+	unsigned int i_end_first = std::min(i_end_whole, static_cast<unsigned int>(1));
+
 	for (const auto& group: groups_) {
 		// storage for robot intrusions against groups throughout the trajectory
 		std::vector<double> group_intrusions;
 
 		// check all robot trajectory points ...
-		// NOTE: velocities are not considered below, therefore getSteps() can be considered instead
-		// of getVelocitiesNum()
-		for (unsigned int i = 0; i < robot_traj.getSteps(); i++) {
+		for (
+			unsigned int i = 0;
+			i < (compute_whole_horizon_ ? i_end_whole : i_end_first);
+			i++
+		) {
 			// retrieve poses
 			auto p_robot = robot_traj.getPose(i);
 			auto p_group = group.getTrajectoryPrediction().getPose(i);

src/heading_disturbance_cost_function.cpp

@@ -10,19 +10,22 @@ HeadingDisturbanceCostFunction::HeadingDisturbanceCostFunction(const std::vector
 	fov_person_(3.31613),
 	person_model_radius_(0.28),
 	robot_circumradius_(0.275),
-	max_speed_(0.55)
+	max_speed_(0.55),
+	compute_whole_horizon_(true)
 {}
 
 void HeadingDisturbanceCostFunction::setParameters(
 	double fov_person,
 	double person_model_radius,
 	double robot_circumradius,
-	double max_speed
+	double max_speed,
+	bool compute_whole_horizon
 ) {
 	fov_person_ = fov_person;
 	person_model_radius_ = person_model_radius;
 	robot_circumradius_ = robot_circumradius;
 	max_speed_ = max_speed;
+	compute_whole_horizon_ = compute_whole_horizon;
 }
 
 bool HeadingDisturbanceCostFunction::prepare() {
@@ -40,12 +43,22 @@ double HeadingDisturbanceCostFunction::scoreTrajectory(base_local_planner::Traje
 	// iterate over predicted poses of a person, compare against each pose of robot trajectory
 	// against all people pose predictions ...
 	Trajectory robot_traj(traj);
+
+	// select the ending iteration number when not the whole horizon is meant to be evaluated;
+	// prepare for the case when `robot_traj.getVelocitiesNum()` is 0
+	unsigned int i_end_whole = static_cast<unsigned int>(robot_traj.getVelocitiesNum());
+	unsigned int i_end_first = std::min(i_end_whole, static_cast<unsigned int>(1));
+
 	for (const auto& person: people_) {
 		// storage for disturbances against person throughout the trajectory
 		std::vector<double> person_disturbances;
 
 		// check all robot trajectory poses that have matching velocities
-		for (unsigned int i = 0; i < robot_traj.getVelocitiesNum(); i++) {
+		for (
+			unsigned int i = 0;
+			i < (compute_whole_horizon_ ? i_end_whole : i_end_first);
+			i++
+		) {
 			// retrieve poses and velocities
 			auto p_robot = robot_traj.getPose(i);
 			auto v_robot = robot_traj.getVelocity(i);

src/humap_planner.cpp

@@ -760,8 +760,12 @@ void HumapPlanner::updateCostParameters() {
 		cfg_->getGeneral()->person_model_radius,
 		// TODO: for non-circular robots this won't be a valid circumradius
 		robot_model_->getInscribedRadius(),
-		cfg_->getLimits()->max_vel_trans
+		cfg_->getLimits()->max_vel_trans,
+		cfg_->getCost()->heading_dir_compute_whole_horizon
 	);
+	personal_space_costs_.setParameters(cfg_->getCost()->personal_space_compute_whole_horizon);
+	fformation_space_costs_.setParameters(cfg_->getCost()->fformation_space_compute_whole_horizon);
+	passing_speed_costs_.setParameters(cfg_->getCost()->passing_speed_compute_whole_horizon);
 }
 
 void HumapPlanner::createEnvironmentModel(const Pose& pose_ref, World& world_model) {

src/passing_speed_cost_function.cpp

@@ -6,9 +6,14 @@
 namespace humap_local_planner {
 
 PassingSpeedCostFunction::PassingSpeedCostFunction(const std::vector<Person>& people):
-	people_(people)
+	people_(people),
+	compute_whole_horizon_(true)
 {}
 
+void PassingSpeedCostFunction::setParameters(bool compute_whole_horizon) {
+	compute_whole_horizon_ = compute_whole_horizon;
+}
+
 bool PassingSpeedCostFunction::prepare() {
 	return true;
 }
@@ -24,12 +29,22 @@ double PassingSpeedCostFunction::scoreTrajectory(base_local_planner::Trajectory&
 	// iterate over predicted poses of a person, compare against each pose of robot trajectory
 	// against all people pose predictions ...
 	Trajectory robot_traj(traj);
+
+	// select the ending iteration number when not the whole horizon is meant to be evaluated;
+	// prepare for the case when `robot_traj.getVelocitiesNum()` is 0
+	unsigned int i_end_whole = static_cast<unsigned int>(robot_traj.getVelocitiesNum());
+	unsigned int i_end_first = std::min(i_end_whole, static_cast<unsigned int>(1));
+
 	for (const auto& person: people_) {
 		// storage for intrusions against person throughout the trajectory
 		std::vector<double> person_comforts;
 
 		// check all robot trajectory points ...
-		for (unsigned int i = 0; i < robot_traj.getVelocitiesNum(); i++) {
+		for (
+			unsigned int i = 0;
+			i < (compute_whole_horizon_ ? i_end_whole : i_end_first);
+			i++
+		) {
 			// retrieve poses
 			auto p_robot = robot_traj.getPose(i);
 			auto v_robot = robot_traj.getVelocity(i);

src/personal_space_intrusion_cost_function.cpp

@@ -6,9 +6,14 @@
 namespace humap_local_planner {
 
 PersonalSpaceIntrusionCostFunction::PersonalSpaceIntrusionCostFunction(const std::vector<Person>& people):
-	people_(people)
+	people_(people),
+	compute_whole_horizon_(true)
 {}
 
+void PersonalSpaceIntrusionCostFunction::setParameters(bool compute_whole_horizon) {
+	compute_whole_horizon_ = compute_whole_horizon;
+}
+
 bool PersonalSpaceIntrusionCostFunction::prepare() {
 	return true;
 }
@@ -24,12 +29,22 @@ double PersonalSpaceIntrusionCostFunction::scoreTrajectory(base_local_planner::T
 	// iterate over predicted poses of a person, compare against each pose of robot trajectory
 	// against all people pose predictions ...
 	Trajectory robot_traj(traj);
+
+	// select the ending iteration number when not the whole horizon is meant to be evaluated;
+	// prepare for the case when `robot_traj.getVelocitiesNum()` is 0
+	unsigned int i_end_whole = static_cast<unsigned int>(robot_traj.getVelocitiesNum());
+	unsigned int i_end_first = std::min(i_end_whole, static_cast<unsigned int>(1));
+
 	for (const auto& person: people_) {
 		// storage for intrusions against person throughout the trajectory
 		std::vector<double> person_intrusions;
 
 		// check all robot trajectory poses that have matching velocities
-		for (unsigned int i = 0; i < robot_traj.getVelocitiesNum(); i++) {
+		for (
+			unsigned int i = 0;
+			i < (compute_whole_horizon_ ? i_end_whole : i_end_first);
+			i++
+		) {
 			// retrieve poses and velocities
 			auto p_robot = robot_traj.getPose(i);
 			auto v_robot = robot_traj.getVelocity(i);