feat(command_mode_decider): optimize update function call rate (#10869)

system/autoware_command_mode_decider/include/autoware_command_mode_decider/status.hpp

@@ -42,6 +42,7 @@ struct CommandModeStatusItem
   uint16_t mode() const { return message_.mode; }
   uint16_t mrm() const { return message_.mrm; }
   bool request() const { return message_.request; }
+  bool operator!=(const CommandModeStatusItem & other) const { return message_ != other.message_; }
 
 private:
   StatusMessage message_;
@@ -70,9 +71,9 @@ class CommandModeStatusTable
 {
 public:
   void init(const std::vector<uint16_t> & modes);
-  void set(const StatusMessage & item, const rclcpp::Time & stamp);
-  void set(const AvailabilityMessage & item, const rclcpp::Time & stamp);
-  void set(uint16_t mode, bool drivable, const rclcpp::Time & stamp);
+  bool set(const StatusMessage & item, const rclcpp::Time & stamp);
+  bool set(const AvailabilityMessage & item, const rclcpp::Time & stamp);
+  bool set(uint16_t mode, bool drivable, const rclcpp::Time & stamp);
   void check_timeout(const rclcpp::Time & now);
   bool ready() const;
   bool available(uint16_t mode, bool is_manual) const;

system/autoware_command_mode_decider/src/command_mode_decider_base.cpp

@@ -73,7 +73,6 @@ CommandModeDeciderBase::CommandModeDeciderBase(const rclcpp::NodeOptions & optio
 
   curr_autoware_control_ = false;
   curr_manual_control_ = false;
-  prev_manual_control_ = false;
   curr_operation_mode_ = autoware::command_mode_types::modes::unknown;
   curr_mode_ = autoware::command_mode_types::modes::unknown;
   last_mode_ = system_request_.operation_mode;
@@ -166,41 +165,40 @@ void CommandModeDeciderBase::on_diagnostics(diagnostic_updater::DiagnosticStatus
 
 void CommandModeDeciderBase::on_timer()
 {
-  // Update transition availability other than autonomous mode.
+  // TODO(Takagi, Isamu): Remove this process when all mode flags are supported.
   // Note: The is_modes_ready_ depends on this process.
+  // Update transition availability other than autonomous mode.
   const auto stamp = now();
   for (const auto & [mode, item] : command_mode_status_) {
     if (mode != command_mode_types::modes::autonomous) {
       command_mode_status_.set(mode, true, stamp);
     }
   }
 
-  if (!is_modes_ready_) {
-    return;
-  }
-  command_mode_status_.check_timeout(stamp);
   update();
 }
 
 void CommandModeDeciderBase::on_control_mode(const ControlModeReport & msg)
 {
+  const auto is_changed = prev_control_mode_ != msg.mode;
+  prev_control_mode_ = msg.mode;
   curr_autoware_control_ = (msg.mode == ControlModeReport::AUTONOMOUS);
   curr_manual_control_ = (msg.mode == ControlModeReport::MANUAL);
 
   // Check override.
-  if (!prev_manual_control_ && curr_manual_control_) {
+  if (is_changed && curr_manual_control_) {
     if (system_request_.autoware_control) {
       // curr_mode_ and last_mode_ will be updated in the update_current_mode function.
       system_request_.autoware_control = false;
       RCLCPP_WARN_STREAM(get_logger(), "override detected");
     }
   }
-  prev_manual_control_ = curr_manual_control_;
 
   // Update command mode status for manual mode.
   {
     StatusMessage item;
     item.mode = autoware::command_mode_types::modes::manual;
+    item.mrm = StatusMessage::NORMAL;
     item.transition = false;
     item.request = curr_manual_control_;
     item.vehicle_selected = curr_manual_control_;
@@ -220,48 +218,54 @@ void CommandModeDeciderBase::on_control_mode(const ControlModeReport & msg)
     command_mode_status_.set(item, msg.stamp);
   }
 
-  // Check if all command mode status items are ready.
-  is_modes_ready_ = is_modes_ready_ ? true : command_mode_status_.ready();
-  if (!is_modes_ready_) {
+  if (!is_changed) {
     return;
   }
   update();
 }
 
 void CommandModeDeciderBase::on_status(const CommandModeStatus & msg)
 {
-  // Update command mode status.
+  bool is_changed = false;
   for (const auto & item : msg.items) {
-    command_mode_status_.set(item, msg.stamp);
+    is_changed |= command_mode_status_.set(item, msg.stamp);
+  }
+  if (!is_changed) {
+    return;
   }
-
-  // Check if all command mode status items are ready.
-  is_modes_ready_ = is_modes_ready_ ? true : command_mode_status_.ready();
-  if (!is_modes_ready_) return;
   update();
 }
 
 void CommandModeDeciderBase::on_availability(const CommandModeAvailability & msg)
 {
+  bool is_changed = false;
   for (const auto & item : msg.items) {
-    command_mode_status_.set(item, msg.stamp);
+    is_changed |= command_mode_status_.set(item, msg.stamp);
+  }
+  if (!is_changed) {
+    return;
   }
-
-  // Check if all command mode status items are ready.
-  is_modes_ready_ = is_modes_ready_ ? true : command_mode_status_.ready();
-  if (!is_modes_ready_) return;
   update();
 }
 
 void CommandModeDeciderBase::on_transition_available(const ModeChangeAvailable & msg)
 {
-  command_mode_status_.set(command_mode_types::modes::autonomous, msg.available, msg.stamp);
+  // TODO(Takagi, Isamu): Update all modes when all mode flags are supported.
+  if (command_mode_status_.set(command_mode_types::modes::autonomous, msg.available, msg.stamp)) {
+    update();
+  }
 }
 
 void CommandModeDeciderBase::update()
 {
-  // Note: is_modes_ready_ should be checked in the function that called this.
-  // TODO(Takagi, Isamu): Check call rate.
+  if (!is_modes_ready_) {
+    is_modes_ready_ = command_mode_status_.ready();
+    if (!is_modes_ready_) {
+      return;
+    }
+  }
+  command_mode_status_.check_timeout(now());
+
   detect_operation_mode_timeout();
   update_request_mode();
   update_current_mode();

system/autoware_command_mode_decider/src/command_mode_decider_base.hpp

@@ -113,7 +113,7 @@ class CommandModeDeciderBase : public rclcpp::Node
 
   bool curr_autoware_control_;
   bool curr_manual_control_;
-  bool prev_manual_control_;
+  std::optional<uint8_t> prev_control_mode_;
 
   uint16_t curr_operation_mode_;
   uint16_t curr_mode_;

system/autoware_command_mode_decider/src/plugin/status.cpp

@@ -74,31 +74,41 @@ bool CommandModeStatusTable::ready() const
   return true;
 }
 
-void CommandModeStatusTable::set(const StatusMessage & item, const rclcpp::Time & stamp)
+bool CommandModeStatusTable::set(const StatusMessage & item, const rclcpp::Time & stamp)
 {
   const auto iter = items_.find(item.mode);
-  if (iter != items_.end()) {
-    iter->second.status.data = CommandModeStatusItem(item);
-    iter->second.status.stamp = stamp;
+  if (iter == items_.end()) {
+    return false;
   }
+  const auto data = CommandModeStatusItem(item);
+  const auto is_changed = iter->second.status.data != data;
+  iter->second.status.data = data;
+  iter->second.status.stamp = stamp;
+  return is_changed;
 }
 
-void CommandModeStatusTable::set(const AvailabilityMessage & item, const rclcpp::Time & stamp)
+bool CommandModeStatusTable::set(const AvailabilityMessage & item, const rclcpp::Time & stamp)
 {
   const auto iter = items_.find(item.mode);
-  if (iter != items_.end()) {
-    iter->second.available.data = item.available;
-    iter->second.available.stamp = stamp;
+  if (iter == items_.end()) {
+    return false;
   }
+  const auto is_changed = iter->second.available.data != item.available;
+  iter->second.available.data = item.available;
+  iter->second.available.stamp = stamp;
+  return is_changed;
 }
 
-void CommandModeStatusTable::set(uint16_t mode, bool drivable, const rclcpp::Time & stamp)
+bool CommandModeStatusTable::set(uint16_t mode, bool drivable, const rclcpp::Time & stamp)
 {
   const auto iter = items_.find(mode);
-  if (iter != items_.end()) {
-    iter->second.drivable.data = drivable;
-    iter->second.drivable.stamp = stamp;
+  if (iter == items_.end()) {
+    return false;
   }
+  const auto is_changed = iter->second.drivable.data != drivable;
+  iter->second.drivable.data = drivable;
+  iter->second.drivable.stamp = stamp;
+  return is_changed;
 }
 
 void CommandModeStatusTable::check_timeout(const rclcpp::Time & now)