Rob 42 implement the autonomous mission coordinator state machine for NONE case

urc_state_machine/CMakeLists.txt

@@ -5,6 +5,7 @@ include(../cmake/default_settings.cmake)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -23,9 +24,16 @@ include_directories(
 # Library creation
 add_library(${PROJECT_NAME} SHARED
   src/nav_coordinator.cpp
+  src/waypoint_requests.cpp
+  src/gps_waypoint_conversion.cpp
+  src/follower_navigation.cpp
+  src/coordinator_state.cpp
+  src/mission_action.cpp
+  src/mission_navigation.cpp
 )
 
 set(dependencies
+  builtin_interfaces
   rclcpp
   rclcpp_action
   rclcpp_components

urc_state_machine/README.md

@@ -21,9 +21,17 @@ waypoint -> NavCoordinator -> NavigateToWaypoint -> GeneratePlan -> path followi
 
 ## State and failure behavior
 
-The coordinator publishes `IDLE`, `WAITING_FOR_SERVER`, `SENDING_GOAL`,
-`TRACKING_GOAL`, `SUCCEEDED`, `FAILED`, or `CANCELED` on
-`nav_coordinator_state`, together with its latest error classification.
+The mission action server (`mission_action_name`, default
+`execute_autonomous_mission`) currently accepts only `SEARCH_NONE`, with a finite
+waypoint in the configured map frame and a unit quaternion. It rejects requests
+while navigation is busy or the follower server is unavailable. Accepted missions
+return `SUCCESS` on arrival, `NAVIGATION_FAILED` if navigation fails, or `CANCELED`
+after the active navigation goal stops. Waypoint topics and new mission requests
+are rejected while a mission is active. Search and mission feedback are not
+implemented yet.
+
+The coordinator publishes its autonomous mission state on `nav_coordinator_state`,
+together with its latest error classification.
 
 By default, a new waypoint cancels the active follower goal before being sent.
 Missing UTM-to-map transforms, an unavailable follower action server, rejected

urc_state_machine/include/urc_state_machine/active_mission.hpp

@@ -0,0 +1,39 @@
+#ifndef URC_STATE_MACHINE__ACTIVE_MISSION_HPP_
+#define URC_STATE_MACHINE__ACTIVE_MISSION_HPP_
+
+#include <memory>
+#include <optional>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <urc_msgs/action/execute_autonomous_mission.hpp>
+#include <urc_msgs/msg/detection_bounding_box.hpp>
+
+#include "urc_state_machine/mission_state.hpp"
+
+namespace urc_state_machine
+{
+
+enum class NavigationLeg
+{
+  INITIAL_WAYPOINT,
+  ARUCO_APPROACH
+};
+
+struct ActiveMission
+{
+  using ExecuteMission = urc_msgs::action::ExecuteAutonomousMission;
+  using MissionGoalHandle = rclcpp_action::ServerGoalHandle<ExecuteMission>;
+
+  std::shared_ptr<MissionGoalHandle> goal_handle;
+  geometry_msgs::msg::PoseStamped original_waypoint;
+  SearchMode search_mode{SearchMode::NONE};
+  NavigationLeg navigation_leg{NavigationLeg::INITIAL_WAYPOINT};
+  std::optional<urc_msgs::msg::DetectionBoundingBox> bounding_box;
+  std::optional<geometry_msgs::msg::PoseStamped> aruco_pose;
+  bool cancellation_requested{false};
+};
+
+}  // namespace urc_state_machine
+
+#endif  // URC_STATE_MACHINE__ACTIVE_MISSION_HPP_

urc_state_machine/include/urc_state_machine/gps_waypoint_conversion.hpp

@@ -0,0 +1,27 @@
+#ifndef URC_STATE_MACHINE__GPS_WAYPOINT_CONVERSION_HPP_
+#define URC_STATE_MACHINE__GPS_WAYPOINT_CONVERSION_HPP_
+
+#include <string>
+
+#include <builtin_interfaces/msg/time.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <urc_msgs/msg/waypoint.hpp>
+
+namespace tf2_ros
+{
+class Buffer;
+}
+
+namespace nav_coordinator
+{
+// Converts latitude/longitude at zero altitude through UTM into the map frame.
+// Returns an identity orientation; throws std::runtime_error if the transform fails.
+geometry_msgs::msg::PoseStamped convertGpsToMapWaypoint(
+  const urc_msgs::msg::Waypoint & waypoint,
+  tf2_ros::Buffer & tf_buffer,
+  const std::string & map_frame,
+  const std::string & utm_frame,
+  const builtin_interfaces::msg::Time & stamp);
+}
+
+#endif

urc_state_machine/include/urc_state_machine/mission_state.hpp

@@ -0,0 +1,30 @@
+#ifndef URC_STATE_MACHINE__MISSION_STATE_HPP_
+#define URC_STATE_MACHINE__MISSION_STATE_HPP_
+
+namespace urc_state_machine
+{
+
+enum class SearchMode
+{
+  NONE,
+  YOLO,
+  ARUCO_1,
+  ARUCO_2
+};
+
+enum class MissionState
+{
+  IDLE,
+  NAVIGATING,
+  SEARCHING_YOLO,
+  SEARCHING_ARUCO,
+  CALCULATING_APPROACH,
+  NAVIGATING_TO_ARUCO,
+  SUCCEEDED,
+  FAILED,
+  CANCELED
+};
+
+}  // namespace urc_state_machine
+
+#endif  // URC_STATE_MACHINE__MISSION_STATE_HPP_

urc_state_machine/include/urc_state_machine/nav_coordinator.hpp

@@ -1,19 +1,20 @@
 #ifndef NAV_COORDINATOR_HPP_
 #define NAV_COORDINATOR_HPP_
 
-#include <math.h>
 #include <memory>
+#include <string>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geographic_msgs/msg/geo_point.hpp>
-#include <geodesy/utm.h>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <urc_msgs/action/execute_autonomous_mission.hpp>
 #include <urc_msgs/action/navigate_to_waypoint.hpp>
 #include <urc_msgs/msg/waypoint.hpp>
+#include "urc_state_machine/active_mission.hpp"
+#include "urc_state_machine/mission_state.hpp"
 
 namespace nav_coordinator
 {
@@ -26,17 +27,8 @@ class NavCoordinator : public rclcpp::Node
 private:
   using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint;
   using GoalHandleNavigate = rclcpp_action::ClientGoalHandle<NavigateToWaypoint>;
-
-  enum class State
-  {
-    IDLE,
-    WAITING_FOR_SERVER,
-    SENDING_GOAL,
-    TRACKING_GOAL,
-    SUCCEEDED,
-    FAILED,
-    CANCELED
-  };
+  using ExecuteMission = urc_msgs::action::ExecuteAutonomousMission;
+  using MissionGoalHandle = rclcpp_action::ServerGoalHandle<ExecuteMission>;
 
   enum class ErrorType
   {
@@ -52,22 +44,37 @@ class NavCoordinator : public rclcpp::Node
   void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
   void handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg);
   void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint);
-  geometry_msgs::msg::PoseStamped convertGpsToMapWaypoint(
-    const urc_msgs::msg::Waypoint & waypoint);
+
+  void initializeMissionActionServer();
 
   void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle);
   void handleFeedback(
     GoalHandleNavigate::SharedPtr,
     const std::shared_ptr<const NavigateToWaypoint::Feedback> feedback);
   void handleResult(const GoalHandleNavigate::WrappedResult & result);
+  rclcpp_action::GoalResponse handleMissionGoal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const ExecuteMission::Goal> goal);
+
+  rclcpp_action::CancelResponse handleMissionCancel(
+    std::shared_ptr<MissionGoalHandle> goal_handle);
+
+  void handleMissionAccepted(
+    std::shared_ptr<MissionGoalHandle> goal_handle);
+  void sendMissionNavigation();
+  void finishMissionNavigation(
+    const GoalHandleNavigate::WrappedResult & result);
+  void finishCanceledMission();
+  void failMissionNavigation(const std::string & reason);
 
