chore: sync upstream

.github/CODEOWNERS

@@ -16,25 +16,25 @@ common/global_parameter_loader/** kenji.miyake@tier4.jp
 common/goal_distance_calculator/** taiki.tanaka@tier4.jp
 common/grid_map_utils/** maxime.clement@tier4.jp
 common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
-common/kalman_filter/** yukihiro.saito@tier4.jp
+common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
 common/motion_common/** christopherj.ho@gmail.com
 common/motion_testing/** christopherj.ho@gmail.com
-common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp
+common/motion_utils/** satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
 common/neural_networks_provider/** ambroise.vincent@arm.com
 common/osqp_interface/** maxime.clement@tier4.jp
 common/path_distance_calculator/** isamu.takagi@tier4.jp
-common/perception_utils/** takayuki.murooka@tier4.jp
+common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp
 common/polar_grid/** yukihiro.saito@tier4.jp
 common/signal_processing/** takayuki.murooka@tier4.jp
 common/tier4_api_utils/** isamu.takagi@tier4.jp
-common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp
+common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp
 common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp
 common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
 common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
 common/tier4_debug_tools/** kenji.miyake@tier4.jp
 common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp
 common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
-common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp
+common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp
 common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp
 common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp
 common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp
@@ -63,23 +63,23 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp
 launch/tier4_localization_launch/** yamato.ando@tier4.jp
 launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp
 launch/tier4_perception_launch/** yukihiro.saito@tier4.jp
-launch/tier4_planning_launch/** takamasa.horibe@tier4.jp
+launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp
 launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp
 launch/tier4_simulator_launch/** keisuke.shima@tier4.jp
-launch/tier4_system_launch/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp
+launch/tier4_system_launch/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp
 launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp
-localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp
+localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp yamato.ando@tier4.jp
 localization/gyro_odometer/** yamato.ando@tier4.jp
 localization/initial_pose_button_panel/** yamato.ando@tier4.jp
 localization/localization_error_monitor/** yamato.ando@tier4.jp
 localization/ndt/** yamato.ando@tier4.jp
 localization/ndt_pcl_modified/** yamato.ando@tier4.jp
 localization/ndt_scan_matcher/** yamato.ando@tier4.jp
 localization/pose2twist/** yamato.ando@tier4.jp
-localization/pose_initializer/** yamato.ando@tier4.jp isamu.takagi@tier4.jp
+localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp
 localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp
 localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp
-map/map_loader/** ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp
+map/map_loader/** kenji.miyake@tier4.jp ryohsuke.mitsudome@tier4.jp
 map/map_tf_generator/** azumi.suzuki@tier4.jp
 map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp
 perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
@@ -96,7 +96,7 @@ perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muram
 perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
 perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp
 perception/map_based_prediction/** yutaka.shimizu@tier4.jp
-perception/multi_object_tracker/** yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp
+perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp
 perception/object_merger/** yukihiro.saito@tier4.jp
 perception/object_range_splitter/** yukihiro.saito@tier4.jp
 perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
@@ -108,8 +108,8 @@ perception/traffic_light_classifier/** yukihiro.saito@tier4.jp
 perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp
 perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp
 perception/traffic_light_visualization/** yukihiro.saito@tier4.jp
-planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
-planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
+planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@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 yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp
+planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp
 planning/costmap_generator/** kenji.miyake@tier4.jp
 planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp
 planning/freespace_planner/** takamasa.horibe@tier4.jp
@@ -119,7 +119,7 @@ planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp
 planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp
 planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
 planning/obstacle_stop_planner/** satoshi.ota@tier4.jp
-planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp
+planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp
 planning/planning_error_monitor/** yutaka.shimizu@tier4.jp
 planning/planning_evaluator/** maxime.clement@tier4.jp
 planning/route_handler/** fumiya.watanabe@tier4.jp
@@ -148,7 +148,7 @@ system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.j
 system/dummy_diag_publisher/** kenji.miyake@tier4.jp
 system/dummy_infrastructure/** kenji.miyake@tier4.jp
 system/emergency_handler/** kenji.miyake@tier4.jp
-system/system_error_monitor/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp
+system/system_error_monitor/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp
 system/system_monitor/** fumihito.ito@tier4.jp
 system/topic_state_monitor/** kenji.miyake@tier4.jp
 system/velodyne_monitor/** fumihito.ito@tier4.jp
@@ -157,4 +157,4 @@ vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@t
 vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp
 vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp
 vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp
-vehicle/vehicle_info_util/** yamato.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp
+vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp

.github/workflows/build-and-test-differential.yaml

@@ -80,7 +80,7 @@ jobs:
 
       - name: Get modified files
         id: get-modified-files
-        uses: tj-actions/changed-files@v29
+        uses: tj-actions/changed-files@v31
         with:
           files: |
             **/*.cpp

.github/workflows/cancel-previous-workflows.yaml

@@ -8,7 +8,7 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - name: Cancel previous runs
-        uses: styfle/cancel-workflow-action@0.10.0
+        uses: styfle/cancel-workflow-action@0.10.1
         with:
           workflow_id: all
           all_but_latest: true

common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp

@@ -48,15 +48,13 @@ class TimeDelayKalmanFilter : public KalmanFilter
 
   /**
    * @brief get latest time estimated state
-   * @param x latest time estimated state
    */
