Function find_disparity_steering_angle

Function Documentation

float find_disparity_steering_angle(std::vector<float> processed_ranges, float angle_increment, float lidar_fov, float desired_fov, float angle_eq_thr, float ray_eq_thr, bool ignore_angle_eq_thr, bool ignore_ray_eq_thr, std::string angle_priority)

Find the steering angle that corresponds to the maximum distance in the processed ranges vector.

This function identifies the best steering angle by analyzing the processed ranges from a LIDAR sensor. It considers multiple criteria such as angle priority, ray equality thresholds, and angle equality thresholds. The function can ignore certain thresholds based on the provided parameters.

This function keeps track of the best rays by storing their indexes in the vector best_indexes. During the search, if a new maximum distance is found, the vector is updated and reordered. In addition, the rays that have distance_from_max_ray greater than ray_eq_thr are removed from the vector.

Parameters:

  • processed_ranges – The processed ranges vector from the LIDAR sensor.

  • angle_increment – The angle increment of the LIDAR in radians.

  • lidar_fov – The field of view of the LIDAR in radians.

  • desired_fov – The desired field of view to consider in radians.

  • disparity_thrashold – The threshold for disparity (not used in the current implementation).

  • angle_eq_thr – The threshold for considering two angles as equal.

  • ray_eq_thr – The threshold for considering two rays as equal.

  • ignore_angle_eq_thr – Flag to ignore the angle equality threshold.

  • ignore_ray_eq_thr – Flag to ignore the ray equality threshold.

  • angle_priority – The priority direction (“left” or “right”) when choosing between equal distances.

Returns:

The steering angle that corresponds to the maximum distance in the processed ranges vector in radians.