Merge branch 'main' into feat/sigma_loss

Author: technolojin

.github/CODEOWNERS

@@ -205,7 +205,7 @@ planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp
 planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp
 planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp
 planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
-planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
+planning/static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
 sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 sensing/image_diagnostics/** dai.nguyen@tier4.jp

build_depends.repos

@@ -41,3 +41,8 @@ repositories:
     type: git
     url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
     version: main
+  #vehicle
+  vehicle/sample_vehicle_launch:
+    type: git
+    url: https://github.com/autowarefoundation/sample_vehicle_launch.git
+    version: main

common/global_parameter_loader/CMakeLists.txt

@@ -4,6 +4,11 @@ project(global_parameter_loader)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/*.cpp)
+  ament_add_ros_isolated_gtest(test_global_params_launch ${test_files})
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch

common/global_parameter_loader/package.xml

@@ -10,8 +10,10 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <exec_depend>vehicle_info_util</exec_depend>
+  <depend>sample_vehicle_description</depend>
+  <depend>vehicle_info_util</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

common/global_parameter_loader/test/test_global_params_launch.cpp

@@ -0,0 +1,44 @@
+// Copyright 2023 The Autoware Foundation
+//
+// 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 <cstdlib>
+#include <iostream>
+#include <string>
+
+TEST(TestLaunchFile, test_launch_file)
+{
+  // Define the path of Python launch file
+  std::string global_params_launch_path = "global_params.launch.py";
+
+  // Define the parameters you want to pass to the launch file
+  std::string use_sim_time_param = "false";
+  std::string vehicle_model_param = "sample_vehicle";
+  // Construct the command to run the Python launch script with parameters
+  std::string command = "ros2 launch global_parameter_loader " + global_params_launch_path +
+                        " use_sim_time:=" + use_sim_time_param +
+                        " vehicle_model:=" + vehicle_model_param;
+
+  // Use the system() function to execute the command
+  int result = std::system(command.c_str());
+  // Check the result of running the launch file
+  EXPECT_EQ(result, 0);
+}
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

common/object_recognition_utils/include/object_recognition_utils/matching.hpp

@@ -29,6 +29,8 @@
 namespace object_recognition_utils
 {
 using tier4_autoware_utils::Polygon2d;
+// minimum area to avoid division by zero
+static const double MIN_AREA = 1e-6;
 
 inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon)
 {
@@ -66,10 +68,12 @@ template <class T1, class T2>
 double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
+  if (intersection_area < MIN_AREA) return 0.0;
   const double union_area = getUnionArea(source_polygon, target_polygon);
 
   const double iou =
@@ -81,7 +85,9 @@ template <class T1, class T2>
 double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
   const double union_area = getUnionArea(source_polygon, target_polygon);
@@ -95,11 +101,13 @@ template <class T1, class T2>
 double get2dPrecision(const T1 source_object, const T2 target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  const double source_area = boost::geometry::area(source_polygon);
+  if (source_area < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
-  const double source_area = boost::geometry::area(source_polygon);
+  if (intersection_area < MIN_AREA) return 0.0;
 
   return std::min(1.0, intersection_area / source_area);
 }
@@ -108,11 +116,13 @@ template <class T1, class T2>
 double get2dRecall(const T1 source_object, const T2 target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  const double target_area = boost::geometry::area(target_polygon);
+  if (target_area < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
-  const double target_area = boost::geometry::area(target_polygon);
+  if (intersection_area < MIN_AREA) return 0.0;
 
   return std::min(1.0, intersection_area / target_area);
 }

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

@@ -0,0 +1,66 @@
+// 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.
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+
+template <typename T>
+class InterProcessPollingSubscriber
+{
+private:
+  typename rclcpp::Subscription<T>::SharedPtr subscriber_;
+  std::optional<T> data_;
+
+public:
+  explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+  {
+    auto noexec_callback_group =
+      node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto noexec_subscription_options = rclcpp::SubscriptionOptions();
+    noexec_subscription_options.callback_group = noexec_callback_group;
+
+    subscriber_ = node->create_subscription<T>(
+      topic_name, rclcpp::QoS{1},
+      [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
+      noexec_subscription_options);
+  };
+  bool updateLatestData()
+  {
+    rclcpp::MessageInfo message_info;
+    T tmp;
+    // The queue size (QoS) must be 1 to get the last message data.
+    if (subscriber_->take(tmp, message_info)) {
+      data_ = tmp;
+    }
+    return data_.has_value();
+  };
+  const T & getData() const
+  {
+    if (!data_.has_value()) {
+      throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
+    }
+    return data_.value();
+  };
+};
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_

control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp

@@ -537,6 +537,10 @@ void PredictedPathCheckerNode::filterObstacles(
   const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold,
   const bool use_prediction, PredictedObjects & filtered_objects)
 {
+  if (traj.size() < 2) {
+    RCLCPP_ERROR(rclcpp::get_logger("predicted_path_checker"), "Trajectory size is less than 2.");
+    return;
+  }
   filtered_objects.header.frame_id = object_ptr_.get()->header.frame_id;
   filtered_objects.header.stamp = this->now();
 

evaluator/perception_online_evaluator/README.md

@@ -11,6 +11,7 @@ This module allows for the evaluation of how accurately perception results are g
 - Calculates lateral deviation between the predicted path and the actual traveled trajectory.
 - Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition.
 - Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
+- Calculates yaw rate based on the yaw of the object received in the previous cycle to evaluate the stability of the yaw rate recognition.
 
 ## Inputs / Outputs
 

evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp

@@ -63,8 +63,8 @@ MarkerArray createPoseMarkerArray(
   const float & b);
 
 MarkerArray createPosesMarkerArray(
-  const std::vector<Pose> poses, std::string && ns, const float & r, const float & g,
-  const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2,
+  const std::vector<Pose> poses, std::string && ns, const int32_t & first_id, const float & r,
+  const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2,
   const float & z_scale = 0.2);
 
 std_msgs::msg::ColorRGBA createColorFromString(const std::string & str);
@@ -75,7 +75,7 @@ MarkerArray createObjectPolygonMarkerArray(
 
 MarkerArray createDeviationLines(
   const std::vector<Pose> poses1, const std::vector<Pose> poses2, const std::string & ns,
-  const float r, const float g, const float b);
+  const int32_t & first_id, const float r, const float g, const float b);
 
 }  // namespace marker_utils
 

evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml

@@ -11,7 +11,7 @@
 
     prediction_time_horizons: [1.0, 2.0, 3.0, 5.0]
 
-    stopped_velocity_threshold: 0.3
+    stopped_velocity_threshold: 1.0
 
     target_object:
       car: