Merge branch 'main' into feat/add_dynamic_lanelet_to_planning

Author: YamatoAndo

.cppcheck_suppressions

@@ -6,7 +6,6 @@ missingIncludeSystem
 noConstructor
 unknownMacro
 unmatchedSuppression
-unreadVariable
 unusedFunction
 useInitializationList
 useStlAlgorithm

.github/CODEOWNERS

@@ -134,7 +134,7 @@ perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier
 perception/autoware_shape_estimation/** kcolak@leodrive.ai yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp
-perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp
+perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp kotaro.uetake@tier4.jp
 perception/autoware_tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp
 perception/autoware_traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp
@@ -200,6 +200,7 @@ planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.
 planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp
 sensing/autoware_image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp
+sensing/autoware_imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp
 sensing/autoware_image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp
 sensing/autoware_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp
 sensing/autoware_pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
@@ -221,7 +222,7 @@ system/autoware_component_monitor/** memin@leodrive.ai
 system/autoware_processing_time_checker/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 system/bluetooth_monitor/** fumihito.ito@tier4.jp
 system/component_state_monitor/** isamu.takagi@tier4.jp
-system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp
+system/autoware_default_adapi/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp
 system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp
 system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp
 system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold = 0.0);
 
+/// @brief calculate the time_from_start fields of the given trajectory points
+/// @details this function assumes constant longitudinal velocity between points
+/// @param trajectory trajectory for which to calculate the time_from_start
+/// @param current_ego_point current ego position
+/// @param min_velocity minimum velocity used for a trajectory point
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);
+
 }  // namespace autoware::motion_utils
 
 #endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold);
 
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
+{
+  const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
+  if (nearest_segment_idx + 1 == trajectory.size()) {
+    return;
+  }
+  for (auto & p : trajectory) {
+    p.time_from_start = rclcpp::Duration::from_seconds(0);
+  }
+  // TODO(Maxime): some points can have very low velocities which introduce huge time errors
+  // Temporary solution: use a minimum velocity
+  for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
+    const auto & from = trajectory[idx - 1];
+    const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
+    if (velocity != 0.0) {
+      auto & to = trajectory[idx];
+      const auto t = universe_utils::calcDistance2d(from, to) / velocity;
+      to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
+    }
+  }
+}
 }  // namespace autoware::motion_utils

common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp

@@ -25,6 +25,7 @@
 #include <memory>
 #include <ostream>
 #include <string>
+#include <thread>
 #include <vector>
 
 namespace autoware::universe_utils
@@ -169,6 +170,7 @@ class TimeKeeper
   std::shared_ptr<ProcessingTimeNode>
     current_time_node_;                            //!< Shared pointer to the current time node
   std::shared_ptr<ProcessingTimeNode> root_node_;  //!< Shared pointer to the root time node
+  std::thread::id root_node_thread_id_;            //!< ID of the thread that started the tracking
   autoware::universe_utils::StopWatch<
     std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
     stop_watch_;  //!< StopWatch object for tracking the processing time

common/autoware_universe_utils/src/system/time_keeper.cpp

@@ -14,6 +14,9 @@
 
 #include "autoware/universe_utils/system/time_keeper.hpp"
 
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+
 #include <fmt/format.h>
 
 #include <stdexcept>
@@ -129,7 +132,14 @@ void TimeKeeper::start_track(const std::string & func_name)
   if (current_time_node_ == nullptr) {
     current_time_node_ = std::make_shared<ProcessingTimeNode>(func_name);
     root_node_ = current_time_node_;
+    root_node_thread_id_ = std::this_thread::get_id();
   } else {
+    if (root_node_thread_id_ != std::this_thread::get_id()) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("TimeKeeper"),
+        "TimeKeeper::start_track() is called from a different thread. Ignoring the call.");
+      return;
+    }
     current_time_node_ = current_time_node_->add_child(func_name);
   }
   stop_watch_.tic(func_name);
@@ -145,6 +155,9 @@ void TimeKeeper::comment(const std::string & comment)
 
 void TimeKeeper::end_track(const std::string & func_name)
 {
+  if (root_node_thread_id_ != std::this_thread::get_id()) {
+    return;
+  }
   if (current_time_node_->get_name() != func_name) {
     throw std::runtime_error(fmt::format(
       "You must call end_track({}) first, but end_track({}) is called",
@@ -178,7 +191,7 @@ ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & tim
   time_keeper_.start_track(func_name_);
 }
 
-ScopedTimeTrack::~ScopedTimeTrack()
+ScopedTimeTrack::~ScopedTimeTrack()  // NOLINT
 {
   time_keeper_.end_track(func_name_);
 }

common/autoware_universe_utils/test/src/system/test_time_keeper.cpp

@@ -11,7 +11,6 @@
 // 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 "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <rclcpp/node.hpp>
@@ -20,34 +19,74 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
-#include <iostream>
+#include <sstream>
 #include <thread>
 
-TEST(system, TimeKeeper)
+class TimeKeeperTest : public ::testing::Test
 {
-  using autoware::universe_utils::ScopedTimeTrack;
-  using autoware::universe_utils::TimeKeeper;
+protected:
+  std::ostringstream oss;
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr publisher;
+  std::unique_ptr<autoware::universe_utils::TimeKeeper> time_keeper;
 
-  rclcpp::Node node{"sample_node"};
-
-  auto publisher = node.create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
-    "~/debug/processing_time_tree", 1);
+  void SetUp() override
+  {
+    node = std::make_shared<rclcpp::Node>("test_node");
+    publisher = node->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+      "~/debug/processing_time_tree", 1);
+    time_keeper = std::make_unique<autoware::universe_utils::TimeKeeper>(&oss, publisher);
+  }
+};
 
-  TimeKeeper time_keeper(&std::cerr, publisher);
+TEST_F(TimeKeeperTest, BasicFunctionality)
+{
+  using autoware::universe_utils::ScopedTimeTrack;
 
-  ScopedTimeTrack st{"main_func", time_keeper};
+  {
+    ScopedTimeTrack st{"main_func", *time_keeper};
 
-  {  // funcA
-    ScopedTimeTrack st{"funcA", time_keeper};
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-  }
+    {  // funcA
+      ScopedTimeTrack st{"funcA", *time_keeper};
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
 
-  {  // funcB
-    ScopedTimeTrack st{"funcB", time_keeper};
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-    {  // funcC
-      ScopedTimeTrack st{"funcC", time_keeper};
-      std::this_thread::sleep_for(std::chrono::seconds(1));
+    {  // funcB
+      ScopedTimeTrack st{"funcB", *time_keeper};
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      {  // funcC
+        ScopedTimeTrack st{"funcC", *time_keeper};
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      }
     }
   }
+
+  // Check if the output contains all function names
+  std::string output = oss.str();
+  EXPECT_TRUE(output.find("main_func") != std::string::npos);
+  EXPECT_TRUE(output.find("funcA") != std::string::npos);
+  EXPECT_TRUE(output.find("funcB") != std::string::npos);
+  EXPECT_TRUE(output.find("funcC") != std::string::npos);
+}
+
+TEST_F(TimeKeeperTest, MultiThreadWarning)
+{
+  testing::internal::CaptureStderr();
+
+  std::thread t([this]() {
+    time_keeper->start_track("ThreadFunction");
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    time_keeper->end_track("ThreadFunction");
+  });
+
+  time_keeper->start_track("MainFunction");
+  std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  time_keeper->end_track("MainFunction");
+
+  t.join();
+
+  std::string err = testing::internal::GetCapturedStderr();
+  EXPECT_TRUE(
+    err.find("TimeKeeper::start_track() is called from a different thread. Ignoring the call.") !=
+    std::string::npos);
 }

common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp

@@ -99,6 +99,7 @@ void BirdEyeViewController::reset()
   y_property_->setFloat(0);
 }
 
+// cppcheck-suppress unusedFunction
 void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event)
 {
   if (event.shift()) {
@@ -178,11 +179,13 @@ void BirdEyeViewController::update(float dt, float ros_dt)
   updateCamera();
 }
 
+// cppcheck-suppress unusedFunction
 void BirdEyeViewController::lookAt(const Ogre::Vector3 & point)
 {
   setPosition(point - target_scene_node_->getPosition());
 }
 
+// cppcheck-suppress unusedFunction
 void BirdEyeViewController::onTargetFrameChanged(
   const Ogre::Vector3 & old_reference_position,
   const Ogre::Quaternion & /*old_reference_orientation*/)

