From 456cb1eb247990ef6ce70e538982b086f27dcc60 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Tue, 3 Sep 2024 19:02:44 +0900 Subject: [PATCH] fix(hesai): fix supported return modes for Hesai sensors (#188) * chore(nebula_common): add utility to get std::string from < #include #include #include #include +#include #include #include namespace nebula @@ -402,28 +404,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; @@ -436,8 +444,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; @@ -449,14 +458,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; @@ -469,13 +482,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; @@ -483,14 +497,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_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index 0ec03169e..5f822f4fc 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -62,6 +62,7 @@ "enum": [ "Last", "Strongest", + "LastStrongest", "Dual" ] }, diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index cf95db7d5..680ffc501 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -59,6 +59,7 @@ "enum": [ "Last", "Strongest", + "LastStrongest", "Dual" ] }, diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index d3aea4059..5b4c07f13 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -80,9 +80,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 81ee37460..81f30a875 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -62,6 +62,7 @@ "enum": [ "Last", "First", + "LastFirst", "Dual" ] },