-  void transitionTo(State new_state, const std::string & reason);
+  void transitionTo(urc_state_machine::MissionState new_state, const std::string & reason);
   void handleError(ErrorType error_type, const std::string & details);
   void publishState();
   std::string errorTypeToString(ErrorType error_type) const;
-  std::string stateToString(State state) const;
+  std::string stateToString(urc_state_machine::MissionState state) const;
 
-  State state_;
+  urc_state_machine::MissionState state_{urc_state_machine::MissionState::IDLE};
+  std::shared_ptr<urc_state_machine::ActiveMission> active_mission_;
   std::string follower_action_name_;
   bool cancel_on_new_waypoint_;
   std::string map_frame_id_;
@@ -79,11 +86,12 @@ class NavCoordinator : public rclcpp::Node
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr waypoint_subscriber_;
   rclcpp::Subscription<urc_msgs::msg::Waypoint>::SharedPtr gps_waypoint_subscriber_;
   rclcpp_action::Client<NavigateToWaypoint>::SharedPtr follower_client_;
+  rclcpp_action::Server<ExecuteMission>::SharedPtr mission_server_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  ErrorType last_error_;
+  ErrorType last_error_{ErrorType::NONE};
   std::string last_error_details_;
 };
 

urc_state_machine/package.xml

@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>builtin_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_components</depend>

