Merge branch 'main' into feature/dynamic_map_loading

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_;
 

control/joy_controller/launch/joy_controller.launch.xml

@@ -28,7 +28,7 @@
     <remap from="output/vehicle_engage" to="$(var output_vehicle_engage)"/>
 
     <remap from="service/emergency_stop" to="/api/autoware/set/emergency"/>
-    <remap from="service/autoware_engage" to="/api/external/set/engage"/>
+    <remap from="service/autoware_engage" to="/api/autoware/set/engage"/>
 
     <param from="$(var config_file)"/>
     <param name="joy_type" value="$(var joy_type)"/>

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -70,6 +70,7 @@ struct Input
   lanelet::LaneletMapPtr lanelet_map{};
   LaneletRoute::ConstSharedPtr route{};
   lanelet::ConstLanelets route_lanelets{};
+  lanelet::ConstLanelets shoulder_lanelets{};
   Trajectory::ConstSharedPtr reference_trajectory{};
   Trajectory::ConstSharedPtr predicted_trajectory{};
 };

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

@@ -66,6 +66,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
   nav_msgs::msg::Odometry::ConstSharedPtr current_odom_;
   lanelet::LaneletMapPtr lanelet_map_;
+  lanelet::ConstLanelets shoulder_lanelets_;
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_;
   lanelet::routing::RoutingGraphPtr routing_graph_;
   LaneletRoute::ConstSharedPtr route_;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -123,7 +123,15 @@ Output LaneDepartureChecker::update(const Input & input)
   output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints);
   output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true);
 
-  output.candidate_lanelets = getCandidateLanelets(input.route_lanelets, output.vehicle_footprints);
+  const auto candidate_road_lanelets =
+    getCandidateLanelets(input.route_lanelets, output.vehicle_footprints);
+  const auto candidate_shoulder_lanelets =
+    getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints);
+  output.candidate_lanelets = candidate_road_lanelets;
+  output.candidate_lanelets.insert(
+    output.candidate_lanelets.end(), candidate_shoulder_lanelets.begin(),
+    candidate_shoulder_lanelets.end());
+
   output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true);
 
   output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -197,6 +197,10 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m
 {
   lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_);
+
+  // get all shoulder lanes
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
+  shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
 }
 
 void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; }
@@ -313,6 +317,7 @@ void LaneDepartureCheckerNode::onTimer()
   input_.lanelet_map = lanelet_map_;
   input_.route = route_;
   input_.route_lanelets = route_lanelets_;
+  input_.shoulder_lanelets = shoulder_lanelets_;
   input_.reference_trajectory = reference_trajectory_;
   input_.predicted_trajectory = predicted_trajectory_;
   processing_time_map["Node: setInputData"] = stop_watch.toc(true);

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_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp

@@ -22,13 +22,7 @@
 #include <iostream>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 
 /**
@@ -96,8 +90,5 @@ class MPCTrajectory
     return points;
   }
 };
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp

@@ -41,13 +41,7 @@
 #include <string>
 #include <vector>
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 namespace MPCUtils
 {
@@ -192,8 +186,5 @@ void extendTrajectoryInYawDirection(
   const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
 
 }  // namespace MPCUtils
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp

@@ -19,13 +19,7 @@
 #include "eigen3/Eigen/Dense"
 #include "eigen3/Eigen/LU"
 
-namespace autoware
-{
-namespace motion
-{
-namespace control
-{
-namespace mpc_lateral_controller
+namespace autoware::motion::control::mpc_lateral_controller
 {
 
 /// Interface for solvers of Quadratic Programming (QP) problems
@@ -54,8 +48,5 @@ class QPSolverInterface
     const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
     const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0;
 };
-}  // namespace mpc_lateral_controller
-}  // namespace control
-}  // namespace motion
-}  // namespace autoware
+}  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_