Merge branch 'main' into fix/obstacle_point_z

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -64,6 +64,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
     h_layout->addWidget(makeRoutingGroup());
     h_layout->addWidget(makeLocalizationGroup());
     h_layout->addWidget(makeMotionGroup());
+    h_layout->addWidget(makeFailSafeGroup());
     v_layout->addLayout(h_layout);
   }
 
@@ -186,6 +187,25 @@ QGroupBox * AutowareStatePanel::makeMotionGroup()
   return group;
 }
 
+QGroupBox * AutowareStatePanel::makeFailSafeGroup()
+{
+  auto * group = new QGroupBox("FalSafe");
+  auto * grid = new QGridLayout;
+
+  mrm_state_label_ptr_ = new QLabel("INIT");
+  mrm_state_label_ptr_->setAlignment(Qt::AlignCenter);
+  mrm_state_label_ptr_->setStyleSheet("border:1px solid black;");
+  grid->addWidget(mrm_state_label_ptr_, 0, 0);
+
+  mrm_behavior_label_ptr_ = new QLabel("INIT");
+  mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter);
+  mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;");
+  grid->addWidget(mrm_behavior_label_ptr_, 1, 0);
+
+  group->setLayout(grid);
+  return group;
+}
+
 void AutowareStatePanel::onInitialize()
 {
   raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
@@ -234,6 +254,12 @@ void AutowareStatePanel::onInitialize()
   client_accept_start_ = raw_node_->create_client<AcceptStart>(
     "/api/motion/accept_start", rmw_qos_profile_services_default);
 
+  // FailSafe
+  sub_mrm_ = raw_node_->create_subscription<MRMState>(
+    "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(),
+    std::bind(&AutowareStatePanel::onMRMState, this, _1));
+
+  // Others
   sub_gear_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
     "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1));
 
@@ -421,6 +447,72 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg)
   }
 }
 
+void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg)
+{
+  // state
+  {
+    QString text = "";
+    QString style_sheet = "";
+    switch (msg->state) {
+      case MRMState::NONE:
+        text = "NONE";
+        style_sheet = "background-color: #00FF00;";  // green
+        break;
+
+      case MRMState::MRM_OPERATING:
+        text = "MRM_OPERATING";
+        style_sheet = "background-color: #FFA500;";  // orange
+        break;
+
+      case MRMState::MRM_SUCCEEDED:
+        text = "MRM_SUCCEEDED";
+        style_sheet = "background-color: #FFFF00;";  // yellow
+        break;
+
+      case MRMState::MRM_FAILED:
+        text = "MRM_FAILED";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+
+      default:
+        text = "UNKNOWN";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+    }
+
+    updateLabel(mrm_state_label_ptr_, text, style_sheet);
+  }
+
+  // behavior
+  {
+    QString text = "";
+    QString style_sheet = "";
+    switch (msg->state) {
+      case MRMState::NONE:
+        text = "NONE";
+        style_sheet = "background-color: #00FF00;";  // green
+        break;
+
+      case MRMState::COMFORTABLE_STOP:
+        text = "COMFORTABLE_STOP";
+        style_sheet = "background-color: #FFFF00;";  // yellow
+        break;
+
+      case MRMState::EMERGENCY_STOP:
+        text = "EMERGENCY_STOP";
+        style_sheet = "background-color: #FFA500;";  // orange
+        break;
+
+      default:
+        text = "UNKNOWN";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+    }
+
+    updateLabel(mrm_behavior_label_ptr_, text, style_sheet);
+  }
+}
+
 void AutowareStatePanel::onShift(
   const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg)
 {

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -27,6 +27,7 @@
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/motion_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/route_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
@@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel
     autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
   using MotionState = autoware_adapi_v1_msgs::msg::MotionState;
   using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart;
+  using MRMState = autoware_adapi_v1_msgs::msg::MrmState;
 
   Q_OBJECT
 
@@ -77,6 +79,7 @@ public Q_SLOTS:  // NOLINT for Qt
   QGroupBox * makeRoutingGroup();
   QGroupBox * makeLocalizationGroup();
   QGroupBox * makeMotionGroup();
+  QGroupBox * makeFailSafeGroup();
 
   void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
   void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
@@ -139,6 +142,15 @@ public Q_SLOTS:  // NOLINT for Qt
 
   void onMotion(const MotionState::ConstSharedPtr msg);
 
+  // FailSafe
+  QLabel * mrm_state_label_ptr_{nullptr};
+  QLabel * mrm_behavior_label_ptr_{nullptr};
+
+  rclcpp::Subscription<MRMState>::SharedPtr sub_mrm_;
+
+  void onMRMState(const MRMState::ConstSharedPtr msg);
+
+  // Others
   QPushButton * velocity_limit_button_ptr_;
   QLabel * gear_label_ptr_;
 

common/tvm_utility/include/tvm_utility/pipeline.hpp

@@ -347,6 +347,59 @@ class InferenceEngineTVM : public InferenceEngine
   const std::array<char, 3> version_up_to{2, 1, 0};
 };
 
