feat: wait until autonomous mode is enabled (#74)

Signed-off-by: tomoya.kimura <[email protected]>
autowarefoundation · Jul 11, 2024 · 5652129 · 5652129
1 parent d6c8374
commit 5652129
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp
@@ -97,6 +97,7 @@ void AutowareAutomaticGoalSender::onOperationMode(const OperationModeState::Cons
     state_ = State::STOPPED;
   else if (msg->mode == OperationModeState::AUTONOMOUS && state_ == State::STARTING)
     state_ = State::STARTED;
+  is_autonomous_mode_available_ = msg->is_autonomous_mode_available;
   onOperationModeUpdated(msg);
 }
 
@@ -131,7 +132,8 @@ void AutowareAutomaticGoalSender::updateAutoExecutionTimerTick()
     RCLCPP_INFO_STREAM(get_logger(), goal << ": Goal set as the next. Planning in progress...");
     if (callPlanToGoalIndex(cli_set_route_, current_goal_)) state_ = State::PLANNING;
 
-  } else if (state_ == State::PLANNED) {  // start plan to next goal
+  } else if (state_ == State::PLANNED && is_autonomous_mode_available_) {  // start plan to next
+                                                                           // goal
     RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been planned. Route starting...");
     if (callService<ChangeOperationMode>(cli_change_to_autonomous_)) state_ = State::STARTING;
 

diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp
@@ -168,6 +168,7 @@ class AutowareAutomaticGoalSender : public rclcpp::Node
   std::vector<Route> goals_list_{};
   std::map<unsigned, std::pair<std::string, unsigned>> goals_achieved_{};
   std::string goals_achieved_file_path_{};
+  bool is_autonomous_mode_available_{false};
 
 private:
   void loadParams(rclcpp::Node * node);