-  void getLatestX(Eigen::MatrixXd & x);
+  Eigen::MatrixXd getLatestX() const;
 
   /**
    * @brief get latest time estimation covariance
-   * @param P latest time estimation covariance
    */
-  void getLatestP(Eigen::MatrixXd & P);
+  Eigen::MatrixXd getLatestP() const;
 
   /**
    * @brief calculate kalman filter covariance by precision model with time delay. This is mainly

common/kalman_filter/package.xml

@@ -5,6 +5,8 @@
   <version>0.1.0</version>
   <description>The kalman filter package</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="takeshi.ishita@tier4.jp">Takeshi Ishita</maintainer>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>

common/kalman_filter/src/time_delay_kalman_filter.cpp

@@ -32,8 +32,9 @@ void TimeDelayKalmanFilter::init(
   }
 }
 
-void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd & x) { x = x_.block(0, 0, dim_x_, 1); }
-void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd & P) { P = P_.block(0, 0, dim_x_, dim_x_); }
+Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const { return x_.block(0, 0, dim_x_, 1); }
+
+Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const { return P_.block(0, 0, dim_x_, dim_x_); }
 
 bool TimeDelayKalmanFilter::predictWithDelay(
   const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q)

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -347,7 +347,8 @@ boost::optional<size_t> findNearestSegmentIndex(
  */
 template <class T>
 double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx,
+  const bool throw_exception = false)
 {
   const auto overlap_removed_points = removeOverlapPoints(points, 0);
 
@@ -371,8 +372,6 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
-
   const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
   const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
 
@@ -383,6 +382,36 @@ double calcLateralOffset(
   return cross_vec(2) / segment_vec.norm();
 }
 
+template <class T>
+double calcLateralOffset(
+  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+{
+  const auto overlap_removed_points = removeOverlapPoints(points, 0);
+
+  if (throw_exception) {
+    validateNonEmpty(overlap_removed_points);
+  } else {
+    try {
+      validateNonEmpty(overlap_removed_points);
+    } catch (const std::exception & e) {
+      std::cerr << e.what() << std::endl;
+      return std::nan("");
+    }
+  }
+
+  if (overlap_removed_points.size() == 1) {
+    const std::runtime_error e("Same points are given.");
+    if (throw_exception) {
+      throw e;
+    }
+    std::cerr << e.what() << std::endl;
+    return std::nan("");
+  }
+
+  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
+  return calcLateralOffset(points, p_target, seg_idx, throw_exception);
+}
+
 /**
  * @brief calcSignedArcLength from index to index
  */

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -564,6 +564,67 @@ TEST(trajectory, calcLateralOffset)
   EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon);
 }
 
+TEST(trajectory, calcLateralOffset_without_segment_idx)
+{
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  const bool throw_exception = true;
+
+  // Empty
+  EXPECT_THROW(
+    calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception),
+    std::invalid_argument);
+
+  // Trajectory size is 1
+  {
+    const auto one_point_traj = generateTestTrajectory<Trajectory>(1, 1.0);
+    EXPECT_THROW(
+      calcLateralOffset(
+        one_point_traj.points, geometry_msgs::msg::Point{}, static_cast<size_t>(0),
+        throw_exception),
+      std::runtime_error);
+  }
+
+  // Same close points in trajectory
+  {
+    const auto invalid_traj = generateTestTrajectory<Trajectory>(10, 0.0);
+    const auto p = createPoint(3.0, 0.0, 0.0);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(2), throw_exception),
+      std::runtime_error);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(3), throw_exception),
+      std::runtime_error);
+  }
+
+  // Point on trajectory
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast<size_t>(3)), 0.0,
+    epsilon);
+
+  // Point before start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast<size_t>(0)), 3.0,
+    epsilon);
+
+  // Point after start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast<size_t>(8)), -10.0,
+    epsilon);
+
+  // Random cases
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast<size_t>(4)), 7.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(0)), -3.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(1)), -3.0,
+    epsilon);
+}
+
 TEST(trajectory, calcLateralOffset_CurveTrajectory)
 {
   using motion_utils::calcLateralOffset;

common/perception_utils/package.xml

@@ -6,6 +6,8 @@
   <description>The perception_utils package</description>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <maintainer email="yusuke.muramatsu@tier4.jp">Yusuke Muramatsu</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/rtc_manager_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.14)
+project(rtc_manager_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/rtc_manager_panel.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  icons
+  plugins
+)

common/rtc_manager_rviz_plugin/README.md

@@ -0,0 +1,20 @@
+# rtc_manager_rviz_plugin
+
+## Purpose
+
+This plugin displays each content of RTC status and switches each module of RTC auto mode.
+
+## Inputs / Outputs
+
+### Input
+
+tbd.
+
+### Output
+
+tbd.
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+   ![select_panel](./images/select_panels.png)

common/rtc_manager_rviz_plugin/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtc_manager_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The rtc manager rviz plugin package</description>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>tier4_rtc_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/rtc_manager_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="rtc_manager_rviz_plugin">
+
+  <class
+    type="rviz_plugins::RTCManagerPanel"
+    base_class_type="rviz_common::Panel">
+    <description>RTCManagerPanel</description>
+  </class>
+
+</library>