+template <
+  class PreProcessorType, class InferenceEngineType, class TVMScriptEngineType,
+  class PostProcessorType>
+class TowStagePipeline
+{
+  using InputType = decltype(std::declval<PreProcessorType>().input_type_indicator_);
+  using OutputType = decltype(std::declval<PostProcessorType>().output_type_indicator_);
+
+public:
+  /**
+   * @brief Construct a new Pipeline object
+   *
+   * @param pre_processor a PreProcessor object
+   * @param post_processor a PostProcessor object
+   * @param inference_engine a InferenceEngine object
+   */
+  TowStagePipeline(
+    std::shared_ptr<PreProcessorType> pre_processor,
+    std::shared_ptr<InferenceEngineType> inference_engine_1,
+    std::shared_ptr<TVMScriptEngineType> tvm_script_engine,
+    std::shared_ptr<InferenceEngineType> inference_engine_2,
+    std::shared_ptr<PostProcessorType> post_processor)
+  : pre_processor_(pre_processor),
+    inference_engine_1_(inference_engine_1),
+    tvm_script_engine_(tvm_script_engine),
+    inference_engine_2_(inference_engine_2),
+    post_processor_(post_processor)
+  {
+  }
+
+  /**
+   * @brief run the pipeline. Return asynchronously in a callback.
+   *
+   * @param input The data to push into the pipeline
+   * @return The pipeline output
+   */
+  OutputType schedule(const InputType & input)
+  {
+    auto input_tensor = pre_processor_->schedule(input);
+    auto output_infer_1 = inference_engine_1_->schedule(input_tensor);
+    auto output_tvm_script = tvm_script_engine_->schedule(output_infer_1);
+    auto output_infer_2 = inference_engine_2_->schedule(output_tvm_script);
+    return post_processor_->schedule(output_infer_2);
+  }
+
+private:
+  std::shared_ptr<PreProcessorType> pre_processor_;
+  std::shared_ptr<InferenceEngineType> inference_engine_1_;
+  std::shared_ptr<TVMScriptEngineType> tvm_script_engine_;
+  std::shared_ptr<InferenceEngineType> inference_engine_2_;
+  std::shared_ptr<PostProcessorType> post_processor_;
+};
+
 }  // namespace pipeline
 }  // namespace tvm_utility
 #endif  // TVM_UTILITY__PIPELINE_HPP_

common/tvm_utility/package.xml

@@ -24,6 +24,7 @@
     TVM as the inference engine.
   </description>
   <maintainer email="ambroise.vincent@arm.com">Ambroise Vincent</maintainer>
+  <maintainer email="xinyu.wang@tier4.jp">Xinyu Wang</maintainer>
   <license>Apache License 2.0</license>
 
   <build_depend>autoware_cmake</build_depend>

common/tvm_utility/tvm_utility-extras.cmake

