chore: refactoring

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
autowarefoundation · May 17, 2024 · 2ffb7d8 · 2ffb7d8
1 parent 9edd2e0
commit 2ffb7d8
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 26 deletions.
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp
@@ -97,26 +97,6 @@ class TrackerObjectDebugger
     visualization_msgs::msg::MarkerArray & marker_array) const;
   void process();
   void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const;
-
-private:
-  std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
-  {
-    std::stringstream ss;
-    for (auto i = 0; i < 16; ++i) {
-      ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i];
-    }
-    return ss.str();
-  }
-
-  boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
-  {
-    const std::string uuid_str = uuid_to_string(uuid_msg);
-    boost::uuids::string_generator gen;
-    boost::uuids::uuid uuid = gen(uuid_str);
-    return uuid;
-  }
-
-  int32_t uuid_to_int(const boost::uuids::uuid & uuid) { return boost::uuids::hash_value(uuid); }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -23,6 +23,31 @@
 #include <sstream>
 #include <string>
 
+namespace
+{
+std::string uuidToString(const unique_identifier_msgs::msg::UUID & uuid_msg)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +uuid_msg.uuid[i];
+  }
+  return ss.str();
+}
+
+boost::uuids::uuid uuidToBoostUuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
+{
+  const std::string uuid_str = uuidToString(uuid_msg);
+  boost::uuids::string_generator gen;
+  boost::uuids::uuid uuid = gen(uuid_str);
+  return uuid;
+}
+
+int32_t uuidToInt(const boost::uuids::uuid & uuid)
+{
+  return boost::uuids::hash_value(uuid);
+}
+}  // namespace
+
 TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
 {
   // set frame id
@@ -67,9 +92,9 @@ void TrackerObjectDebugger::collect(
 
     autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
     (*(tracker_itr))->getTrackedObject(message_time, tracked_object);
-    object_data.uuid = to_boost_uuid(tracked_object.object_id);
-    object_data.uuid_int = uuid_to_int(object_data.uuid);
-    object_data.uuid_str = uuid_to_string(tracked_object.object_id);
+    object_data.uuid = uuidToBoostUuid(tracked_object.object_id);
+    object_data.uuid_int = uuidToInt(object_data.uuid);
+    object_data.uuid_str = uuidToString(tracked_object.object_id);
 
     // tracker
     bool is_associated = false;

diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -30,12 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_
   if (debug_settings_.publish_tentative_objects) {
     debug_tentative_objects_pub_ =
       node_.create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>(
-        "multi_object_tracker/debug/tentative_objects", rclcpp::QoS{1});
+        "~/debug/tentative_objects", rclcpp::QoS{1});
   }
 
   if (debug_settings_.publish_debug_markers) {
     debug_objects_markers_pub_ = node_.create_publisher<visualization_msgs::msg::MarkerArray>(
-      "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1});
+      "~/debug/objects_markers", rclcpp::QoS{1});
   }
 
   // initialize timestamps

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -122,7 +122,8 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
 
   // Update time
   latest_message_time_ = now;
-  if (std::abs((latest_measurement_time_ - objects_time).seconds()) > 3.0) {
+  constexpr double delay_threshold = 3.0;  // [s]
+  if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) {
     // Reset the latest measurement time if the time difference is too large
     latest_measurement_time_ = objects_time;
     RCLCPP_WARN(