fix(hesai): fix supported return modes for Hesai sensors (#188)

* chore(nebula_common): add utility to get std::string from <<streamable types

* fix(hesai_common): support correct return types for all sensors

* chore(nebula_ros): update supported return modes in parameter schemas for hesai sensors

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -17,12 +17,14 @@
 
 #include "nebula_common/nebula_common.hpp"
 #include "nebula_common/nebula_status.hpp"
+#include "nebula_common/util/string_conversions.hpp"
 
 #include <cmath>
 #include <fstream>
 #include <iostream>
 #include <map>
 #include <sstream>
+#include <stdexcept>
 #include <string>
 #include <vector>
 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,28 +482,34 @@ 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;
       if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5;
       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;

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 <ostream>
+#include <sstream>
+#include <string>
+#include <type_traits>
+
+namespace nebula::util
+{
+
+template <typename T, typename = void>
+struct IsStreamable : std::false_type
+{
+};
+
+template <typename T>
+struct IsStreamable<T, std::void_t<decltype(std::declval<std::ostream &>() << std::declval<T>())> >
+: std::true_type
+{
+};
+
+template <typename T>
+std::enable_if_t<IsStreamable<T>::value, std::string> to_string(const T & value)
+{
+  std::stringstream ss{};
+  ss << value;
+  return ss.str();
+}
+
+}  // namespace nebula::util

nebula_ros/schema/Pandar40P.schema.json

@@ -62,6 +62,7 @@
           "enum": [
             "Last",
             "Strongest",
+            "LastStrongest",
             "Dual"
           ]
         },

nebula_ros/schema/Pandar64.schema.json

@@ -59,6 +59,7 @@
           "enum": [
             "Last",
             "Strongest",
+            "LastStrongest",
             "Dual"
           ]
         },

nebula_ros/schema/PandarAT128.schema.json

@@ -80,9 +80,6 @@
             "Last",
             "Strongest",
             "LastStrongest",
-            "First",
-            "LastFirst",
-            "FirstStrongest",
             "Dual"
           ]
         },

nebula_ros/schema/PandarQT64.schema.json

@@ -62,6 +62,7 @@
           "enum": [
             "Last",
             "First",
+            "LastFirst",
             "Dual"
           ]
         },