From ce69023a650fff292aecd18e1e5bd0ab8b0ed5ba Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Sun, 24 Sep 2023 22:09:34 +0200 Subject: [PATCH] `cfun` and `planner` - human-awareness c. funs extended with the parameters for selecting computations for the whole trajectory vs only the first step [#119] --- ...fformation_space_intrusion_cost_function.h | 9 ++++++++ .../heading_disturbance_cost_function.h | 8 +++++-- .../passing_speed_cost_function.h | 9 ++++++++ .../personal_space_intrusion_cost_function.h | 9 ++++++++ ...ormation_space_intrusion_cost_function.cpp | 23 +++++++++++++++---- src/heading_disturbance_cost_function.cpp | 19 ++++++++++++--- src/humap_planner.cpp | 6 ++++- src/passing_speed_cost_function.cpp | 19 +++++++++++++-- ...personal_space_intrusion_cost_function.cpp | 19 +++++++++++++-- 9 files changed, 107 insertions(+), 14 deletions(-) diff --git a/include/humap_local_planner/fformation_space_intrusion_cost_function.h b/include/humap_local_planner/fformation_space_intrusion_cost_function.h index 3f40b460..228f98e6 100644 --- a/include/humap_local_planner/fformation_space_intrusion_cost_function.h +++ b/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& 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& groups_; + bool compute_whole_horizon_; }; } // namespace humap_local_planner diff --git a/include/humap_local_planner/heading_disturbance_cost_function.h b/include/humap_local_planner/heading_disturbance_cost_function.h index 3d95a8b9..7c87a11f 100644 --- a/include/humap_local_planner/heading_disturbance_cost_function.h +++ b/include/humap_local_planner/heading_disturbance_cost_function.h @@ -17,18 +17,21 @@ class HeadingDisturbanceCostFunction: public base_local_planner::TrajectoryCostF HeadingDisturbanceCostFunction(const std::vector& 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 diff --git a/include/humap_local_planner/passing_speed_cost_function.h b/include/humap_local_planner/passing_speed_cost_function.h index dd06ad6f..9f0b2093 100644 --- a/include/humap_local_planner/passing_speed_cost_function.h +++ b/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& 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& people_; + bool compute_whole_horizon_; }; } // namespace humap_local_planner diff --git a/include/humap_local_planner/personal_space_intrusion_cost_function.h b/include/humap_local_planner/personal_space_intrusion_cost_function.h index 41f69ee1..9141d58b 100644 --- a/include/humap_local_planner/personal_space_intrusion_cost_function.h +++ b/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& 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& people_; + bool compute_whole_horizon_; }; } // namespace humap_local_planner diff --git a/src/fformation_space_intrusion_cost_function.cpp b/src/fformation_space_intrusion_cost_function.cpp index 50d47c34..d7740393 100644 --- a/src/fformation_space_intrusion_cost_function.cpp +++ b/src/fformation_space_intrusion_cost_function.cpp @@ -6,9 +6,14 @@ namespace humap_local_planner { FformationSpaceIntrusionCostFunction::FformationSpaceIntrusionCostFunction(const std::vector& 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(robot_traj.getSteps()); + unsigned int i_end_first = std::min(i_end_whole, static_cast(1)); + for (const auto& group: groups_) { // storage for robot intrusions against groups throughout the trajectory std::vector 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); diff --git a/src/heading_disturbance_cost_function.cpp b/src/heading_disturbance_cost_function.cpp index e0cd2511..2feefa61 100644 --- a/src/heading_disturbance_cost_function.cpp +++ b/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(robot_traj.getVelocitiesNum()); + unsigned int i_end_first = std::min(i_end_whole, static_cast(1)); + for (const auto& person: people_) { // storage for disturbances against person throughout the trajectory std::vector 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); diff --git a/src/humap_planner.cpp b/src/humap_planner.cpp index be8ef0e4..bfafc54b 100644 --- a/src/humap_planner.cpp +++ b/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) { diff --git a/src/passing_speed_cost_function.cpp b/src/passing_speed_cost_function.cpp index 1815c6f9..e7d8bd97 100644 --- a/src/passing_speed_cost_function.cpp +++ b/src/passing_speed_cost_function.cpp @@ -6,9 +6,14 @@ namespace humap_local_planner { PassingSpeedCostFunction::PassingSpeedCostFunction(const std::vector& 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(robot_traj.getVelocitiesNum()); + unsigned int i_end_first = std::min(i_end_whole, static_cast(1)); + for (const auto& person: people_) { // storage for intrusions against person throughout the trajectory std::vector 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); diff --git a/src/personal_space_intrusion_cost_function.cpp b/src/personal_space_intrusion_cost_function.cpp index aa524afc..14091bf6 100644 --- a/src/personal_space_intrusion_cost_function.cpp +++ b/src/personal_space_intrusion_cost_function.cpp @@ -6,9 +6,14 @@ namespace humap_local_planner { PersonalSpaceIntrusionCostFunction::PersonalSpaceIntrusionCostFunction(const std::vector& 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(robot_traj.getVelocitiesNum()); + unsigned int i_end_first = std::min(i_end_whole, static_cast(1)); + for (const auto& person: people_) { // storage for intrusions against person throughout the trajectory std::vector 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);