From e24accc68ab8281dc06db0699df334a39c000c37 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 21 Mar 2025 15:43:19 +0900 Subject: [PATCH 1/4] hot fix for vlp timestamp error --- .../velodyne_hw_interface.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 1b6276f1b..391c7193f 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -170,6 +170,14 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( target_key = "config.fov.start"; auto current_cloud_min_angle = tree.get(target_key); int setting_cloud_min_angle = sensor_configuration->cloud_min_angle; + // // FIXME + // if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { + // if (setting_cloud_min_angle < 5){ + // setting_cloud_min_angle = setting_cloud_min_angle + 360 - 5; + // }else{ + // setting_cloud_min_angle -= 5; + // } + // } // Velodyne only allows a maximum of 359 in the setting if (setting_cloud_min_angle == 360) { setting_cloud_min_angle = 359; @@ -186,6 +194,16 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( target_key = "config.fov.end"; auto current_cloud_max_angle = tree.get(target_key); int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; + // FIXME + if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { + if (setting_cloud_max_angle > 354){ + setting_cloud_max_angle = 360; + }else{ + std::cout << "setting_cloud_max_angle: " << setting_cloud_max_angle << std::endl; + setting_cloud_max_angle += 5; + std::cout << "setting_cloud_max_angle: " << setting_cloud_max_angle << std::endl; + } + } // Velodyne only allows a maximum of 359 in the setting if (setting_cloud_max_angle == 360) { setting_cloud_max_angle = 359; From a33adcd1da4489daec8b3c3ea8920810ef345e42 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 21 Mar 2025 16:55:48 +0900 Subject: [PATCH 2/4] hot fix --- .../velodyne_hw_interface.cpp | 64 +++++++++++-------- 1 file changed, 36 insertions(+), 28 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 391c7193f..ea6ace8de 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -167,21 +167,44 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( << std::endl; } - target_key = "config.fov.start"; - auto current_cloud_min_angle = tree.get(target_key); int setting_cloud_min_angle = sensor_configuration->cloud_min_angle; - // // FIXME - // if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { - // if (setting_cloud_min_angle < 5){ - // setting_cloud_min_angle = setting_cloud_min_angle + 360 - 5; - // }else{ - // setting_cloud_min_angle -= 5; - // } - // } - // Velodyne only allows a maximum of 359 in the setting - if (setting_cloud_min_angle == 360) { - setting_cloud_min_angle = 359; + int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; + + // FIXME: VLP16 has problems for timestamp. Whatch github issue # + if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { + int angle_diff = (setting_cloud_max_angle - setting_cloud_min_angle + 360) % 360; + + if (angle_diff >= 360 || angle_diff == 0) { + setting_cloud_min_angle = 0; + setting_cloud_max_angle = 359; + } else { + if (setting_cloud_min_angle < 5) { + setting_cloud_min_angle = (setting_cloud_min_angle + 355) % 360; + } else { + setting_cloud_min_angle -= 5; + } + + if (setting_cloud_max_angle > 354) { + setting_cloud_max_angle = 359; + } else { + setting_cloud_max_angle += 5; + if (setting_cloud_max_angle >= 360) { + setting_cloud_max_angle = 359; + } + } + } + + } else { + if (setting_cloud_min_angle == 360) { + setting_cloud_min_angle = 359; + } + if (setting_cloud_max_angle == 360) { + setting_cloud_max_angle = 359; + } } + + target_key = "config.fov.start"; + auto current_cloud_min_angle = tree.get(target_key); if (setting_cloud_min_angle != current_cloud_min_angle) { status = set_fov_start(setting_cloud_min_angle); if (status != ok) return status; @@ -193,21 +216,6 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( target_key = "config.fov.end"; auto current_cloud_max_angle = tree.get(target_key); - int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; - // FIXME - if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { - if (setting_cloud_max_angle > 354){ - setting_cloud_max_angle = 360; - }else{ - std::cout << "setting_cloud_max_angle: " << setting_cloud_max_angle << std::endl; - setting_cloud_max_angle += 5; - std::cout << "setting_cloud_max_angle: " << setting_cloud_max_angle << std::endl; - } - } - // Velodyne only allows a maximum of 359 in the setting - if (setting_cloud_max_angle == 360) { - setting_cloud_max_angle = 359; - } if (setting_cloud_max_angle != current_cloud_max_angle) { status = set_fov_end(setting_cloud_max_angle); if (status != ok) return status; From 4e287774cc562149da25031a34b0fa66ef155774 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Mon, 24 Mar 2025 16:15:31 +0900 Subject: [PATCH 3/4] hot fix implementation --- .../velodyne_hw_interface.cpp | 32 +++++++------------ 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index ea6ace8de..93a88558e 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -139,6 +139,7 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( { VelodyneStatus status; const auto & ok = VelodyneStatus::OK; + const int32_t fov_tolerance_deg = 5; std::string target_key = "config.returns"; auto current_return_mode_str = tree.get(target_key); @@ -170,38 +171,27 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( int setting_cloud_min_angle = sensor_configuration->cloud_min_angle; int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; - // FIXME: VLP16 has problems for timestamp. Whatch github issue # + // FIXME: VLP16 has problems for timestamp. Whatch github issue #292 + // VLP16 timestamps are weird when pointcloud are filtered by filtered by same hardware fov as software fov. To improve this, we need to set the hardware fov wider than the software fov. if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { int angle_diff = (setting_cloud_max_angle - setting_cloud_min_angle + 360) % 360; - if (angle_diff >= 360 || angle_diff == 0) { + if (angle_diff == 0 || (angle_diff >= 360 - 2 * fov_tolerance_deg && setting_cloud_min_angle < setting_cloud_max_angle)) { setting_cloud_min_angle = 0; setting_cloud_max_angle = 359; } else { - if (setting_cloud_min_angle < 5) { - setting_cloud_min_angle = (setting_cloud_min_angle + 355) % 360; - } else { - setting_cloud_min_angle -= 5; - } - - if (setting_cloud_max_angle > 354) { + setting_cloud_min_angle = (setting_cloud_min_angle - fov_tolerance_deg + 360) % 360; + setting_cloud_max_angle = (setting_cloud_max_angle + fov_tolerance_deg) % 360; + if (setting_cloud_min_angle == 0 && setting_cloud_max_angle == 0) { setting_cloud_max_angle = 359; - } else { - setting_cloud_max_angle += 5; - if (setting_cloud_max_angle >= 360) { - setting_cloud_max_angle = 359; - } } } - } else { - if (setting_cloud_min_angle == 360) { - setting_cloud_min_angle = 359; - } - if (setting_cloud_max_angle == 360) { - setting_cloud_max_angle = 359; - } + if (setting_cloud_min_angle == 360) setting_cloud_min_angle = 359; + if (setting_cloud_max_angle == 360) setting_cloud_max_angle = 359; } + std::cout << "setting_cloud_min_angle: " << setting_cloud_min_angle << std::endl; + std::cout << "setting_cloud_max_angle: " << setting_cloud_max_angle << std::endl; target_key = "config.fov.start"; auto current_cloud_min_angle = tree.get(target_key); From c6e2672ad1f625a47f9b74f70e9243596301bdf2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Mar 2025 07:23:15 +0000 Subject: [PATCH 4/4] ci(pre-commit): autofix --- .../velodyne_hw_interface.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 93a88558e..45750e73a 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -172,11 +172,14 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; // FIXME: VLP16 has problems for timestamp. Whatch github issue #292 - // VLP16 timestamps are weird when pointcloud are filtered by filtered by same hardware fov as software fov. To improve this, we need to set the hardware fov wider than the software fov. + // VLP16 timestamps are weird when pointcloud are filtered by filtered by same hardware fov as + // software fov. To improve this, we need to set the hardware fov wider than the software fov. if (sensor_configuration->sensor_model == SensorModel::VELODYNE_VLP16) { int angle_diff = (setting_cloud_max_angle - setting_cloud_min_angle + 360) % 360; - if (angle_diff == 0 || (angle_diff >= 360 - 2 * fov_tolerance_deg && setting_cloud_min_angle < setting_cloud_max_angle)) { + if ( + angle_diff == 0 || (angle_diff >= 360 - 2 * fov_tolerance_deg && + setting_cloud_min_angle < setting_cloud_max_angle)) { setting_cloud_min_angle = 0; setting_cloud_max_angle = 359; } else {