Remove member function AutowareUniverse::updateLocalization

Signed-off-by: yamacir-kit <[email protected]>
tier4 · Dec 24, 2024 · 14f2990 · 14f2990
1 parent 3fd6cde
commit 14f2990
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 52 deletions.
diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp
@@ -83,7 +83,7 @@ class AutowareUniverse : public rclcpp::Node,
 
   const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer;
 
-  std::thread localization_and_vehicle_state_update_thread;
+  std::thread spinner;
 
   std::atomic<bool> is_stop_requested = false;
 
@@ -100,8 +100,6 @@ class AutowareUniverse : public rclcpp::Node,
 
   auto rethrow() -> void;
 
-  auto updateLocalization() -> void;
-
   auto updateVehicleState() -> void;
 
   auto getVehicleCommand() const -> std::tuple<double, double, double, double, int>;

diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp
@@ -64,13 +64,56 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization)
           break;
       }
     })),
-  localization_update_timer(
-    rclcpp::create_timer(  // Autoware.Universe requires localization topics to send data at 50Hz
-      this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })),
+  localization_update_timer(rclcpp::create_timer(
+    this, get_clock(),
+    std::chrono::milliseconds(
+      20),  // Autoware.Universe requires localization topics to send data at 50Hz
+    [this]() {
+      setAcceleration([this]() {
+        AccelWithCovarianceStamped message;
+        message.header.stamp = get_clock()->now();
+        message.header.frame_id = "/base_link";
+        message.accel.accel = current_acceleration.load();
+        message.accel.covariance.at(6 * 0 + 0) = 0.001;  // linear x
+        message.accel.covariance.at(6 * 1 + 1) = 0.001;  // linear y
+        message.accel.covariance.at(6 * 2 + 2) = 0.001;  // linear z
+        message.accel.covariance.at(6 * 3 + 3) = 0.001;  // angular x
+        message.accel.covariance.at(6 * 4 + 4) = 0.001;  // angular y
+        message.accel.covariance.at(6 * 5 + 5) = 0.001;  // angular z
+        return message;
+      }());
+
+      setOdometry([this]() {
+        Odometry message;
+        message.header.stamp = get_clock()->now();
+        message.header.frame_id = "map";
+        message.pose.pose = current_pose.load();
+        message.pose.covariance = {};
+        message.twist.twist = current_twist.load();
+        return message;
+      }());
+
+      setPose([this]() {
+        // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803
+        PoseWithCovarianceStamped message;
+        message.header.stamp = get_clock()->now();
+        message.header.frame_id = "map";
+        message.pose.pose = current_pose.load();
+        message.pose.covariance.at(6 * 0 + 0) = 0.0225;    // XYZRPY_COV_IDX::X_X
+        message.pose.covariance.at(6 * 1 + 1) = 0.0225;    // XYZRPY_COV_IDX::Y_Y
+        message.pose.covariance.at(6 * 2 + 2) = 0.0225;    // XYZRPY_COV_IDX::Z_Z
+        message.pose.covariance.at(6 * 3 + 3) = 0.000625;  // XYZRPY_COV_IDX::ROLL_ROLL
+        message.pose.covariance.at(6 * 4 + 4) = 0.000625;  // XYZRPY_COV_IDX::PITCH_PITCH
+        message.pose.covariance.at(6 * 5 + 5) = 0.000625;  // XYZRPY_COV_IDX::YAW_YAW
+        return message;
+      }());
+
+      setTransform(current_pose.load());
+    })),
   vehicle_state_update_timer(
     rclcpp::create_timer(  // Autoware.Universe requires vehicle state topics to send data at 30Hz
       this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })),
-  localization_and_vehicle_state_update_thread(std::thread([this]() {
+  spinner(std::thread([this]() {
     try {
       while (rclcpp::ok() and not is_stop_requested.load()) {
         rclcpp::spin_some(get_node_base_interface());
@@ -86,7 +129,7 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization)
 AutowareUniverse::~AutowareUniverse()
 {
   is_stop_requested.store(true);
-  localization_and_vehicle_state_update_thread.join();
+  spinner.join();
 }
 
 auto AutowareUniverse::rethrow() -> void
@@ -96,50 +139,6 @@ auto AutowareUniverse::rethrow() -> void
   }
 }
 
-auto AutowareUniverse::updateLocalization() -> void
-{
-  setAcceleration([this]() {
-    AccelWithCovarianceStamped message;
-    message.header.stamp = get_clock()->now();
-    message.header.frame_id = "/base_link";
-    message.accel.accel = current_acceleration.load();
-    message.accel.covariance.at(6 * 0 + 0) = 0.001;  // linear x
-    message.accel.covariance.at(6 * 1 + 1) = 0.001;  // linear y
-    message.accel.covariance.at(6 * 2 + 2) = 0.001;  // linear z
-    message.accel.covariance.at(6 * 3 + 3) = 0.001;  // angular x
-    message.accel.covariance.at(6 * 4 + 4) = 0.001;  // angular y
-    message.accel.covariance.at(6 * 5 + 5) = 0.001;  // angular z
-    return message;
-  }());
-
-  setOdometry([this]() {
-    Odometry message;
-    message.header.stamp = get_clock()->now();
-    message.header.frame_id = "map";
-    message.pose.pose = current_pose.load();
-    message.pose.covariance = {};
-    message.twist.twist = current_twist.load();
-    return message;
-  }());
-
-  setPose([this]() {
-    // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803
-    PoseWithCovarianceStamped message;
-    message.header.stamp = get_clock()->now();
-    message.header.frame_id = "map";
-    message.pose.pose = current_pose.load();
-    message.pose.covariance.at(6 * 0 + 0) = 0.0225;    // XYZRPY_COV_IDX::X_X
-    message.pose.covariance.at(6 * 1 + 1) = 0.0225;    // XYZRPY_COV_IDX::Y_Y
-    message.pose.covariance.at(6 * 2 + 2) = 0.0225;    // XYZRPY_COV_IDX::Z_Z
-    message.pose.covariance.at(6 * 3 + 3) = 0.000625;  // XYZRPY_COV_IDX::ROLL_ROLL
-    message.pose.covariance.at(6 * 4 + 4) = 0.000625;  // XYZRPY_COV_IDX::PITCH_PITCH
-    message.pose.covariance.at(6 * 5 + 5) = 0.000625;  // XYZRPY_COV_IDX::YAW_YAW
-    return message;
-  }());
-
-  setTransform(current_pose.load());
-}
-
 auto AutowareUniverse::updateVehicleState() -> void
 {
   setControlModeReport(getControlModeReport());