Support UUST2 with fixed firmware

README.md

@@ -11,11 +11,16 @@ Also, the resolution of the timestamp was not enough for a high-speed motion of
 
 So, urg\_stamped estimates sub-millisecond by the following algorithm:
 
+- Select the algorithms based on the sensor model
+  - UTM: UTM-30LX-EW
+  - UST(UUST1): UST-\*LX, UST-\*LC with firmware version <4.0.0
+  - UST(UUST2-Unfixed): UST-\*LX, UST-\*LC with firmware version >=4.0.0 and <4.0.3
+  - UST(UUST2-Fixed): UST-\*LX, UST-\*LC with firmware version >=4.0.3
 - Determine sensor internal clock state (clock offset and gain) using TM command (Triggered by 30s timer by default)
-  - UTM/UST(UUST1): (UTM and UST(UUST1) responds to TM command as expected in SCIP2 protocol)
+  - UTM/UST(UUST1)/UST(UUST2-Fixed): (These models respond to TM command as expected in SCIP2 protocol)
     - 1. Observe sub-millisecond clock offset by finding increment of millisecond resolution sensor timestamp
     - 2. Observe clock gain from multiple observations of the clock offset
-  - UST(UUST2): (UST(UUST2) responds to TM command on the next 5ms frame which breaks SCIP2's time synchronization logic)
+  - UST(UUST2-Unfixed): (This model responds to TM command on the next 5ms frame which breaks SCIP2's time synchronization logic)
     - 1. Request TM command many times with different timing
     - 2. Filter responses with large delay
     - 3. Collect sensor response timings which should be synchronized to the sensor timestamp increment
@@ -29,7 +34,7 @@ So, urg\_stamped estimates sub-millisecond by the following algorithm:
     - 2. Observe scan origin time and scan interval using scan timestamp jitter
 - Calculate sub-millisecond scan timestamp based on the observed scan origin time and scan interval
 
-LaserScan data is stopped during sensor internal clock estimation which takes at most ~100ms on UTM/UUST1 and ~1s on UUST2.
+LaserScan data is stopped during sensor internal clock estimation which takes at most ~100ms on UTM/UST(UUST1)/UST(UUST2-Fixed) and ~1s on UST(UUST2-Unfixed).
 To avoid stopping all sensors at once on multi-sensor configuration, urg\_stamped automatically adjusts the timing of sensor internal clock estimation based on the messages on urg\_stamped\_sync\_start topic.
 
 ## Usages

include/urg_stamped/strings.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright 2024 The urg_stamped Authors
+ *
+ * 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.
+ */
+
+#ifndef URG_STAMPED_STRINGS_H
+#define URG_STAMPED_STRINGS_H
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace urg_stamped
+{
+namespace strings
+{
+inline std::vector<std::string> split(const std::string& s, const char delim)
+{
+  std::vector<std::string> out;
+  std::stringstream stream(s);
+  std::string el;
+  while (std::getline(stream, el, delim))
+  {
+    out.push_back(el);
+  }
+  return out;
+}
+};  // namespace strings
+};  // namespace urg_stamped
+
+#endif  // URG_STAMPED_STRINGS_H
+

src/urg_stamped.cpp

@@ -39,6 +39,7 @@
 #include <scip2/logger.h>
 
 #include <urg_stamped/ros_logger.h>
+#include <urg_stamped/strings.h>
 
 #include <urg_stamped/urg_stamped.h>
 
@@ -333,19 +334,32 @@
   }
   if (!est_)
   {
-    int firm_major;
+    int firm_major = 1;
+    int firm_minor = 0;
+    int firm_patch = 0;
     {
       const auto firm_it = params.find("FIRM");
       if (firm_it == params.end())
       {
         scip2::logger::error()
             << "Could not detect sensor hardware revision. Fallback to UUST1 mode"
             << std::endl;
-        firm_major = 1;
       }
       else
       {
-        firm_major = std::stoi(firm_it->second);
+        const std::vector<std::string> version = urg_stamped::strings::split(firm_it->second, '.');
+        if (version.size() < 3)
+        {
+          scip2::logger::error()
+              << "Invalid firmware version (" << firm_it->second << "). Fallback to UUST1 mode"
+              << std::endl;
+        }
+        else
+        {
+          firm_major = std::stoi(version[0]);
+          firm_minor = std::stoi(version[1]);
+          firm_patch = std::stoi(version[2]);
+        }
       }
     }
     std::string prod;
@@ -380,15 +394,15 @@
             << "Unknown sensor model. Fallback to UST mode"
             << std::endl;
       }
-      if (firm_major >= 4)
+      if (firm_major == 4 && firm_minor == 0 && firm_patch < 3)
       {
-        model = "UST (UUST2)";
+        model = "UST (UUST2Unfixed)";
         clock.reset(new device_state_estimator::ClockEstimatorUUST2());
         is_uust2_ = true;
       }
       else
       {
-        model = "UST (UUST1)";
+        model = "UST (UUST1, UUST2Fixed)";
         clock.reset(new device_state_estimator::ClockEstimatorUUST1());
       }
       scan.reset(new device_state_estimator::ScanEstimatorUST(clock, ideal_scan_interval_));
@@ -586,15 +600,13 @@
   tm_try_count_ = 0;
   scip2::logger::debug() << "Starting communication delay estimation" << std::endl;
   delay_estim_state_ = DelayEstimState::STOPPING_SCAN;
-  timer_retry_tm_.stop();
-  timer_retry_tm_ = nh_.createTimer(
-      tm_command_interval_,
-      &UrgStampedNode::retryTM, this);
   retryTM();
 }
 
 void UrgStampedNode::retryTM(const ros::TimerEvent& event)
 {
+  bool retry = true;
+  timer_retry_tm_.stop();
   switch (delay_estim_state_)
   {
     case DelayEstimState::STOPPING_SCAN:
@@ -620,10 +632,16 @@
     case DelayEstimState::ESTIMATING:
       scip2::logger::warn() << "Timeout occured during the time synchronization" << std::endl;
       scip_->sendCommand("TM2");
+      retry = false;
       break;
     default:
+      retry = false;
       break;
   }
+  if (retry)
+  {
+    timer_retry_tm_ = nh_.createTimer(tm_command_interval_, &UrgStampedNode::retryTM, this, true);
+  }
 }
 
 void UrgStampedNode::errorCountIncrement(const std::string& status)

test/CMakeLists.txt

@@ -14,6 +14,8 @@ target_link_libraries(test_device_state_estimator ${catkin_LIBRARIES} ${Boost_LI
 catkin_add_gtest(test_decode src/test_decode.cpp)
 target_link_libraries(test_decode ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
+catkin_add_gtest(test_strings src/test_strings.cpp)
+
 catkin_add_gtest(test_param src/test_param.cpp)
 
 catkin_add_gtest(test_scip2

test/src/test_strings.cpp

@@ -0,0 +1,39 @@
+/*
+ * Copyright 2024 The urg_stamped Authors
+ *
+ * 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.
+ */
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#include <urg_stamped/strings.h>
+
+TEST(Strings, Split)
+{
+  const std::string in("1.23.456");
+  const std::vector<std::string> out = urg_stamped::strings::split(in, '.');
+  ASSERT_EQ(out.size(), 3);
+  ASSERT_EQ(out[0], std::string("1"));
+  ASSERT_EQ(out[1], std::string("23"));
+  ASSERT_EQ(out[2], std::string("456"));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  return RUN_ALL_TESTS();
+}