diff --git a/.github/workflows/documentation.yml b/.github/workflows/documentation.yml index d6289f835..48d8dad35 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/documentation.yml @@ -1,3 +1,4 @@ +# cspell: ignore nojekyll name: documentation on: push: diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 6dfbcf850..79c3946c7 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -17,6 +17,7 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_common/util/string_conversions.hpp" #include #include @@ -24,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -428,28 +430,34 @@ inline ReturnMode ReturnModeFromStringHesai( const std::string & return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; - if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST; + if (return_mode == "Dual" || return_mode == "LastStrongest") + return ReturnMode::DUAL_LAST_STRONGEST; if (return_mode == "First") return ReturnMode::FIRST; if (return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST; if (return_mode == "FirstStrongest") return ReturnMode::DUAL_FIRST_STRONGEST; - if (return_mode == "Dual") return ReturnMode::DUAL; break; case SensorModel::HESAI_PANDARQT64: if (return_mode == "Last") return ReturnMode::LAST; - if (return_mode == "Dual") return ReturnMode::DUAL; + if (return_mode == "Dual" || return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST; if (return_mode == "First") return ReturnMode::FIRST; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; - if (return_mode == "Dual") return ReturnMode::DUAL; + if (return_mode == "Dual" || return_mode == "LastStrongest") + return ReturnMode::DUAL_LAST_STRONGEST; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return ReturnMode::UNKNOWN; @@ -462,8 +470,9 @@ inline ReturnMode ReturnModeFromStringHesai( inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == 0) return ReturnMode::LAST; @@ -475,14 +484,18 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode break; case SensorModel::HESAI_PANDARQT64: if (return_mode == 0) return ReturnMode::LAST; - if (return_mode == 2) return ReturnMode::DUAL; + if (return_mode == 2) return ReturnMode::DUAL_LAST_FIRST; if (return_mode == 3) return ReturnMode::FIRST; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == 0) return ReturnMode::LAST; if (return_mode == 1) return ReturnMode::STRONGEST; - if (return_mode == 2) return ReturnMode::DUAL; + if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return ReturnMode::UNKNOWN; @@ -495,13 +508,14 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST) return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; @@ -509,14 +523,19 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode break; case SensorModel::HESAI_PANDARQT64: if (return_mode == ReturnMode::LAST) return 0; - if (return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_FIRST) return 2; if (return_mode == ReturnMode::FIRST) return 3; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST) + return 2; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return -1; diff --git a/nebula_common/include/nebula_common/util/string_conversions.hpp b/nebula_common/include/nebula_common/util/string_conversions.hpp new file mode 100644 index 000000000..df5cb2b48 --- /dev/null +++ b/nebula_common/include/nebula_common/util/string_conversions.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +namespace nebula::util +{ + +template +struct IsStreamable : std::false_type +{ +}; + +template +struct IsStreamable() << std::declval())> > +: std::true_type +{ +}; + +template +std::enable_if_t::value, std::string> to_string(const T & value) +{ + std::stringstream ss{}; + ss << value; + return ss.str(); +} + +} // namespace nebula::util diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index 770e06428..b05d64406 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -35,7 +35,7 @@ enum ReturnMode { DUAL_FIRST_LAST = 0x3b, DUAL_FIRST_STRONGEST = 0x3c, TRIPLE_FIRST_LAST_STRONGEST = 0x3d, - DUAL_STRONGEST_SECONDSTRONGEST = 0x3, + DUAL_STRONGEST_SECONDSTRONGEST = 0x3e, }; } // namespace return_mode diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index f550e527f..5fab49cc9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -69,6 +69,10 @@ static const float VLP16_BLOCK_DURATION = 110.592f; // [µs] static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] +/** Special Defines for VLP32 support **/ +static const float VLP32_CHANNEL_DURATION = 2.304f; // [µs] +static const float VLP32_SEQ_DURATION = 55.296f; // [µs] + /** Special Definitions for VLS128 support **/ static const float VLP128_DISTANCE_RESOLUTION = 0.004f; // [m] diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 75497ca74..d73eb29c4 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -226,10 +226,15 @@ void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_se distance > sensor_configuration_->min_range && distance < sensor_configuration_->max_range) { // Correct for the laser rotation as a function of timing during the firings. - const float azimuth_corrected_f = + float azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) / - VLP16_BLOCK_DURATION); + VLP16_BLOCK_DURATION) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { + azimuth_corrected_f += 36000.0; + } const uint16_t azimuth_corrected = (static_cast(round(azimuth_corrected_f))) % 36000; @@ -245,13 +250,8 @@ void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_se // Convert polar coordinates to Euclidean XYZ. const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_correction = corrections.cos_rot_correction; - const float sin_rot_correction = corrections.sin_rot_correction; - - const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction + - sin_rot_table_[azimuth_corrected] * sin_rot_correction; - const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction - - cos_rot_table_[azimuth_corrected] * sin_rot_correction; + const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; // Compute the distance in the xy plane (w/o accounting for rotation). const float xy_distance = distance * cos_vert_angle; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 2ba96d17f..3bf88e2a3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -138,16 +138,44 @@ void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_se checkAndHandleScanComplete(packet, packet_seconds, phase_); const raw_packet_t * raw = (const raw_packet_t *)packet.data(); + float last_azimuth_diff = 0; + uint16_t azimuth_next; uint8_t return_mode = packet[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); - for (int i = 0; i < BLOCKS_PER_PACKET; i++) { + for (uint i = 0; i < BLOCKS_PER_PACKET; i++) { int bank_origin = 0; if (raw->blocks[i].header == LOWER_BANK) { // lower bank lasers are [32..63] bank_origin = 32; } - for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { + float azimuth_diff; + uint16_t azimuth; + + // Calculate difference between current and next block's azimuth angle. + if (i == 0) { + azimuth = raw->blocks[i].rotation; + } else { + azimuth = azimuth_next; + } + if (i < static_cast(BLOCKS_PER_PACKET - (1 + dual_return))) { + // Get the next block rotation to calculate how far we rotate between blocks + azimuth_next = raw->blocks[i + (1 + dual_return)].rotation; + + // Finds the difference between two successive blocks + azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); + + // This is used when the last block is next to predict rotation amount + last_azimuth_diff = azimuth_diff; + } else { + // This makes the assumption the difference between the last block and the next packet is the + // same as the last to the second to last. + // Assumes RPM doesn't change much between blocks. + azimuth_diff = + (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; + } + + for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { float x, y, z; uint8_t intensity; const uint8_t laser_number = j + bank_origin; @@ -201,13 +229,17 @@ void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_se raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) { const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_correction = corrections.cos_rot_correction; - const float sin_rot_correction = corrections.sin_rot_correction; + float azimuth_corrected_f = + azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - + corrections.rot_correction * 180.0 / M_PI * 100; + if (azimuth_corrected_f < 0) { + azimuth_corrected_f += 36000; + } + const uint16_t azimuth_corrected = + (static_cast(std::round(azimuth_corrected_f))) % 36000; - const float cos_rot_angle = cos_rot_table_[block.rotation] * cos_rot_correction + - sin_rot_table_[block.rotation] * sin_rot_correction; - const float sin_rot_angle = sin_rot_table_[block.rotation] * cos_rot_correction - - cos_rot_table_[block.rotation] * sin_rot_correction; + const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; const float horiz_offset = corrections.horiz_offset_correction; const float vert_offset = corrections.vert_offset_correction; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index f9f2f2d7e..9c683d48d 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -247,9 +247,14 @@ void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_s } // Correct for the laser rotation as a function of timing during the firings. - const float azimuth_corrected_f = - azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]); - const uint16_t azimuth_corrected = ((uint16_t)round(azimuth_corrected_f)) % 36000; + float azimuth_corrected_f = azimuth + + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { + azimuth_corrected_f += 36000.0; + } + const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000; if ( distance > sensor_configuration_->min_range && @@ -266,13 +271,8 @@ void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_s // convert polar coordinates to Euclidean XYZ. const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_correction = corrections.cos_rot_correction; - const float sin_rot_correction = corrections.sin_rot_correction; - - const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction + - sin_rot_table_[azimuth_corrected] * sin_rot_correction; - const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction - - cos_rot_table_[azimuth_corrected] * sin_rot_correction; + const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; // Compute the distance in the xy plane (w/o accounting for rotation). const float xy_distance = distance * cos_vert_angle; diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index ccbf10ca0..6cb486ae9 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -2,6 +2,7 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include #include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -246,7 +247,9 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) { boost::property_tree::ptree tree; try { - boost::property_tree::read_json(str, tree); + std::stringstream ss; + ss << str; + boost::property_tree::read_json(ss, tree); } catch (boost::property_tree::json_parser_error & e) { std::cerr << e.what() << std::endl; } @@ -496,7 +499,7 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() throw std::runtime_error("Response payload too short"); } - HesaiLidarRangeAll hesai_range_all; + HesaiLidarRangeAll hesai_range_all{}; hesai_range_all.method = response[0]; switch (hesai_range_all.method) { case 0: // for all channels @@ -640,6 +643,12 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() } auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); + + // FIXME(mojomex): this is a hotfix for sensors that do not support this command + if (!response_or_err.has_value()) { + return HesaiLidarMonitor{}; + } + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return CheckSizeAndParse(response); } diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index 33197ed52..737d7c1b1 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -65,6 +65,7 @@ "enum": [ "Last", "Strongest", + "LastStrongest", "Dual" ] }, diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 7991be037..0c3f8323f 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -62,6 +62,7 @@ "enum": [ "Last", "Strongest", + "LastStrongest", "Dual" ] }, diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 150ef0cad..dd9fa190f 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -86,9 +86,6 @@ "Last", "Strongest", "LastStrongest", - "First", - "LastFirst", - "FirstStrongest", "Dual" ] }, diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index f5d37a293..e8272336c 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -65,6 +65,7 @@ "enum": [ "Last", "First", + "LastFirst", "Dual" ] }, diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 0d2d9bd10..6847722f9 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -324,7 +324,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( } if (_return_mode.length() > 0) - new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + new_cfg.return_mode = + nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model); // //////////////////////////////////////// // Get and validate new calibration, if any diff --git a/nebula_tests/data/velodyne/vlp32/1713492677464078412.pcd b/nebula_tests/data/velodyne/vlp32/1713492677464078412.pcd index 6ae2e557f..0e8f9965b 100644 Binary files a/nebula_tests/data/velodyne/vlp32/1713492677464078412.pcd and b/nebula_tests/data/velodyne/vlp32/1713492677464078412.pcd differ diff --git a/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd b/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd index 5da07e063..f3032247e 100644 Binary files a/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd and b/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd differ