@@ -50,35 +50,36 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY)
         COPYONLY
       )
     endif()
-    set(DOWNLOAD_DIR "")
     set(SOURCE_DIR "${DATA_PATH}/user/${MODEL_NAME}")
     set(INSTALL_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}")
   else()
     set(ARCHIVE_NAME "${MODEL_NAME}-${CMAKE_SYSTEM_PROCESSOR}-${MODEL_BACKEND}-${MODELZOO_VERSION}.tar.gz")
 
     # Use previously-downloaded archives if available.
-    if(EXISTS "${DATA_PATH}/downloads/${ARCHIVE_NAME}")
-      set(URL "${DATA_PATH}/downloads/${ARCHIVE_NAME}")
-    elseif(DOWNLOAD_ARTIFACTS)
+    set(DOWNLOAD_DIR "${DATA_PATH}/downloads")
+    if(DOWNLOAD_ARTIFACTS)
       message(STATUS "Downloading ${ARCHIVE_NAME} ...")
-      set(URL "https://autoware-modelzoo.s3.us-east-2.amazonaws.com/models/${MODELZOO_VERSION}/${ARCHIVE_NAME}")
+      if(NOT EXISTS "${DATA_PATH}/downloads/${ARCHIVE_NAME}")
+        set(URL "https://autoware-modelzoo.s3.us-east-2.amazonaws.com/models/${MODELZOO_VERSION}/${ARCHIVE_NAME}")
+        file(DOWNLOAD ${URL} "${DOWNLOAD_DIR}/${ARCHIVE_NAME}")
+      endif()
     else()
       message(WARNING "Skipped download for ${MODEL_NAME} (enable by setting DOWNLOAD_ARTIFACTS)")
       set(${DEPENDENCY} "" PARENT_SCOPE)
       return()
     endif()
-    set(DOWNLOAD_DIR "${DATA_PATH}/downloads")
     set(SOURCE_DIR "${DATA_PATH}/models/${MODEL_NAME}")
     set(INSTALL_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}")
+    file(ARCHIVE_EXTRACT INPUT "${DOWNLOAD_DIR}/${ARCHIVE_NAME}" DESTINATION "${SOURCE_DIR}")
+    if(EXISTS "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp")
+      set(PREPROCESSING "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp")
+    endif()
+
   endif()
-  if(EXISTS "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp")
-    set(PREPROCESSING "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp")
-  endif()
+
   include(ExternalProject)
   externalproject_add(${EXTERNALPROJECT_NAME}
-    DOWNLOAD_DIR ${DOWNLOAD_DIR}
     SOURCE_DIR ${SOURCE_DIR}
-    URL ${URL}
     CONFIGURE_COMMAND ""
     BUILD_COMMAND ""
     BUILD_BYPRODUCTS "${DATA_PATH}/models/${MODEL_NAME}/inference_engine_tvm_config.hpp"

control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp

@@ -19,13 +19,7 @@
 #include <iostream>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 
 /**
@@ -48,8 +42,5 @@ bool linearInterpolate(
 bool linearInterpolate(
   const std::vector<double> & base_index, const std::vector<double> & base_value,
   const double & return_index, double & return_value);
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp

@@ -20,13 +20,7 @@
 #include <iostream>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 /**
  * @brief 2nd-order Butterworth Filter
@@ -108,8 +102,5 @@ namespace MoveAverageFilter
  */
 bool filt_vector(const int num, std::vector<double> & u);
 }  // namespace MoveAverageFilter
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -43,13 +43,7 @@
 #include <string>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 
 struct MPCParam
@@ -444,9 +438,6 @@ class MPC
    */
   inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
 };  // class MPC
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 
 #endif  // MPC_LATERAL_CONTROLLER__MPC_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -48,13 +48,7 @@
 #include <string>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
@@ -212,9 +206,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   rcl_interfaces::msg::SetParametersResult paramCallback(
     const std::vector<rclcpp::Parameter> & parameters);
 };
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 
 #endif  // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_