common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp

@@ -226,6 +226,7 @@ void ThirdPersonViewController::updateCamera()
       distance_property_->getFloat() * CAMERA_OFFSET);
 }
 
+// cppcheck-suppress unusedFunction
 void ThirdPersonViewController::updateTargetSceneNode()
 {
   if (FramePositionTrackingViewController::getNewTransform()) {

common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp

@@ -30,13 +30,6 @@
 namespace traffic_light_utils
 {
 
-bool isRoiValid(
-  const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height);
-
-void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi);
-
-bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal);
-
 void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1);
 
 /**

common/traffic_light_utils/src/traffic_light_utils.cpp

@@ -17,29 +17,6 @@
 namespace traffic_light_utils
 {
 
-bool isRoiValid(
-  const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height)
-{
-  uint32_t x1 = roi.roi.x_offset;
-  uint32_t x2 = roi.roi.x_offset + roi.roi.width;
-  uint32_t y1 = roi.roi.y_offset;
-  uint32_t y2 = roi.roi.y_offset + roi.roi.height;
-  return roi.roi.width > 0 && roi.roi.height > 0 && x1 < width && y1 < height && x2 < width &&
-         y2 < height;
-}
-
-void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi)
-{
-  roi.roi.height = roi.roi.width = 0;
-}
-
-bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal)
-{
-  return signal.elements.size() == 1 &&
-         signal.elements[0].shape == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN &&
-         signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
-}
-
 void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence)
 {
   signal.elements.resize(1);

common/traffic_light_utils/test/test_traffic_light_utils.cpp

@@ -18,46 +18,6 @@
 namespace traffic_light_utils
 {
 
-TEST(isRoiValid, roi_validity)
-{
-  tier4_perception_msgs::msg::TrafficLightRoi test_roi;
-  test_roi.roi.x_offset = 300;
-  test_roi.roi.y_offset = 200;
-  test_roi.roi.width = 340;
-  test_roi.roi.height = 200;
-  uint32_t img_width = 640;
-  uint32_t img_heigh = 480;
-  EXPECT_FALSE(isRoiValid(test_roi, img_width, img_heigh));
-  test_roi.roi.width = 339;
-  EXPECT_TRUE(isRoiValid(test_roi, img_width, img_heigh));
-}
-
-TEST(setRoiInvalid, set_roi_size)
-{
-  tier4_perception_msgs::msg::TrafficLightRoi test_roi;
-  test_roi.roi.x_offset = 300;
-  test_roi.roi.y_offset = 200;
-  test_roi.roi.width = 300;
-  test_roi.roi.height = 200;
-  EXPECT_EQ(test_roi.roi.width, (uint32_t)300);
-  EXPECT_EQ(test_roi.roi.height, (uint32_t)200);
-  setRoiInvalid(test_roi);
-  EXPECT_EQ(test_roi.roi.width, (uint32_t)0);
-  EXPECT_EQ(test_roi.roi.height, (uint32_t)0);
-}
-
-TEST(isSignalUnknown, signal_element)
-{
-  tier4_perception_msgs::msg::TrafficLight test_signal;
-  tier4_perception_msgs::msg::TrafficLightElement element;
-  element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
-  element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
-  test_signal.elements.push_back(element);
-  EXPECT_TRUE(isSignalUnknown(test_signal));
-  test_signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::RED;
-  EXPECT_FALSE(isSignalUnknown(test_signal));
-}
-
 TEST(setSignalUnknown, set_signal_element)
 {
   tier4_perception_msgs::msg::TrafficLight test_signal;

control/autoware_autonomous_emergency_braking/README.md

@@ -8,8 +8,6 @@
 
 This module has following assumptions.
 
-- It is used when driving at low speeds (about 15 km/h).
-
 - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both.
 
 - The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles.
@@ -25,7 +23,7 @@ The pros and cons of both approaches are:
 IMU angular velocity:
 
 - (+) Usually, it has high accuracy
-- (-)Vehicle vibration might introduce noise.
+- (-) Vehicle vibration might introduce noise.
 
 Steering angle:
 
@@ -186,7 +184,7 @@ The AEB module can also prevent collisions when the ego vehicle is moving backwa
 
 When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision.
 
-![backward driving](./image/wrong-mpc.drawio.svg)
+![wrong mpc](./image/wrong-mpc.drawio.svg)
 
 ## Parameters
 
@@ -217,6 +215,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t
 
 ## Limitations
 
+- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly.
+
 - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud.
 
 - Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise.