search

rayvburn / humap_local_planner

Human-aware robot trajectory planner using a hybrid trajectory candidates generation and spatiotemporal cost functions
BSD 3-Clause "New" or "Revised" License
0 stars 1 forks source link

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

Open rayvburn opened 1 year ago

rayvburn commented 1 year ago

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);
}