void lidarCB()

in sensor_fusion_pkg/src/sensor_fusion_node.cpp [310:358]
45 lines of code
7 McCabe index (conditional complexity)

        void lidarCB(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
            try {
                lastLidarMsgRecievedTime = std::chrono::steady_clock::now();
                std::vector<float> overlayRanges, overlayDegrees, lidarRanges, lidarDegrees;
                for (size_t i = 0; i < msg->ranges.size(); i++) {
                    float degree = RAD2DEG(msg->angle_min + msg->angle_increment * i);
                    if(degree >= sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_ANGLE_KEY] &&
                       degree <= sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_ANGLE_KEY]){
                        if(std::isinf(msg->ranges[i])) {
                            lidarRanges.push_back(maxLiDARDist_);
                        }
                        else {
                            lidarRanges.push_back(std::min(std::max(static_cast<float>(msg->ranges[i]),
                            sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_DIST_KEY]),
                            maxLiDARDist_));
                        }
                        lidarDegrees.push_back(degree);
                    }
                    overlayRanges.push_back(std::min(std::max(static_cast<float>(msg->ranges[i]),
                                            sensorConfiguration_[LIDAR_OVERLAY_KEY][
                                             LIDAR_OVERLAY_CONFIG_MIN_LIDAR_DIST_KEY]),
                                            sensorConfiguration_[LIDAR_OVERLAY_KEY][
                                             LIDAR_OVERLAY_CONFIG_MAX_LIDAR_DIST_KEY]));
                    overlayDegrees.push_back(degree);
                }

                auto desiredLidarDegrees = linspace(sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_ANGLE_KEY],
                                                    sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_ANGLE_KEY],
                                                    sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_NUM_LIDAR_VALUES_KEY]);

                auto desiredOverlayLidarDegrees = linspace(sensorConfiguration_[LIDAR_OVERLAY_KEY][LIDAR_OVERLAY_CONFIG_MIN_LIDAR_ANGLE_KEY],
                                                           sensorConfiguration_[LIDAR_OVERLAY_KEY][LIDAR_OVERLAY_CONFIG_MAX_LIDAR_ANGLE_KEY],
                                                           sensorConfiguration_[LIDAR_OVERLAY_KEY][LIDAR_OVERLAY_CONFIG_NUM_LIDAR_VALUES_KEY]);

                // Add the min/max lidarDegrees and ranges for interpolation in case not present.
                if(!std::count(lidarDegrees.begin(), lidarDegrees.end(), sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_ANGLE_KEY])) {
                    lidarDegrees.insert(lidarDegrees.begin(), sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_ANGLE_KEY]);
                    lidarDegrees.push_back(sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_ANGLE_KEY]);
                    lidarRanges.insert(lidarRanges.begin(), maxLiDARDist_);
                    lidarRanges.push_back(maxLiDARDist_);
                }
                std::lock_guard<std::mutex> guard(lidarMutex_);
                lidarData_ = interp(desiredLidarDegrees, lidarDegrees, lidarRanges);
                overlayLidarData_ = interp(desiredOverlayLidarDegrees, overlayDegrees, overlayRanges);
            }
            catch (const std::exception &ex) {
                RCLCPP_ERROR(this->get_logger(), "LiDAR callback failed: %s", ex.what());
            }
        }