feat: Time Delay Compensation Package

.github/CODEOWNERS

@@ -16,7 +16,7 @@ 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/** satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
@@ -34,7 +34,7 @@ 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,7 +63,7 @@ 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/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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_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/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp
@@ -109,7 +109,7 @@ 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/** 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 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/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

.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

.github/workflows/sync-upstream.yaml

@@ -0,0 +1,28 @@
+name: sync-upstream
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  sync-upstream:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-branches
+        uses: autowarefoundation/autoware-github-actions/sync-branches@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          base-branch: tier4/main
+          sync-pr-branch: sync-upstream
+          sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git
+          sync-target-branch: main
+          pr-title: "chore: sync upstream"
+          auto-merge-method: merge

.gitmodules

@@ -0,0 +1,7 @@
+[submodule "control/mpc_nonlinear/external_libs/CppAD"]
+	path = control/mpc_nonlinear/external_libs/CppAD
+	url = git@github.com:coin-or/CppAD.git
+[submodule "control/mpc_nonlinear/external_libs/CppADCodeGen"]
+	path = control/mpc_nonlinear/external_libs/CppADCodeGen
+	url = https://github.com/joaoleal/CppADCodeGen.git
+

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/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/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>