diff --git a/urc_state_machine/CMakeLists.txt b/urc_state_machine/CMakeLists.txt index 7dce48ca..74ce1b58 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,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 diff --git a/urc_state_machine/README.md b/urc_state_machine/README.md index f4b546e6..a77d6b57 100644 --- a/urc_state_machine/README.md +++ b/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 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/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/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/nav_coordinator.hpp b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp index 349d1648..61c5db96 100644 --- a/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp +++ b/urc_state_machine/include/urc_state_machine/nav_coordinator.hpp @@ -1,19 +1,20 @@ #ifndef NAV_COORDINATOR_HPP_ #define NAV_COORDINATOR_HPP_ -#include #include +#include #include -#include -#include #include #include #include #include #include +#include #include #include +#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; - - 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; 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 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 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 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::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_; - 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..472b76e7 --- /dev/null +++ b/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); +} + +} diff --git a/urc_state_machine/src/follower_navigation.cpp b/urc_state_machine/src/follower_navigation.cpp new file mode 100644 index 00000000..d58670cb --- /dev/null +++ b/urc_state_machine/src/follower_navigation.cpp @@ -0,0 +1,106 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +#include +#include + +namespace nav_coordinator +{ +void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) +{ + if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { + handleError( + ErrorType::SERVER_UNAVAILABLE, + "Follower action server '" + follower_action_name_ + "' not available."); + transitionTo(urc_state_machine::MissionState::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(urc_state_machine::MissionState::NAVIGATING, "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(urc_state_machine::MissionState::FAILED, "follower rejected goal"); + return; + } + + active_goal_handle_ = goal_handle; +} + +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(urc_state_machine::MissionState::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(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(urc_state_machine::MissionState::FAILED, "follower aborted goal"); + return; + } + + if (result.code == rclcpp_action::ResultCode::CANCELED) { + 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(urc_state_machine::MissionState::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/mission_action.cpp b/urc_state_machine/src/mission_action.cpp new file mode 100644 index 00000000..8a0c9a48 --- /dev/null +++ b/urc_state_machine/src/mission_action.cpp @@ -0,0 +1,94 @@ +#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 (active_mission_ || active_goal_handle_ || !follower_client_->action_server_is_ready()) { + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse NavCoordinator::handleMissionCancel( + std::shared_ptr goal_handle) +{ + 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_ = 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(); + + 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..5b9450af --- /dev/null +++ b/urc_state_machine/src/mission_navigation.cpp @@ -0,0 +1,133 @@ +#include "urc_state_machine/nav_coordinator.hpp" + +#include + +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_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, 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; + if (mission->cancellation_requested) { + 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) { + return; + } + finishMissionNavigation(result); + }; + + transitionTo(urc_state_machine::MissionState::NAVIGATING, "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_) { + return; + } + + active_goal_handle_.reset(); + if (result.code == rclcpp_action::ResultCode::CANCELED && + active_mission_->cancellation_requested) + { + finishCanceledMission(); + return; + } + + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || !result.result || + result.result->error_code != NavigateToWaypoint::Result::SUCCESS) + { + 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_->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_) { + return; + } + + auto result = std::make_shared(); + result->error_code = ExecuteMission::Result::NAVIGATION_FAILED; + result->message = reason; + active_mission_->goal_handle->abort(result); + active_mission_.reset(); + active_goal_handle_.reset(); + handleError(ErrorType::FOLLOWER_FAILURE, reason); + transitionTo(urc_state_machine::MissionState::FAILED, reason); +} + +} diff --git a/urc_state_machine/src/nav_coordinator.cpp b/urc_state_machine/src/nav_coordinator.cpp index 338272dd..afeaca16 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_); @@ -40,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'.", @@ -52,314 +50,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..05fd8326 --- /dev/null +++ b/urc_state_machine/src/waypoint_requests.cpp @@ -0,0 +1,75 @@ +#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) +{ + if (active_mission_) { + return; + } + + 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( + urc_state_machine::MissionState::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) +{ + if (active_mission_) { + return; + } + + 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( + urc_state_machine::MissionState::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( + 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(); + } + + sendFollowerGoal(active_waypoint_); +} + +}