From c628707127f17a67ad79de5f07fc26705da1d834 Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Fri, 11 Sep 2026 16:34:43 -0400 Subject: [PATCH 1/4] refactored into new files for readability --- urc_state_machine/CMakeLists.txt | 6 + .../gps_waypoint_conversion.hpp | 27 ++ .../urc_state_machine/nav_coordinator.hpp | 10 +- urc_state_machine/package.xml | 1 + urc_state_machine/src/coordinator_state.cpp | 103 ++++++ urc_state_machine/src/follower_navigation.cpp | 108 ++++++ .../src/gps_waypoint_conversion.cpp | 57 ++++ urc_state_machine/src/nav_coordinator.cpp | 316 +----------------- urc_state_machine/src/waypoint_requests.cpp | 61 ++++ 9 files changed, 368 insertions(+), 321 deletions(-) create mode 100644 urc_state_machine/include/urc_state_machine/gps_waypoint_conversion.hpp create mode 100644 urc_state_machine/src/coordinator_state.cpp create mode 100644 urc_state_machine/src/follower_navigation.cpp create mode 100644 urc_state_machine/src/gps_waypoint_conversion.cpp create mode 100644 urc_state_machine/src/waypoint_requests.cpp diff --git a/urc_state_machine/CMakeLists.txt b/urc_state_machine/CMakeLists.txt index 7dce48ca..9c68228f 100644 --- a/urc_state_machine/CMakeLists.txt +++ b/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,14 @@ 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 ) set(dependencies + builtin_interfaces rclcpp rclcpp_action rclcpp_components diff --git a/urc_state_machine/include/urc_state_machine/gps_waypoint_conversion.hpp b/urc_state_machine/include/urc_state_machine/gps_waypoint_conversion.hpp new file mode 100644 index 00000000..30421e73 --- /dev/null +++ b/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 + +#include +#include +#include + +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 diff --git a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp index 349d1648..7644bb71 100644 --- a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp +++ b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp @@ -1,12 +1,10 @@ #ifndef NAV_COORDINATOR_HPP_ #define NAV_COORDINATOR_HPP_ -#include #include +#include #include -#include -#include #include #include #include @@ -52,8 +50,6 @@ 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 handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); void handleFeedback( @@ -67,7 +63,7 @@ class NavCoordinator : public rclcpp::Node std::string errorTypeToString(ErrorType error_type) const; std::string stateToString(State state) const; - State state_; + State state_{State::IDLE}; std::string follower_action_name_; bool cancel_on_new_waypoint_; std::string map_frame_id_; @@ -83,7 +79,7 @@ class NavCoordinator : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - ErrorType last_error_; + ErrorType last_error_{ErrorType::NONE}; std::string last_error_details_; }; diff --git a/urc_state_machine/package.xml b/urc_state_machine/package.xml index 4846dda4..73cf2c6a 100644 --- a/urc_state_machine/package.xml +++ b/urc_state_machine/package.xml @@ -8,6 +8,7 @@ MIT ament_cmake + builtin_interfaces rclcpp rclcpp_action rclcpp_components diff --git a/urc_state_machine/src/coordinator_state.cpp b/urc_state_machine/src/coordinator_state.cpp new file mode 100644 index 00000000..94be0662 --- /dev/null +++ b/urc_state_machine/src/coordinator_state.cpp @@ -0,0 +1,103 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +namespace nav_coordinator +{ +void NavCoordinator::transitionTo(State 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(State state) const +{ + switch (state) { + case State::IDLE: + return "IDLE"; + case State::WAITING_FOR_SERVER: + return "WAITING_FOR_SERVER"; + case State::SENDING_GOAL: + return "SENDING_GOAL"; + case State::TRACKING_GOAL: + return "TRACKING_GOAL"; + case State::SUCCEEDED: + return "SUCCEEDED"; + case State::FAILED: + return "FAILED"; + case State::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); +} + +} diff --git a/urc_state_machine/src/follower_navigation.cpp b/urc_state_machine/src/follower_navigation.cpp new file mode 100644 index 00000000..01967f04 --- /dev/null +++ b/urc_state_machine/src/follower_navigation.cpp @@ -0,0 +1,108 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +#include +#include + +namespace nav_coordinator +{ +void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) +{ + transitionTo(State::WAITING_FOR_SERVER, "checking follower action server"); + if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { + handleError( + ErrorType::SERVER_UNAVAILABLE, + "Follower action server '" + follower_action_name_ + "' not available."); + transitionTo(State::FAILED, "follower action server unavailable"); + return; + } + + NavigateToWaypoint::Goal goal_msg; + goal_msg.goal = waypoint; + goal_msg.has_goal = true; + goal_msg.has_path = false; + goal_msg.enforce_goal_heading = false; + + transitionTo(State::SENDING_GOAL, "forwarding waypoint to follower"); + + rclcpp_action::Client::SendGoalOptions options; + options.goal_response_callback = std::bind( + &NavCoordinator::handleGoalResponse, this, std::placeholders::_1); + options.feedback_callback = std::bind( + &NavCoordinator::handleFeedback, this, std::placeholders::_1, std::placeholders::_2); + options.result_callback = std::bind( + &NavCoordinator::handleResult, this, std::placeholders::_1); + + follower_client_->async_send_goal(goal_msg, options); +} + +void NavCoordinator::handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle) +{ + if (!goal_handle) { + handleError(ErrorType::FOLLOWER_FAILURE, "Follower action server rejected the goal."); + transitionTo(State::FAILED, "follower rejected goal"); + return; + } + + active_goal_handle_ = goal_handle; + transitionTo(State::TRACKING_GOAL, "follower accepted goal"); +} + +void NavCoordinator::handleFeedback( + GoalHandleNavigate::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_DEBUG( + get_logger(), "Feedback: dist=%.2f planning=%s replans=%u", + feedback->distance_to_goal, + feedback->is_planning ? "true" : "false", + feedback->replan_count); +} + +void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & result) +{ + active_goal_handle_.reset(); + + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + if (result.result->error_code == NavigateToWaypoint::Result::SUCCESS) { + transitionTo(State::SUCCEEDED, "follower reported success"); + return; + } + + switch (result.result->error_code) { + case NavigateToWaypoint::Result::OBSTACLE_DETECTED: + handleError(ErrorType::OBSTACLE_DETECTED, "Obstacle detected during trajectory following."); + break; + case NavigateToWaypoint::Result::PLANNING_FAILED: + handleError(ErrorType::PLANNING_FAILED_IN_FOLLOWER, "Path planning failed in follower."); + break; + case NavigateToWaypoint::Result::FAILURE: + handleError(ErrorType::FOLLOWER_FAILURE, "Follower reported generic failure."); + break; + default: + handleError( + ErrorType::UNKNOWN_ERROR, + "Follower finished with error_code=" + std::to_string(result.result->error_code)); + break; + } + transitionTo(State::FAILED, "follower finished with error"); + return; + } + + if (result.code == rclcpp_action::ResultCode::ABORTED) { + handleError(ErrorType::FOLLOWER_FAILURE, "Follower aborted goal."); + transitionTo(State::FAILED, "follower aborted goal"); + return; + } + + if (result.code == rclcpp_action::ResultCode::CANCELED) { + transitionTo(State::CANCELED, "follower canceled goal"); + return; + } + + handleError( + ErrorType::UNKNOWN_ERROR, + "Unknown follower result code: " + std::to_string(static_cast(result.code))); + transitionTo(State::FAILED, "unknown follower result code"); +} + +} diff --git a/urc_state_machine/src/gps_waypoint_conversion.cpp b/urc_state_machine/src/gps_waypoint_conversion.cpp new file mode 100644 index 00000000..c430aa72 --- /dev/null +++ b/urc_state_machine/src/gps_waypoint_conversion.cpp @@ -0,0 +1,57 @@ +#include "urc_state_machine/gps_waypoint_conversion.hpp" + +// Humble's geodesy headers require global math declarations before inclusion. +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nav_coordinator +{ +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) +{ + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = waypoint.latitude; + geo_point.longitude = waypoint.longitude; + geo_point.altitude = 0.0; + + geodesy::UTMPoint waypoint_utm; + geodesy::fromMsg(geo_point, waypoint_utm); + + geometry_msgs::msg::PointStamped utm_point; + utm_point.header.stamp = stamp; + utm_point.header.frame_id = utm_frame; + utm_point.point.x = waypoint_utm.easting; + utm_point.point.y = waypoint_utm.northing; + utm_point.point.z = 0.0; + + geometry_msgs::msg::PointStamped map_point; + try { + tf_buffer.transform(utm_point, map_point, map_frame); + } catch (const tf2::TransformException & ex) { + throw std::runtime_error( + "Failed to transform waypoint from '" + utm_frame + "' to '" + map_frame + "': " + + ex.what()); + } + + geometry_msgs::msg::PoseStamped pose; + pose.header = map_point.header; + pose.pose.position.x = map_point.point.x; + pose.pose.position.y = map_point.point.y; + pose.pose.position.z = map_point.point.z; + pose.pose.orientation.w = 1.0; + + return pose; +} + +} diff --git a/urc_state_machine/src/nav_coordinator.cpp b/urc_state_machine/src/nav_coordinator.cpp index 338272dd..f0724d77 100644 --- a/urc_state_machine/src/nav_coordinator.cpp +++ b/urc_state_machine/src/nav_coordinator.cpp @@ -1,9 +1,7 @@ #include "urc_state_machine/nav_coordinator.hpp" -#include -#include -#include -#include +#include +#include namespace nav_coordinator { @@ -21,8 +19,6 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) cancel_on_new_waypoint_ = get_parameter("cancel_on_new_waypoint").as_bool(); map_frame_id_ = get_parameter("map_frame_id").as_string(); utm_frame_id_ = get_parameter("utm_frame_id").as_string(); - state_ = State::IDLE; - last_error_ = ErrorType::NONE; tf_buffer_ = std::make_shared(get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -52,314 +48,6 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) } } -void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - active_waypoint_ = *msg; - RCLCPP_INFO( - get_logger(), "Received waypoint: frame=%s x=%.3f y=%.3f", - active_waypoint_.header.frame_id.c_str(), - active_waypoint_.pose.position.x, - active_waypoint_.pose.position.y); - - if (active_goal_handle_ && cancel_on_new_waypoint_) { - transitionTo(State::CANCELED, "canceling current goal due to new waypoint"); - follower_client_->async_cancel_goal(active_goal_handle_); - active_goal_handle_.reset(); - } - - sendFollowerGoal(active_waypoint_); -} - -void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) -{ - geometry_msgs::msg::PoseStamped converted_waypoint; - try { - converted_waypoint = convertGpsToMapWaypoint(*msg); - } catch (const std::exception & ex) { - handleError( - ErrorType::PLANNER_FAILURE, - std::string("Cannot process GPS waypoint: ") + ex.what()); - transitionTo(State::FAILED, "gps waypoint rejected - transform unavailable"); - return; - } - - active_waypoint_ = converted_waypoint; - - RCLCPP_INFO( - get_logger(), - "Received GPS waypoint: lat=%.8f lon=%.8f -> frame=%s x=%.3f y=%.3f", - msg->latitude, - msg->longitude, - active_waypoint_.header.frame_id.c_str(), - active_waypoint_.pose.position.x, - active_waypoint_.pose.position.y); - - if (active_goal_handle_ && cancel_on_new_waypoint_) { - transitionTo(State::CANCELED, "canceling current goal due to new GPS waypoint"); - follower_client_->async_cancel_goal(active_goal_handle_); - active_goal_handle_.reset(); - } - - sendFollowerGoal(active_waypoint_); -} - -geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( - const urc_msgs::msg::Waypoint & waypoint) -{ - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = waypoint.latitude; - geo_point.longitude = waypoint.longitude; - geo_point.altitude = 0.0; - - geodesy::UTMPoint waypoint_utm; - geodesy::fromMsg(geo_point, waypoint_utm); - - geometry_msgs::msg::PointStamped utm_point; - utm_point.header.stamp = now(); - utm_point.header.frame_id = utm_frame_id_; - utm_point.point.x = waypoint_utm.easting; - utm_point.point.y = waypoint_utm.northing; - utm_point.point.z = 0.0; - - geometry_msgs::msg::PointStamped map_point; - try { - tf_buffer_->transform(utm_point, map_point, map_frame_id_); - } catch (const tf2::TransformException & ex) { - throw std::runtime_error( - "Failed to transform waypoint from '" + utm_frame_id_ + "' to '" + map_frame_id_ + "': " + - ex.what()); - } - - geometry_msgs::msg::PoseStamped pose; - pose.header = map_point.header; - pose.pose.position.x = map_point.point.x; - pose.pose.position.y = map_point.point.y; - pose.pose.position.z = map_point.point.z; - pose.pose.orientation.w = 1.0; - - return pose; -} - -void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) -{ - transitionTo(State::WAITING_FOR_SERVER, "checking follower action server"); - if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { - handleError( - ErrorType::SERVER_UNAVAILABLE, - "Follower action server '" + follower_action_name_ + "' not available."); - transitionTo(State::FAILED, "follower action server unavailable"); - return; - } - - NavigateToWaypoint::Goal goal_msg; - goal_msg.goal = waypoint; - goal_msg.has_goal = true; - goal_msg.has_path = false; - goal_msg.enforce_goal_heading = false; - - transitionTo(State::SENDING_GOAL, "forwarding waypoint to follower"); - - rclcpp_action::Client::SendGoalOptions options; - options.goal_response_callback = std::bind( - &NavCoordinator::handleGoalResponse, this, std::placeholders::_1); - options.feedback_callback = std::bind( - &NavCoordinator::handleFeedback, this, std::placeholders::_1, std::placeholders::_2); - options.result_callback = std::bind( - &NavCoordinator::handleResult, this, std::placeholders::_1); - - follower_client_->async_send_goal(goal_msg, options); -} - -void NavCoordinator::handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle) -{ - if (!goal_handle) { - handleError(ErrorType::FOLLOWER_FAILURE, "Follower action server rejected the goal."); - transitionTo(State::FAILED, "follower rejected goal"); - return; - } - - active_goal_handle_ = goal_handle; - transitionTo(State::TRACKING_GOAL, "follower accepted goal"); } -void NavCoordinator::handleFeedback( - GoalHandleNavigate::SharedPtr, - const std::shared_ptr feedback) -{ - RCLCPP_DEBUG( - get_logger(), "Feedback: dist=%.2f planning=%s replans=%u", - feedback->distance_to_goal, - feedback->is_planning ? "true" : "false", - feedback->replan_count); -} - -void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & result) -{ - active_goal_handle_.reset(); - - if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { - if (result.result->error_code == NavigateToWaypoint::Result::SUCCESS) { - transitionTo(State::SUCCEEDED, "follower reported success"); - return; - } - - switch (result.result->error_code) { - case NavigateToWaypoint::Result::OBSTACLE_DETECTED: - handleError(ErrorType::OBSTACLE_DETECTED, "Obstacle detected during trajectory following."); - break; - case NavigateToWaypoint::Result::PLANNING_FAILED: - handleError(ErrorType::PLANNING_FAILED_IN_FOLLOWER, "Path planning failed in follower."); - break; - case NavigateToWaypoint::Result::FAILURE: - handleError(ErrorType::FOLLOWER_FAILURE, "Follower reported generic failure."); - break; - default: - handleError( - ErrorType::UNKNOWN_ERROR, - "Follower finished with error_code=" + std::to_string(result.result->error_code)); - break; - } - transitionTo(State::FAILED, "follower finished with error"); - return; - } - - if (result.code == rclcpp_action::ResultCode::ABORTED) { - handleError(ErrorType::FOLLOWER_FAILURE, "Follower aborted goal."); - transitionTo(State::FAILED, "follower aborted goal"); - return; - } - - if (result.code == rclcpp_action::ResultCode::CANCELED) { - transitionTo(State::CANCELED, "follower canceled goal"); - return; - } - - handleError( - ErrorType::UNKNOWN_ERROR, - "Unknown follower result code: " + std::to_string(static_cast(result.code))); - transitionTo(State::FAILED, "unknown follower result code"); -} - -void NavCoordinator::transitionTo(State new_state, const std::string & reason) -{ - if (state_ == new_state) { - return; - } - - const auto state_name = [](State state) -> const char * { - switch (state) { - case State::IDLE: - return "IDLE"; - case State::WAITING_FOR_SERVER: - return "WAITING_FOR_SERVER"; - case State::SENDING_GOAL: - return "SENDING_GOAL"; - case State::TRACKING_GOAL: - return "TRACKING_GOAL"; - case State::SUCCEEDED: - return "SUCCEEDED"; - case State::FAILED: - return "FAILED"; - case State::CANCELED: - return "CANCELED"; - default: - return "UNKNOWN"; - } - }; - - RCLCPP_INFO( - get_logger(), "State transition: %s -> %s (%s)", state_name(state_), - state_name(new_state), 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(State state) const -{ - switch (state) { - case State::IDLE: - return "IDLE"; - case State::WAITING_FOR_SERVER: - return "WAITING_FOR_SERVER"; - case State::SENDING_GOAL: - return "SENDING_GOAL"; - case State::TRACKING_GOAL: - return "TRACKING_GOAL"; - case State::SUCCEEDED: - return "SUCCEEDED"; - case State::FAILED: - return "FAILED"; - case State::CANCELED: - return "CANCELED"; - default: - return "UNKNOWN"; - } -} - -void NavCoordinator::publishState() -{ - auto msg = std::make_shared(); - msg->data = "state=" + stateToString(state_) + " error=" + errorTypeToString(last_error_); - if (!last_error_details_.empty()) { - msg->data += " details=" + last_error_details_; - } - state_publisher_->publish(*msg); -} - -} // namespace nav_coordinator - -#include RCLCPP_COMPONENTS_REGISTER_NODE(nav_coordinator::NavCoordinator) diff --git a/urc_state_machine/src/waypoint_requests.cpp b/urc_state_machine/src/waypoint_requests.cpp new file mode 100644 index 00000000..eea2f286 --- /dev/null +++ b/urc_state_machine/src/waypoint_requests.cpp @@ -0,0 +1,61 @@ +#include "urc_state_machine/nav_coordinator.hpp" +#include "urc_state_machine/gps_waypoint_conversion.hpp" + +#include + +namespace nav_coordinator +{ +void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + active_waypoint_ = *msg; + RCLCPP_INFO( + get_logger(), "Received waypoint: frame=%s x=%.3f y=%.3f", + active_waypoint_.header.frame_id.c_str(), + active_waypoint_.pose.position.x, + active_waypoint_.pose.position.y); + + if (active_goal_handle_ && cancel_on_new_waypoint_) { + transitionTo(State::CANCELED, "canceling current goal due to new waypoint"); + follower_client_->async_cancel_goal(active_goal_handle_); + active_goal_handle_.reset(); + } + + sendFollowerGoal(active_waypoint_); +} + +void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) +{ + geometry_msgs::msg::PoseStamped converted_waypoint; + try { + converted_waypoint = convertGpsToMapWaypoint( + *msg, *tf_buffer_, map_frame_id_, utm_frame_id_, + now()); + } catch (const std::exception & ex) { + handleError( + ErrorType::PLANNER_FAILURE, + std::string("Cannot process GPS waypoint: ") + ex.what()); + transitionTo(State::FAILED, "gps waypoint rejected - transform unavailable"); + return; + } + + active_waypoint_ = converted_waypoint; + + RCLCPP_INFO( + get_logger(), + "Received GPS waypoint: lat=%.8f lon=%.8f -> frame=%s x=%.3f y=%.3f", + msg->latitude, + msg->longitude, + active_waypoint_.header.frame_id.c_str(), + active_waypoint_.pose.position.x, + active_waypoint_.pose.position.y); + + if (active_goal_handle_ && cancel_on_new_waypoint_) { + transitionTo(State::CANCELED, "canceling current goal due to new GPS waypoint"); + follower_client_->async_cancel_goal(active_goal_handle_); + active_goal_handle_.reset(); + } + + sendFollowerGoal(active_waypoint_); +} + +} From 79657136fc07a5fa2894e596aa772e8160a3f325 Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Fri, 11 Sep 2026 16:58:46 -0400 Subject: [PATCH 2/4] in progress --- urc_state_machine/CMakeLists.txt | 3 + urc_state_machine/README.md | 9 ++ .../mission_state_machine.hpp | 71 ++++++++++++++ .../urc_state_machine/nav_coordinator.hpp | 24 +++++ urc_state_machine/src/mission_action.cpp | 96 +++++++++++++++++++ urc_state_machine/src/mission_navigation.cpp | 92 ++++++++++++++++++ .../src/mission_state_machine.cpp | 83 ++++++++++++++++ urc_state_machine/src/nav_coordinator.cpp | 2 + urc_state_machine/src/waypoint_requests.cpp | 8 ++ 9 files changed, 388 insertions(+) create mode 100644 urc_state_machine/include/urc_state_machine/mission_state_machine.hpp create mode 100644 urc_state_machine/src/mission_action.cpp create mode 100644 urc_state_machine/src/mission_navigation.cpp create mode 100644 urc_state_machine/src/mission_state_machine.cpp diff --git a/urc_state_machine/CMakeLists.txt b/urc_state_machine/CMakeLists.txt index 9c68228f..6f512287 100644 --- a/urc_state_machine/CMakeLists.txt +++ b/urc_state_machine/CMakeLists.txt @@ -28,6 +28,9 @@ add_library(${PROJECT_NAME} SHARED src/gps_waypoint_conversion.cpp src/follower_navigation.cpp src/coordinator_state.cpp + src/mission_state_machine.cpp + src/mission_action.cpp + src/mission_navigation.cpp ) set(dependencies diff --git a/urc_state_machine/README.md b/urc_state_machine/README.md index f4b546e6..84347503 100644 --- a/urc_state_machine/README.md +++ b/urc_state_machine/README.md @@ -21,6 +21,15 @@ waypoint -> NavCoordinator -> NavigateToWaypoint -> GeneratePlan -> path followi ## State and failure behavior +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 or `NAVIGATION_FAILED` if navigation fails. Waypoint +topics are ignored during a mission. Search, mission feedback, cancellation, and +mission replacement are not implemented yet. Building requires ROB-41's +`urc_msgs` interfaces. + 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. diff --git a/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp b/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp new file mode 100644 index 00000000..35dfec95 --- /dev/null +++ b/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp @@ -0,0 +1,71 @@ +#pragma once + +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 +}; + +enum class MissionEvent +{ + NAVIGATION_SUCCEEDED, + TARGET_DETECTED, + APPROACH_READY, + OPERATION_FAILED, + CANCEL_REQUESTED, + ACTIVE_OPERATION_STOPPED +}; + +enum class MissionCommand +{ + NONE, + NAVIGATE_TO_WAYPOINT, + START_YOLO_SEARCH, + START_ARUCO_SEARCH, + STOP_SEARCH, + CALCULATE_APPROACH, + NAVIGATE_TO_ARUCO, + CANCEL_ACTIVE_OPERATION, + COMPLETE_SUCCESS, + COMPLETE_FAILURE, + COMPLETE_CANCELED +}; + +struct Transition +{ + bool accepted; + MissionState state; + MissionCommand command; +}; + +class MissionStateMachine +{ +public: + Transition start(SearchMode mode); + Transition handle(MissionEvent event); + MissionState state() const; + +private: + Transition transitionTo(MissionState state, MissionCommand command); + + MissionState state_{MissionState::IDLE}; + SearchMode search_mode_{SearchMode::NONE}; +}; +} diff --git a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp index 7644bb71..68d6ad31 100644 --- a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp +++ b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp @@ -10,8 +10,10 @@ #include #include #include +#include #include #include +#include "urc_state_machine/mission_state_machine.hpp" namespace nav_coordinator { @@ -24,6 +26,8 @@ class NavCoordinator : public rclcpp::Node private: using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint; using GoalHandleNavigate = rclcpp_action::ClientGoalHandle; + using ExecuteMission = urc_msgs::action::ExecuteAutonomousMission; + using MissionGoalHandle = rclcpp_action::ServerGoalHandle; enum class State { @@ -51,11 +55,26 @@ class NavCoordinator : public rclcpp::Node void handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg); void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); + void initializeMissionActionServer(); + void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); void handleFeedback( GoalHandleNavigate::SharedPtr, const std::shared_ptr feedback); void handleResult(const GoalHandleNavigate::WrappedResult & result); + rclcpp_action::GoalResponse handleMissionGoal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handleMissionCancel( + std::shared_ptr goal_handle); + + void handleMissionAccepted( + std::shared_ptr goal_handle); + void sendMissionNavigation(); + void finishMissionNavigation( + const GoalHandleNavigate::WrappedResult & result); + void failMissionNavigation(const std::string & reason); void transitionTo(State new_state, const std::string & reason); void handleError(ErrorType error_type, const std::string & details); @@ -64,6 +83,10 @@ class NavCoordinator : public rclcpp::Node std::string stateToString(State state) const; State state_{State::IDLE}; + urc_state_machine::MissionStateMachine state_machine_; + // Mission and waypoint callbacks use the default mutually exclusive callback group. + bool mission_reserved_{false}; + std::shared_ptr active_mission_handle_; std::string follower_action_name_; bool cancel_on_new_waypoint_; std::string map_frame_id_; @@ -75,6 +98,7 @@ class NavCoordinator : public rclcpp::Node rclcpp::Subscription::SharedPtr waypoint_subscriber_; rclcpp::Subscription::SharedPtr gps_waypoint_subscriber_; rclcpp_action::Client::SharedPtr follower_client_; + rclcpp_action::Server::SharedPtr mission_server_; rclcpp::Publisher::SharedPtr state_publisher_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/urc_state_machine/src/mission_action.cpp b/urc_state_machine/src/mission_action.cpp new file mode 100644 index 00000000..3bc3e6aa --- /dev/null +++ b/urc_state_machine/src/mission_action.cpp @@ -0,0 +1,96 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +#include +#include +#include + +namespace nav_coordinator +{ +namespace +{ +bool hasValidPose(const geometry_msgs::msg::Pose & pose) +{ + const auto & position = pose.position; + const auto & orientation = pose.orientation; + const double orientation_norm = + orientation.x * orientation.x + orientation.y * orientation.y + + orientation.z * orientation.z + orientation.w * orientation.w; + constexpr double quaternion_norm_tolerance = 1e-3; + + return std::isfinite(position.x) && std::isfinite(position.y) && + std::isfinite(position.z) && std::isfinite(orientation_norm) && + std::abs(orientation_norm - 1.0) <= quaternion_norm_tolerance; +} +} + +void NavCoordinator::initializeMissionActionServer() +{ + const auto action_name = declare_parameter( + "mission_action_name", "execute_autonomous_mission"); + + if (action_name.empty()) { + throw std::invalid_argument("mission_action_name must not be empty"); + } + + mission_server_ = rclcpp_action::create_server( + this, + action_name, + std::bind( + &NavCoordinator::handleMissionGoal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind( + &NavCoordinator::handleMissionCancel, this, + std::placeholders::_1), + std::bind( + &NavCoordinator::handleMissionAccepted, this, + std::placeholders::_1)); +} + +rclcpp_action::GoalResponse NavCoordinator::handleMissionGoal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) +{ + if (!goal || goal->search_mode != ExecuteMission::Goal::SEARCH_NONE || + goal->waypoint.header.frame_id != map_frame_id_ || !hasValidPose(goal->waypoint.pose)) + { + return rclcpp_action::GoalResponse::REJECT; + } + + if (mission_reserved_ || active_goal_handle_ || + state_ == State::WAITING_FOR_SERVER || state_ == State::SENDING_GOAL || + state_ == State::TRACKING_GOAL || !follower_client_->action_server_is_ready()) + { + return rclcpp_action::GoalResponse::REJECT; + } + + mission_reserved_ = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse NavCoordinator::handleMissionCancel( + std::shared_ptr) +{ + // TODO: Accept cancellation once active-operation stopping is implemented. + return rclcpp_action::CancelResponse::REJECT; +} + +void NavCoordinator::handleMissionAccepted( + std::shared_ptr goal_handle) +{ + active_mission_handle_ = goal_handle; + active_waypoint_ = goal_handle->get_goal()->waypoint; + last_error_ = ErrorType::NONE; + last_error_details_.clear(); + + const auto transition = state_machine_.start(urc_state_machine::SearchMode::NONE); + if (!transition.accepted || + transition.command != urc_state_machine::MissionCommand::NAVIGATE_TO_WAYPOINT) + { + failMissionNavigation("Mission state machine could not start navigation."); + return; + } + + sendMissionNavigation(); +} + +} diff --git a/urc_state_machine/src/mission_navigation.cpp b/urc_state_machine/src/mission_navigation.cpp new file mode 100644 index 00000000..7009d2a6 --- /dev/null +++ b/urc_state_machine/src/mission_navigation.cpp @@ -0,0 +1,92 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +#include + +namespace nav_coordinator +{ + +void NavCoordinator::sendMissionNavigation() +{ + if (!follower_client_->action_server_is_ready()) { + failMissionNavigation("Follower action server is unavailable."); + return; + } + + NavigateToWaypoint::Goal goal; + goal.goal = active_waypoint_; + goal.has_goal = true; + goal.has_path = false; + goal.enforce_goal_heading = false; + + rclcpp_action::Client::SendGoalOptions options; + options.goal_response_callback = [this](const GoalHandleNavigate::SharedPtr & handle) { + if (!handle) { + failMissionNavigation("Follower rejected the navigation goal."); + return; + } + active_goal_handle_ = handle; + transitionTo(State::TRACKING_GOAL, "follower accepted mission navigation"); + }; + options.result_callback = [this](const GoalHandleNavigate::WrappedResult & result) { + finishMissionNavigation(result); + }; + + transitionTo(State::SENDING_GOAL, "sending mission navigation goal"); + try { + follower_client_->async_send_goal(goal, options); + } catch (const rclcpp::exceptions::RCLError & error) { + failMissionNavigation(error.what()); + } +} + +void NavCoordinator::finishMissionNavigation(const GoalHandleNavigate::WrappedResult & result) +{ + if (!active_mission_handle_) { + return; + } + + active_goal_handle_.reset(); + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || !result.result || + result.result->error_code != NavigateToWaypoint::Result::SUCCESS) + { + failMissionNavigation("Follower navigation did not succeed."); + return; + } + + const auto transition = state_machine_.handle( + urc_state_machine::MissionEvent::NAVIGATION_SUCCEEDED); + if (!transition.accepted || + transition.command != urc_state_machine::MissionCommand::COMPLETE_SUCCESS) + { + failMissionNavigation("Unexpected mission transition after navigation."); + return; + } + + auto mission_result = std::make_shared(); + mission_result->error_code = ExecuteMission::Result::SUCCESS; + mission_result->message = "Reached the mission waypoint."; + active_mission_handle_->succeed(mission_result); + active_mission_handle_.reset(); + mission_reserved_ = false; + transitionTo(State::SUCCEEDED, "mission navigation completed"); +} + +void NavCoordinator::failMissionNavigation(const std::string & reason) +{ + if (!active_mission_handle_) { + return; + } + + state_machine_.handle(urc_state_machine::MissionEvent::OPERATION_FAILED); + auto result = std::make_shared(); + result->error_code = ExecuteMission::Result::NAVIGATION_FAILED; + result->message = reason; + active_mission_handle_->abort(result); + active_mission_handle_.reset(); + active_goal_handle_.reset(); + mission_reserved_ = false; + handleError(ErrorType::FOLLOWER_FAILURE, reason); + transitionTo(State::FAILED, reason); +} + +} diff --git a/urc_state_machine/src/mission_state_machine.cpp b/urc_state_machine/src/mission_state_machine.cpp new file mode 100644 index 00000000..af1cdb66 --- /dev/null +++ b/urc_state_machine/src/mission_state_machine.cpp @@ -0,0 +1,83 @@ +#include "urc_state_machine/mission_state_machine.hpp" + +namespace urc_state_machine +{ + +MissionState MissionStateMachine::state() const +{ + return state_; +} + +Transition MissionStateMachine::transitionTo( + MissionState next_state, + MissionCommand command) +{ + state_ = next_state; + return {true, state_, command}; +} + +Transition MissionStateMachine::start(SearchMode mode) +{ + const bool can_start = + state_ == MissionState::IDLE || + state_ == MissionState::SUCCEEDED || + state_ == MissionState::FAILED || + state_ == MissionState::CANCELED; + + if (!can_start) { + return {false, state_, MissionCommand::NONE}; + } + + switch (mode) { + case SearchMode::NONE: + case SearchMode::YOLO: + case SearchMode::ARUCO_1: + case SearchMode::ARUCO_2: + break; + default: + return {false, state_, MissionCommand::NONE}; + } + + search_mode_ = mode; + + return transitionTo( + MissionState::NAVIGATING, + MissionCommand::NAVIGATE_TO_WAYPOINT); +} + +Transition MissionStateMachine::handle(MissionEvent event) +{ + if (state_ != MissionState::NAVIGATING) { + return {false, state_, MissionCommand::NONE}; + } + + if (event == MissionEvent::OPERATION_FAILED) { + return transitionTo(MissionState::FAILED, MissionCommand::COMPLETE_FAILURE); + } + + if (event != MissionEvent::NAVIGATION_SUCCEEDED) { + return {false, state_, MissionCommand::NONE}; + } + + switch (search_mode_) { + case SearchMode::NONE: + return transitionTo( + MissionState::SUCCEEDED, + MissionCommand::COMPLETE_SUCCESS); + + case SearchMode::YOLO: + return transitionTo( + MissionState::SEARCHING_YOLO, + MissionCommand::START_YOLO_SEARCH); + + case SearchMode::ARUCO_1: + case SearchMode::ARUCO_2: + return transitionTo( + MissionState::SEARCHING_ARUCO, + MissionCommand::START_ARUCO_SEARCH); + } + + return {false, state_, MissionCommand::NONE}; +} + +} // namespace urc_state_machine diff --git a/urc_state_machine/src/nav_coordinator.cpp b/urc_state_machine/src/nav_coordinator.cpp index f0724d77..afeaca16 100644 --- a/urc_state_machine/src/nav_coordinator.cpp +++ b/urc_state_machine/src/nav_coordinator.cpp @@ -36,6 +36,8 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) rclcpp::SystemDefaultsQoS(), std::bind(&NavCoordinator::handleGpsWaypoint, this, std::placeholders::_1)); + initializeMissionActionServer(); + RCLCPP_INFO( get_logger(), "Nav Coordinator ready. Pose waypoints on '%s', GPS waypoints on '%s', forwarding to action '%s'.", diff --git a/urc_state_machine/src/waypoint_requests.cpp b/urc_state_machine/src/waypoint_requests.cpp index eea2f286..0d194279 100644 --- a/urc_state_machine/src/waypoint_requests.cpp +++ b/urc_state_machine/src/waypoint_requests.cpp @@ -7,6 +7,10 @@ namespace nav_coordinator { void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + if (mission_reserved_) { + return; + } + active_waypoint_ = *msg; RCLCPP_INFO( get_logger(), "Received waypoint: frame=%s x=%.3f y=%.3f", @@ -25,6 +29,10 @@ void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::Share void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) { + if (mission_reserved_) { + return; + } + geometry_msgs::msg::PoseStamped converted_waypoint; try { converted_waypoint = convertGpsToMapWaypoint( From ffcb78adf5d16a35377dd7623c6c60977cf6905d Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Sun, 13 Sep 2026 16:47:44 -0400 Subject: [PATCH 3/4] in progress --- urc_state_machine/CMakeLists.txt | 1 - urc_state_machine/README.md | 15 ++-- .../urc_state_machine/active_mission.hpp | 39 +++++++++ .../urc_state_machine/mission_state.hpp | 30 +++++++ .../mission_state_machine.hpp | 71 ---------------- .../urc_state_machine/nav_coordinator.hpp | 26 ++---- urc_state_machine/src/coordinator_state.cpp | 30 ++++--- urc_state_machine/src/follower_navigation.cpp | 18 ++-- urc_state_machine/src/mission_action.cpp | 34 ++++---- urc_state_machine/src/mission_navigation.cpp | 76 +++++++++++------ .../src/mission_state_machine.cpp | 83 ------------------- urc_state_machine/src/waypoint_requests.cpp | 16 ++-- 12 files changed, 188 insertions(+), 251 deletions(-) create mode 100644 urc_state_machine/include/urc_state_machine/active_mission.hpp create mode 100644 urc_state_machine/include/urc_state_machine/mission_state.hpp delete mode 100644 urc_state_machine/include/urc_state_machine/mission_state_machine.hpp delete mode 100644 urc_state_machine/src/mission_state_machine.cpp diff --git a/urc_state_machine/CMakeLists.txt b/urc_state_machine/CMakeLists.txt index 6f512287..74ce1b58 100644 --- a/urc_state_machine/CMakeLists.txt +++ b/urc_state_machine/CMakeLists.txt @@ -28,7 +28,6 @@ add_library(${PROJECT_NAME} SHARED src/gps_waypoint_conversion.cpp src/follower_navigation.cpp src/coordinator_state.cpp - src/mission_state_machine.cpp src/mission_action.cpp src/mission_navigation.cpp ) diff --git a/urc_state_machine/README.md b/urc_state_machine/README.md index 84347503..a77d6b57 100644 --- a/urc_state_machine/README.md +++ b/urc_state_machine/README.md @@ -25,14 +25,13 @@ 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 or `NAVIGATION_FAILED` if navigation fails. Waypoint -topics are ignored during a mission. Search, mission feedback, cancellation, and -mission replacement are not implemented yet. Building requires ROB-41's -`urc_msgs` interfaces. - -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. +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 diff --git a/urc_state_machine/include/urc_state_machine/active_mission.hpp b/urc_state_machine/include/urc_state_machine/active_mission.hpp new file mode 100644 index 00000000..97e6990a --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +#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; + + std::shared_ptr goal_handle; + geometry_msgs::msg::PoseStamped original_waypoint; + SearchMode search_mode{SearchMode::NONE}; + NavigationLeg navigation_leg{NavigationLeg::INITIAL_WAYPOINT}; + std::optional bounding_box; + std::optional aruco_pose; + bool cancellation_requested{false}; +}; + +} // namespace urc_state_machine + +#endif // URC_STATE_MACHINE__ACTIVE_MISSION_HPP_ diff --git a/urc_state_machine/include/urc_state_machine/mission_state.hpp b/urc_state_machine/include/urc_state_machine/mission_state.hpp new file mode 100644 index 00000000..b0265b4b --- /dev/null +++ b/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_ diff --git a/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp b/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp deleted file mode 100644 index 35dfec95..00000000 --- a/urc_state_machine/include/urc_state_machine/mission_state_machine.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -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 -}; - -enum class MissionEvent -{ - NAVIGATION_SUCCEEDED, - TARGET_DETECTED, - APPROACH_READY, - OPERATION_FAILED, - CANCEL_REQUESTED, - ACTIVE_OPERATION_STOPPED -}; - -enum class MissionCommand -{ - NONE, - NAVIGATE_TO_WAYPOINT, - START_YOLO_SEARCH, - START_ARUCO_SEARCH, - STOP_SEARCH, - CALCULATE_APPROACH, - NAVIGATE_TO_ARUCO, - CANCEL_ACTIVE_OPERATION, - COMPLETE_SUCCESS, - COMPLETE_FAILURE, - COMPLETE_CANCELED -}; - -struct Transition -{ - bool accepted; - MissionState state; - MissionCommand command; -}; - -class MissionStateMachine -{ -public: - Transition start(SearchMode mode); - Transition handle(MissionEvent event); - MissionState state() const; - -private: - Transition transitionTo(MissionState state, MissionCommand command); - - MissionState state_{MissionState::IDLE}; - SearchMode search_mode_{SearchMode::NONE}; -}; -} diff --git a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp index 68d6ad31..61c5db96 100644 --- a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp +++ b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp @@ -13,7 +13,8 @@ #include #include #include -#include "urc_state_machine/mission_state_machine.hpp" +#include "urc_state_machine/active_mission.hpp" +#include "urc_state_machine/mission_state.hpp" namespace nav_coordinator { @@ -29,17 +30,6 @@ class NavCoordinator : public rclcpp::Node using ExecuteMission = urc_msgs::action::ExecuteAutonomousMission; using MissionGoalHandle = rclcpp_action::ServerGoalHandle; - enum class State - { - IDLE, - WAITING_FOR_SERVER, - SENDING_GOAL, - TRACKING_GOAL, - SUCCEEDED, - FAILED, - CANCELED - }; - enum class ErrorType { NONE, @@ -74,19 +64,17 @@ class NavCoordinator : public rclcpp::Node 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_{State::IDLE}; - urc_state_machine::MissionStateMachine state_machine_; - // Mission and waypoint callbacks use the default mutually exclusive callback group. - bool mission_reserved_{false}; - std::shared_ptr active_mission_handle_; + urc_state_machine::MissionState state_{urc_state_machine::MissionState::IDLE}; + std::shared_ptr active_mission_; std::string follower_action_name_; bool cancel_on_new_waypoint_; std::string map_frame_id_; diff --git a/urc_state_machine/src/coordinator_state.cpp b/urc_state_machine/src/coordinator_state.cpp index 94be0662..472b76e7 100644 --- a/urc_state_machine/src/coordinator_state.cpp +++ b/urc_state_machine/src/coordinator_state.cpp @@ -2,7 +2,9 @@ namespace nav_coordinator { -void NavCoordinator::transitionTo(State new_state, const std::string & reason) +void NavCoordinator::transitionTo( + urc_state_machine::MissionState new_state, + const std::string & reason) { if (state_ == new_state) { return; @@ -68,22 +70,26 @@ std::string NavCoordinator::errorTypeToString(ErrorType error_type) const } } -std::string NavCoordinator::stateToString(State state) const +std::string NavCoordinator::stateToString(urc_state_machine::MissionState state) const { switch (state) { - case State::IDLE: + case urc_state_machine::MissionState::IDLE: return "IDLE"; - case State::WAITING_FOR_SERVER: - return "WAITING_FOR_SERVER"; - case State::SENDING_GOAL: - return "SENDING_GOAL"; - case State::TRACKING_GOAL: - return "TRACKING_GOAL"; - case State::SUCCEEDED: + 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 State::FAILED: + case urc_state_machine::MissionState::FAILED: return "FAILED"; - case State::CANCELED: + case urc_state_machine::MissionState::CANCELED: return "CANCELED"; default: return "UNKNOWN"; diff --git a/urc_state_machine/src/follower_navigation.cpp b/urc_state_machine/src/follower_navigation.cpp index 01967f04..d58670cb 100644 --- a/urc_state_machine/src/follower_navigation.cpp +++ b/urc_state_machine/src/follower_navigation.cpp @@ -7,12 +7,11 @@ namespace nav_coordinator { void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) { - transitionTo(State::WAITING_FOR_SERVER, "checking follower action server"); if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { handleError( ErrorType::SERVER_UNAVAILABLE, "Follower action server '" + follower_action_name_ + "' not available."); - transitionTo(State::FAILED, "follower action server unavailable"); + transitionTo(urc_state_machine::MissionState::FAILED, "follower action server unavailable"); return; } @@ -22,7 +21,7 @@ void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & wa goal_msg.has_path = false; goal_msg.enforce_goal_heading = false; - transitionTo(State::SENDING_GOAL, "forwarding waypoint to follower"); + transitionTo(urc_state_machine::MissionState::NAVIGATING, "forwarding waypoint to follower"); rclcpp_action::Client::SendGoalOptions options; options.goal_response_callback = std::bind( @@ -39,12 +38,11 @@ void NavCoordinator::handleGoalResponse(const GoalHandleNavigate::SharedPtr & go { if (!goal_handle) { handleError(ErrorType::FOLLOWER_FAILURE, "Follower action server rejected the goal."); - transitionTo(State::FAILED, "follower rejected goal"); + transitionTo(urc_state_machine::MissionState::FAILED, "follower rejected goal"); return; } active_goal_handle_ = goal_handle; - transitionTo(State::TRACKING_GOAL, "follower accepted goal"); } void NavCoordinator::handleFeedback( @@ -64,7 +62,7 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { if (result.result->error_code == NavigateToWaypoint::Result::SUCCESS) { - transitionTo(State::SUCCEEDED, "follower reported success"); + transitionTo(urc_state_machine::MissionState::SUCCEEDED, "follower reported success"); return; } @@ -84,25 +82,25 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu "Follower finished with error_code=" + std::to_string(result.result->error_code)); break; } - transitionTo(State::FAILED, "follower finished with error"); + transitionTo(urc_state_machine::MissionState::FAILED, "follower finished with error"); return; } if (result.code == rclcpp_action::ResultCode::ABORTED) { handleError(ErrorType::FOLLOWER_FAILURE, "Follower aborted goal."); - transitionTo(State::FAILED, "follower aborted goal"); + transitionTo(urc_state_machine::MissionState::FAILED, "follower aborted goal"); return; } if (result.code == rclcpp_action::ResultCode::CANCELED) { - transitionTo(State::CANCELED, "follower canceled goal"); + transitionTo(urc_state_machine::MissionState::CANCELED, "follower canceled goal"); return; } handleError( ErrorType::UNKNOWN_ERROR, "Unknown follower result code: " + std::to_string(static_cast(result.code))); - transitionTo(State::FAILED, "unknown follower result code"); + transitionTo(urc_state_machine::MissionState::FAILED, "unknown follower result code"); } } diff --git a/urc_state_machine/src/mission_action.cpp b/urc_state_machine/src/mission_action.cpp index 3bc3e6aa..8a0c9a48 100644 --- a/urc_state_machine/src/mission_action.cpp +++ b/urc_state_machine/src/mission_action.cpp @@ -56,40 +56,38 @@ rclcpp_action::GoalResponse NavCoordinator::handleMissionGoal( return rclcpp_action::GoalResponse::REJECT; } - if (mission_reserved_ || active_goal_handle_ || - state_ == State::WAITING_FOR_SERVER || state_ == State::SENDING_GOAL || - state_ == State::TRACKING_GOAL || !follower_client_->action_server_is_ready()) - { + if (active_mission_ || active_goal_handle_ || !follower_client_->action_server_is_ready()) { return rclcpp_action::GoalResponse::REJECT; } - mission_reserved_ = true; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse NavCoordinator::handleMissionCancel( - std::shared_ptr) + std::shared_ptr goal_handle) { - // TODO: Accept cancellation once active-operation stopping is implemented. - return rclcpp_action::CancelResponse::REJECT; + if (!active_mission_ || active_mission_->goal_handle != goal_handle) { + return rclcpp_action::CancelResponse::REJECT; + } + + active_mission_->cancellation_requested = true; + if (active_goal_handle_) { + follower_client_->async_cancel_goal(active_goal_handle_); + } + + return rclcpp_action::CancelResponse::ACCEPT; } void NavCoordinator::handleMissionAccepted( std::shared_ptr goal_handle) { - active_mission_handle_ = goal_handle; - active_waypoint_ = goal_handle->get_goal()->waypoint; + active_mission_ = std::make_shared(); + active_mission_->goal_handle = goal_handle; + active_mission_->original_waypoint = goal_handle->get_goal()->waypoint; + active_mission_->search_mode = urc_state_machine::SearchMode::NONE; last_error_ = ErrorType::NONE; last_error_details_.clear(); - const auto transition = state_machine_.start(urc_state_machine::SearchMode::NONE); - if (!transition.accepted || - transition.command != urc_state_machine::MissionCommand::NAVIGATE_TO_WAYPOINT) - { - failMissionNavigation("Mission state machine could not start navigation."); - return; - } - sendMissionNavigation(); } diff --git a/urc_state_machine/src/mission_navigation.cpp b/urc_state_machine/src/mission_navigation.cpp index 7009d2a6..9f3cbc83 100644 --- a/urc_state_machine/src/mission_navigation.cpp +++ b/urc_state_machine/src/mission_navigation.cpp @@ -7,31 +7,49 @@ namespace nav_coordinator void NavCoordinator::sendMissionNavigation() { + if (!active_mission_) { + return; + } + if (!follower_client_->action_server_is_ready()) { failMissionNavigation("Follower action server is unavailable."); return; } NavigateToWaypoint::Goal goal; - goal.goal = active_waypoint_; + goal.goal = active_mission_->original_waypoint; goal.has_goal = true; goal.has_path = false; goal.enforce_goal_heading = false; + const auto mission = active_mission_; rclcpp_action::Client::SendGoalOptions options; - options.goal_response_callback = [this](const GoalHandleNavigate::SharedPtr & handle) { + options.goal_response_callback = + [this, mission](const GoalHandleNavigate::SharedPtr & handle) { + if (active_mission_ != mission) { + if (handle) { + follower_client_->async_cancel_goal(handle); + } + return; + } if (!handle) { failMissionNavigation("Follower rejected the navigation goal."); return; } active_goal_handle_ = handle; - transitionTo(State::TRACKING_GOAL, "follower accepted mission navigation"); + if (mission->cancellation_requested) { + follower_client_->async_cancel_goal(handle); + } }; - options.result_callback = [this](const GoalHandleNavigate::WrappedResult & result) { + options.result_callback = [this, mission]( + const GoalHandleNavigate::WrappedResult & result) { + if (active_mission_ != mission) { + return; + } finishMissionNavigation(result); }; - transitionTo(State::SENDING_GOAL, "sending mission navigation goal"); + transitionTo(urc_state_machine::MissionState::NAVIGATING, "sending mission navigation goal"); try { follower_client_->async_send_goal(goal, options); } catch (const rclcpp::exceptions::RCLError & error) { @@ -41,52 +59,62 @@ void NavCoordinator::sendMissionNavigation() void NavCoordinator::finishMissionNavigation(const GoalHandleNavigate::WrappedResult & result) { - if (!active_mission_handle_) { + if (!active_mission_) { return; } active_goal_handle_.reset(); - if (result.code != rclcpp_action::ResultCode::SUCCEEDED || !result.result || - result.result->error_code != NavigateToWaypoint::Result::SUCCESS) + if (result.code == rclcpp_action::ResultCode::CANCELED && + active_mission_->cancellation_requested) { - failMissionNavigation("Follower navigation did not succeed."); + finishCanceledMission(); return; } - const auto transition = state_machine_.handle( - urc_state_machine::MissionEvent::NAVIGATION_SUCCEEDED); - if (!transition.accepted || - transition.command != urc_state_machine::MissionCommand::COMPLETE_SUCCESS) + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || !result.result || + result.result->error_code != NavigateToWaypoint::Result::SUCCESS) { - failMissionNavigation("Unexpected mission transition after navigation."); + failMissionNavigation("Follower navigation did not succeed."); return; } auto mission_result = std::make_shared(); mission_result->error_code = ExecuteMission::Result::SUCCESS; mission_result->message = "Reached the mission waypoint."; - active_mission_handle_->succeed(mission_result); - active_mission_handle_.reset(); - mission_reserved_ = false; - transitionTo(State::SUCCEEDED, "mission navigation completed"); + active_mission_->goal_handle->succeed(mission_result); + active_mission_.reset(); + transitionTo(urc_state_machine::MissionState::SUCCEEDED, "mission navigation completed"); +} + +void NavCoordinator::finishCanceledMission() +{ + if (!active_mission_) { + return; + } + + auto result = std::make_shared(); + result->error_code = ExecuteMission::Result::CANCELED; + result->message = "Mission canceled."; + active_mission_->goal_handle->canceled(result); + active_goal_handle_.reset(); + active_mission_.reset(); + transitionTo(urc_state_machine::MissionState::CANCELED, "mission canceled"); } void NavCoordinator::failMissionNavigation(const std::string & reason) { - if (!active_mission_handle_) { + if (!active_mission_) { return; } - state_machine_.handle(urc_state_machine::MissionEvent::OPERATION_FAILED); auto result = std::make_shared(); result->error_code = ExecuteMission::Result::NAVIGATION_FAILED; result->message = reason; - active_mission_handle_->abort(result); - active_mission_handle_.reset(); + active_mission_->goal_handle->abort(result); + active_mission_.reset(); active_goal_handle_.reset(); - mission_reserved_ = false; handleError(ErrorType::FOLLOWER_FAILURE, reason); - transitionTo(State::FAILED, reason); + transitionTo(urc_state_machine::MissionState::FAILED, reason); } } diff --git a/urc_state_machine/src/mission_state_machine.cpp b/urc_state_machine/src/mission_state_machine.cpp deleted file mode 100644 index af1cdb66..00000000 --- a/urc_state_machine/src/mission_state_machine.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include "urc_state_machine/mission_state_machine.hpp" - -namespace urc_state_machine -{ - -MissionState MissionStateMachine::state() const -{ - return state_; -} - -Transition MissionStateMachine::transitionTo( - MissionState next_state, - MissionCommand command) -{ - state_ = next_state; - return {true, state_, command}; -} - -Transition MissionStateMachine::start(SearchMode mode) -{ - const bool can_start = - state_ == MissionState::IDLE || - state_ == MissionState::SUCCEEDED || - state_ == MissionState::FAILED || - state_ == MissionState::CANCELED; - - if (!can_start) { - return {false, state_, MissionCommand::NONE}; - } - - switch (mode) { - case SearchMode::NONE: - case SearchMode::YOLO: - case SearchMode::ARUCO_1: - case SearchMode::ARUCO_2: - break; - default: - return {false, state_, MissionCommand::NONE}; - } - - search_mode_ = mode; - - return transitionTo( - MissionState::NAVIGATING, - MissionCommand::NAVIGATE_TO_WAYPOINT); -} - -Transition MissionStateMachine::handle(MissionEvent event) -{ - if (state_ != MissionState::NAVIGATING) { - return {false, state_, MissionCommand::NONE}; - } - - if (event == MissionEvent::OPERATION_FAILED) { - return transitionTo(MissionState::FAILED, MissionCommand::COMPLETE_FAILURE); - } - - if (event != MissionEvent::NAVIGATION_SUCCEEDED) { - return {false, state_, MissionCommand::NONE}; - } - - switch (search_mode_) { - case SearchMode::NONE: - return transitionTo( - MissionState::SUCCEEDED, - MissionCommand::COMPLETE_SUCCESS); - - case SearchMode::YOLO: - return transitionTo( - MissionState::SEARCHING_YOLO, - MissionCommand::START_YOLO_SEARCH); - - case SearchMode::ARUCO_1: - case SearchMode::ARUCO_2: - return transitionTo( - MissionState::SEARCHING_ARUCO, - MissionCommand::START_ARUCO_SEARCH); - } - - return {false, state_, MissionCommand::NONE}; -} - -} // namespace urc_state_machine diff --git a/urc_state_machine/src/waypoint_requests.cpp b/urc_state_machine/src/waypoint_requests.cpp index 0d194279..05fd8326 100644 --- a/urc_state_machine/src/waypoint_requests.cpp +++ b/urc_state_machine/src/waypoint_requests.cpp @@ -7,7 +7,7 @@ namespace nav_coordinator { void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - if (mission_reserved_) { + if (active_mission_) { return; } @@ -19,7 +19,9 @@ void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::Share active_waypoint_.pose.position.y); if (active_goal_handle_ && cancel_on_new_waypoint_) { - transitionTo(State::CANCELED, "canceling current goal due to new waypoint"); + transitionTo( + urc_state_machine::MissionState::CANCELED, + "canceling current goal due to new waypoint"); follower_client_->async_cancel_goal(active_goal_handle_); active_goal_handle_.reset(); } @@ -29,7 +31,7 @@ void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::Share void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) { - if (mission_reserved_) { + if (active_mission_) { return; } @@ -42,7 +44,9 @@ void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr handleError( ErrorType::PLANNER_FAILURE, std::string("Cannot process GPS waypoint: ") + ex.what()); - transitionTo(State::FAILED, "gps waypoint rejected - transform unavailable"); + transitionTo( + urc_state_machine::MissionState::FAILED, + "gps waypoint rejected - transform unavailable"); return; } @@ -58,7 +62,9 @@ void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr active_waypoint_.pose.position.y); if (active_goal_handle_ && cancel_on_new_waypoint_) { - transitionTo(State::CANCELED, "canceling current goal due to new GPS waypoint"); + transitionTo( + urc_state_machine::MissionState::CANCELED, + "canceling current goal due to new GPS waypoint"); follower_client_->async_cancel_goal(active_goal_handle_); active_goal_handle_.reset(); } From 8ef9a747c4f314efe5b0f702d351455439cc38c8 Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Sun, 13 Sep 2026 18:23:29 -0400 Subject: [PATCH 4/4] add state forwarding --- urc_state_machine/src/mission_navigation.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/urc_state_machine/src/mission_navigation.cpp b/urc_state_machine/src/mission_navigation.cpp index 9f3cbc83..5b9450af 100644 --- a/urc_state_machine/src/mission_navigation.cpp +++ b/urc_state_machine/src/mission_navigation.cpp @@ -41,6 +41,19 @@ void NavCoordinator::sendMissionNavigation() follower_client_->async_cancel_goal(handle); } }; + options.feedback_callback = [this, mission]( + GoalHandleNavigate::SharedPtr, + const std::shared_ptr feedback) { + if (active_mission_ != mission) { + return; + } + + auto mission_feedback = std::make_shared(); + mission_feedback->mission_state = ExecuteMission::Feedback::STATE_NAVIGATING; + mission_feedback->distance_to_goal = feedback->distance_to_goal; + mission_feedback->replan_count = feedback->replan_count; + mission->goal_handle->publish_feedback(mission_feedback); + }; options.result_callback = [this, mission]( const GoalHandleNavigate::WrappedResult & result) { if (active_mission_ != mission) {