urc_state_machine/src/coordinator_state.cpp

@@ -0,0 +1,109 @@
+#include "urc_state_machine/nav_coordinator.hpp"
+
+namespace nav_coordinator
+{
+void NavCoordinator::transitionTo(
+  urc_state_machine::MissionState new_state,
+  const std::string & reason)
+{
+  if (state_ == new_state) {
+    return;
+  }
+
+  RCLCPP_INFO(
+    get_logger(), "State transition: %s -> %s (%s)", stateToString(state_).c_str(),
+    stateToString(new_state).c_str(), reason.c_str());
+  state_ = new_state;
+  publishState();
+}
+
+void NavCoordinator::handleError(ErrorType error_type, const std::string & details)
+{
+  last_error_ = error_type;
+  last_error_details_ = details;
+
+  switch (error_type) {
+    case ErrorType::PLANNER_FAILURE:
+      RCLCPP_ERROR(get_logger(), "[PLANNER_FAILURE] %s", details.c_str());
+      break;
+    case ErrorType::OBSTACLE_DETECTED:
+      RCLCPP_WARN(get_logger(), "[OBSTACLE_DETECTED] %s", details.c_str());
+      break;
+    case ErrorType::PLANNING_FAILED_IN_FOLLOWER:
+      RCLCPP_ERROR(get_logger(), "[PLANNING_FAILED] %s", details.c_str());
+      break;
+    case ErrorType::FOLLOWER_FAILURE:
+      RCLCPP_ERROR(get_logger(), "[FOLLOWER_FAILURE] %s", details.c_str());
+      break;
+    case ErrorType::SERVER_UNAVAILABLE:
+      RCLCPP_ERROR(get_logger(), "[SERVER_UNAVAILABLE] %s", details.c_str());
+      break;
+    case ErrorType::UNKNOWN_ERROR:
+      RCLCPP_ERROR(get_logger(), "[UNKNOWN_ERROR] %s", details.c_str());
+      break;
+    default:
+      RCLCPP_ERROR(get_logger(), "[UNHANDLED_ERROR] %s", details.c_str());
+      break;
+  }
+  publishState();
+}
+
+std::string NavCoordinator::errorTypeToString(ErrorType error_type) const
+{
+  switch (error_type) {
+    case ErrorType::NONE:
+      return "NONE";
+    case ErrorType::PLANNER_FAILURE:
+      return "PLANNER_FAILURE";
+    case ErrorType::OBSTACLE_DETECTED:
+      return "OBSTACLE_DETECTED";
+    case ErrorType::PLANNING_FAILED_IN_FOLLOWER:
+      return "PLANNING_FAILED";
+    case ErrorType::FOLLOWER_FAILURE:
+      return "FOLLOWER_FAILURE";
+    case ErrorType::SERVER_UNAVAILABLE:
+      return "SERVER_UNAVAILABLE";
+    case ErrorType::UNKNOWN_ERROR:
+      return "UNKNOWN_ERROR";
+    default:
+      return "UNHANDLED";
+  }
+}
+
+std::string NavCoordinator::stateToString(urc_state_machine::MissionState state) const
+{
+  switch (state) {
+    case urc_state_machine::MissionState::IDLE:
+      return "IDLE";
+    case urc_state_machine::MissionState::NAVIGATING:
+      return "NAVIGATING";
+    case urc_state_machine::MissionState::SEARCHING_YOLO:
+      return "SEARCHING_YOLO";
+    case urc_state_machine::MissionState::SEARCHING_ARUCO:
+      return "SEARCHING_ARUCO";
+    case urc_state_machine::MissionState::CALCULATING_APPROACH:
+      return "CALCULATING_APPROACH";
+    case urc_state_machine::MissionState::NAVIGATING_TO_ARUCO:
+      return "NAVIGATING_TO_ARUCO";
+    case urc_state_machine::MissionState::SUCCEEDED:
+      return "SUCCEEDED";
+    case urc_state_machine::MissionState::FAILED:
+      return "FAILED";
+    case urc_state_machine::MissionState::CANCELED:
+      return "CANCELED";
+    default:
+      return "UNKNOWN";
+  }
+}
+
+void NavCoordinator::publishState()
+{
+  std_msgs::msg::String msg;
+  msg.data = "state=" + stateToString(state_) + " error=" + errorTypeToString(last_error_);
+  if (!last_error_details_.empty()) {
+    msg.data += " details=" + last_error_details_;
+  }
+  state_publisher_->publish(msg);
+}
+
+}