in sensor_fusion_pkg/src/sensor_fusion_node.cpp [409:448]
void setLidarConfigCallback(const std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<deepracer_interfaces_pkg::srv::LidarConfigSrv::Request> req,
std::shared_ptr<deepracer_interfaces_pkg::srv::LidarConfigSrv::Response> res) {
try {
(void)request_header;
if(req->min_angle > req->max_angle ||
req->min_distance > req->max_distance ||
req->min_distance > req->clipping_distance ||
req->num_values < 1){
RCLCPP_ERROR(this->get_logger(), "Incorrect lidar configuration values.");
res->error = 1;
return;
}
// Check if the preprocessing is valid
if(req->preprocess_type < 0 || req->preprocess_type >= numLidarPreprocessingType){
RCLCPP_ERROR(this->get_logger(), "Incorrect lidar preprocess type");
res->error = 1;
return;
}
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_ANGLE_KEY] = req->min_angle;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_ANGLE_KEY] = req->max_angle;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MIN_LIDAR_DIST_KEY] = req->min_distance;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_DIST_KEY] = req->max_distance;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_NUM_LIDAR_VALUES_KEY] = req->num_values;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_LIDAR_CLIPPING_DIST_KEY] = req->clipping_distance;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_NUM_LIDAR_SECTORS_KEY] = req->num_sectors;
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_PREPROCESS_TYPE_KEY] = req->preprocess_type;
// Select the minimum of the clipping distance and max_lidar_distance as upper bound of the range
maxLiDARDist_ = std::min(sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_MAX_LIDAR_DIST_KEY],
sensorConfiguration_[LIDAR_KEY][LIDAR_CONFIG_LIDAR_CLIPPING_DIST_KEY]);
RCLCPP_INFO(this->get_logger(), "Setting the lidar configuration values for the model");
res->error = 0;
return;
}
catch (const std::exception &ex) {
RCLCPP_ERROR(this->get_logger(), "setLidarConfigCallback failed: %s", ex.what());
}
}