diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index e73d03ac5..052af48de 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,5 +16,6 @@ jobs: setup_script: "tests/setup.sh" test_scripts: '[ "tests/simulator_tests/waypoint_navigation/simulator_test.sh", - "tests/simulator_tests/los_test/simulator_test.sh" + "tests/simulator_tests/los_test/simulator_test.sh", + "tests/simulator_tests/waypoint_manager_test/simulator_test.sh" ]' diff --git a/.gitignore b/.gitignore index f86dbd841..9af41aca7 100644 --- a/.gitignore +++ b/.gitignore @@ -57,7 +57,4 @@ qtcreator-* # Emacs .#* -# Catkin custom files -COLCON_IGNORE - # End of https://www.gitignore.io/api/ros diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index 6bc1584f1..2fb46649e 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -74,8 +74,9 @@ class ReferenceFilterNode : public rclcpp::Node { Eigen::Vector18d fill_reference_state(); - Eigen::Vector6d fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal); + Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal); + + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode); vortex_msgs::msg::ReferenceFilter fill_reference_msg(); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 02d463c0f..614e10e93 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -195,12 +195,12 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { } Eigen::Vector6d ReferenceFilterNode::fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal) { - double x{goal.pose.position.x}; - double y{goal.pose.position.y}; - double z{goal.pose.position.z}; + const geometry_msgs::msg::Pose& goal) { + double x{goal.position.x}; + double y{goal.position.y}; + double z{goal.position.z}; - const auto& o = goal.pose.orientation; + const auto& o = goal.orientation; Eigen::Quaterniond q(o.w, o.x, o.y, o.z); Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); double roll{euler_angles(0)}; @@ -231,6 +231,43 @@ vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { return feedback_msg; } +Eigen::Vector6d ReferenceFilterNode::apply_mode_logic( + const Eigen::Vector6d& r_in, + uint8_t mode) { + Eigen::Vector6d r_out = r_in; + + switch (mode) { + case vortex_msgs::msg::Waypoint::FULL_POSE: + break; + + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + r_out(3) = x_(3); + r_out(4) = x_(4); + r_out(5) = x_(5); + break; + + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { + double dx = r_in(0) - x_(0); + double dy = r_in(1) - x_(1); + + double forward_heading = std::atan2(dy, dx); + + r_out(3) = 0.0; + r_out(4) = 0.0; + r_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + r_out(0) = x_(0); + r_out(1) = x_(1); + r_out(2) = x_(2); + break; + } + + return r_out; +} + void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { @@ -243,9 +280,21 @@ void ReferenceFilterNode::execute( x_ = fill_reference_state(); - const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal; + const geometry_msgs::msg::Pose goal = + goal_handle->get_goal()->waypoint.pose; + uint8_t mode = goal_handle->get_goal()->waypoint.mode; + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } - r_ = fill_reference_goal(goal); + Eigen::Vector6d r_temp = fill_reference_goal(goal); + r_ = apply_mode_logic(r_temp, mode); auto feedback = std::make_shared< vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); @@ -274,12 +323,12 @@ void ReferenceFilterNode::execute( vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); - feedback->feedback = feedback_msg; + feedback->reference = feedback_msg; goal_handle->publish_feedback(feedback); reference_pub_->publish(feedback_msg); - if ((x_.head(6) - r_.head(6)).norm() < 0.1) { + if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { result->success = true; goal_handle->succeed(result); x_.head(6) = r_.head(6); @@ -292,6 +341,17 @@ void ReferenceFilterNode::execute( loop_rate.sleep(); } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } } RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) diff --git a/mission/FSM/docking/COLCON_IGNORE b/mission/FSM/docking/COLCON_IGNORE new file mode 100644 index 000000000..f4db3a26d --- /dev/null +++ b/mission/FSM/docking/COLCON_IGNORE @@ -0,0 +1,2 @@ +# Ignored temporarily to skip building while debugging yasmin upgrade +# Remove this file to include the package in colcon builds again diff --git a/mission/FSM/docking/src/docking.cpp b/mission/FSM/docking/src/docking.cpp index 4476d90a1..c251dda75 100644 --- a/mission/FSM/docking/src/docking.cpp +++ b/mission/FSM/docking/src/docking.cpp @@ -79,16 +79,18 @@ ApproachDockingStationState::create_goal_handler( blackboard->set("is_home", false); - docking_fsm::PoseStamped docking_offset_goal = - blackboard->get("docking_offset_goal"); + docking_fsm::Pose docking_offset_goal = + blackboard->get("docking_offset_goal").pose; + + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal; - goal.goal = docking_offset_goal; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", - docking_offset_goal.pose.position.x, - docking_offset_goal.pose.position.y, - docking_offset_goal.pose.position.z); + docking_offset_goal.position.x, docking_offset_goal.position.y, + docking_offset_goal.position.z); return goal; } @@ -113,15 +115,15 @@ void ApproachDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } GoAboveDockingStationState::GoAboveDockingStationState( @@ -145,7 +147,9 @@ GoAboveDockingStationState::create_goal_handler( auto docking_offset_goal = blackboard->get("docking_offset_goal"); - goal.goal = docking_offset_goal; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -179,15 +183,15 @@ void GoAboveDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } ConvergeDockingStationState::ConvergeDockingStationState( @@ -213,7 +217,9 @@ ConvergeDockingStationState::create_goal_handler( docking_fsm::PoseStamped dock_pose = blackboard->get("dock_pose"); - goal.goal = dock_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = dock_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -242,14 +248,14 @@ void ConvergeDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string DockedState( @@ -286,7 +292,9 @@ docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler( docking_fsm::PoseStamped start_pose = blackboard->get("start_pose"); - goal.goal = start_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = start_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", start_pose.pose.position.x, start_pose.pose.position.y, @@ -313,17 +321,17 @@ void ReturnHomeState::print_feedback( std::shared_ptr blackboard, std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; - current_pose.orientation.x = feedback->feedback.roll; - current_pose.orientation.y = feedback->feedback.pitch; - current_pose.orientation.z = feedback->feedback.yaw; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; + current_pose.orientation.x = feedback->reference.roll; + current_pose.orientation.y = feedback->reference.pitch; + current_pose.orientation.z = feedback->reference.yaw; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string AbortState( diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt new file mode 100644 index 000000000..0a76ae510 --- /dev/null +++ b/mission/waypoint_manager/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/waypoint_manager_ros.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + vortex_msgs + vortex_utils + tf2_geometry_msgs + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "WaypointManagerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/mission/waypoint_manager/README.md b/mission/waypoint_manager/README.md new file mode 100644 index 000000000..3e6c6411a --- /dev/null +++ b/mission/waypoint_manager/README.md @@ -0,0 +1,84 @@ + + +# Waypoint Manager — ROS 2 Node + + +The **Waypoint Manager** node coordinates mission-level navigation by managing waypoint queues, forwarding them to a **Reference Filter** for trajectory generation, and exposing both an **action interface** (for mission planners) and a **service interface** (for perception-driven waypoint updates). + + + +# Interfaces + +* **[WaypointManager](https://github.com/vortexntnu/vortex-msgs/blob/main/action/WaypointManager.action) (action server)** +* **[WaypointAddition](https://github.com/vortexntnu/vortex-msgs/blob/main/srv/WaypointAddition.srv) (service server)** +* **[ReferenceFilterWaypoint](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action) (action client)** + + +# Behavior Summary + +### **Mission Start** + +* On receiving a new action goal, any existing mission is aborted. +* Waypoints are stored, state is reset, and the first waypoint is sent to the reference filter. + +### **During Mission** + +* Each waypoint is executed sequentially. +* Pose feedback from the reference filter is forwarded to the mission planner. +* `convergence_threshold` determines when a waypoint is reached. +* If `persistent = true`, the action does not end even when waypoints run out. + +### **Mission End** + +* If `persistent = false`, the action completes when the last waypoint is reached. +* If cancelled externally, all state is cleared and reference filter goals are cancelled. + +### **Dynamic Waypoint Addition** + +Allowed only during **persistent** missions: + +* `overwrite = true` → clears old queue and restarts from new waypoints +* `overwrite = false` → appends waypoints to queue +* `priority = true` → turn on priority mode. In this mode additional service requests with `priority = false` are ignored. Priority mode is set to false again when there are no more waypoints in queue. + +--- + +### Check available interfaces: + +```bash +ros2 action list +ros2 service list +``` +beware of namespace! + + +## Example Action Goal (CLI) + +```bash +ros2 action send_goal /orca/waypoint_manager vortex_msgs/action/WaypointManager \ +'{ + waypoints:[ + { + pose:{position:{x:5.0,y:0.0,z:0.0}, + orientation:{x:0,y:0,z:0,w:1}}, + mode:1 + } + ], + convergence_threshold:0.1, + persistent:false +}' +``` + + +## Example Waypoint Addition Service Call + +```bash +ros2 service call /orca/waypoint_addition vortex_msgs/srv/WaypointAddition \ +'{ + waypoints:[ + {pose:{position:{x:2,y:3,z:0},orientation:{x:0,y:0,z:0,w:1}},mode:1} + ], + overwrite:false, + priority:true +}' +``` diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp new file mode 100644 index 000000000..86ba42d3e --- /dev/null +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp @@ -0,0 +1,120 @@ +#ifndef WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ +#define WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace vortex::mission { + +using WaypointManager = vortex_msgs::action::WaypointManager; +using WaypointManagerGoalHandle = + rclcpp_action::ServerGoalHandle; + +using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; +using ReferenceFilterGoalHandle = + rclcpp_action::ClientGoalHandle; + +class WaypointManagerNode : public rclcpp::Node { + public: + explicit WaypointManagerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~WaypointManagerNode() override; + + private: + // @brief Create the action server for WaypointManager. + void set_waypoint_action_server(); + + // @brief Create the action client for ReferenceFilterWaypoint. + void set_reference_action_client(); + + // @brief Create the service servers for SendWaypoints. + void set_waypoint_service_server(); + + // @brief Construct the result message for the WaypointManager action + // @param success Whether the action was successful + // @return The constructed result message + std::shared_ptr + construct_result(bool success) const; + + // @brief Clean up the mission state after completion or cancellation of a + // waypoint action. Cancel active goals and reset internal variables. Make + // system ready for next action. + void cleanup_mission_state(); + + // @brief Send the next goal to the ReferenceFilter action server based on + // the current waypoint index or finish the waypoint action if all waypoints + // have been processed. + void send_next_reference_filter_goal(); + + // @brief Handle incoming action goal requests + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_waypoint_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal_msg); + + // @brief Handle requests to cancel the waypoint action + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_waypoint_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted goal request + // @param goal_handle The goal handle + void handle_waypoint_accepted( + const std::shared_ptr> goal_handle); + + // @brief Handle incoming send waypoints service requests + // Only accepted if waypoint action is running. + // @param request Incoming service request containing waypoint information. + // @param response Service response that should be populated and sent back + // to the caller. + void handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response); + + // @brief Send a goal to the reference filter + // @param goal_msg The action goal + void send_reference_filter_goal( + const vortex_msgs::action::ReferenceFilterWaypoint::Goal& goal_msg); + + rclcpp_action::Client:: + SharedPtr reference_filter_client_; + rclcpp_action::Server::SharedPtr + waypoint_action_server_; + rclcpp::Service::SharedPtr + waypoint_service_server_; + + std::vector waypoints_{}; + std::size_t current_index_{0}; + double convergence_threshold_{0.1}; + + bool persistent_action_mode_active_{false}; + bool priority_mode_active_{false}; + + ReferenceFilterAction::Feedback latest_ref_feedback_; + bool has_reference_pose_{false}; + bool is_cancel_in_progress_{false}; + + std::uint64_t mission_id_ = 0; + + std::shared_ptr active_reference_filter_goal_; + std::shared_ptr active_action_goal_; +}; + +} // namespace vortex::mission + +#endif // WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100644 index 000000000..65286ceb0 --- /dev/null +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + parameters=[], + output='screen', + ) + return LaunchDescription([waypoint_manager_node]) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml new file mode 100644 index 000000000..db3feabde --- /dev/null +++ b/mission/waypoint_manager/package.xml @@ -0,0 +1,28 @@ + + + + waypoint_manager + 0.0.0 + Waypoint manager for waypoint navigation with dp reference filter + Jorgen Fjermedal + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + tf2_geometry_msgs + + launch_testing + launch_testing_ros + launch_testing_ament_cmake + reference_filter_dp + auv_setup + + + ament_cmake + + diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp new file mode 100644 index 000000000..1d4928470 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -0,0 +1,378 @@ +#include "waypoint_manager/waypoint_manager_ros.hpp" +#include +#include +#include +#include + +namespace vortex::mission { + +WaypointManagerNode::WaypointManagerNode(const rclcpp::NodeOptions& options) + : Node("waypoint_manager_node", options) { + set_reference_action_client(); + set_waypoint_action_server(); + set_waypoint_service_server(); + + spdlog::info("WaypointManagerNode started"); +} + +WaypointManagerNode::~WaypointManagerNode() { + if (active_action_goal_ && (active_action_goal_->is_active() || + active_action_goal_->is_canceling())) { + try { + auto res = construct_result(false); + active_action_goal_->abort(res); + } catch (...) { + } + } + + if (active_reference_filter_goal_) { + try { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + } catch (...) { + } + active_reference_filter_goal_.reset(); + } +} + +// --------------------------------------------------------- +// SETUP INTERFACES +// --------------------------------------------------------- + +void WaypointManagerNode::set_reference_action_client() { + reference_filter_client_ = + rclcpp_action::create_client(this, + "reference_filter"); + + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(3))) { + spdlog::warn("ReferenceFilter server not ready"); + } +} + +void WaypointManagerNode::set_waypoint_action_server() { + waypoint_action_server_ = rclcpp_action::create_server( + this, "waypoint_manager", + + [this](auto goal_id, auto goal) { + return handle_waypoint_goal(goal_id, goal); + }, + + [this](auto goal_id) { return handle_waypoint_cancel(goal_id); }, + + [this](auto goal_handle) { + return handle_waypoint_accepted(goal_handle); + }); +} + +void WaypointManagerNode::set_waypoint_service_server() { + waypoint_service_server_ = + this->create_service( + "waypoint_addition", + std::bind( + &WaypointManagerNode::handle_send_waypoints_service_request, + this, std::placeholders::_1, std::placeholders::_2)); +} + +// --------------------------------------------------------- +// HELPERS +// --------------------------------------------------------- + +std::shared_ptr +WaypointManagerNode::construct_result(bool success) const { + auto result = + std::make_shared(); + result->success = success; + result->pose_valid = has_reference_pose_; + if (has_reference_pose_) { + result->final_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + latest_ref_feedback_.reference); + } + return result; +} + +void WaypointManagerNode::cleanup_mission_state() { + waypoints_.clear(); + current_index_ = 0; + persistent_action_mode_active_ = false; + priority_mode_active_ = false; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + active_action_goal_.reset(); +} + +void WaypointManagerNode::send_next_reference_filter_goal() { + if (current_index_ >= waypoints_.size()) { + if (!persistent_action_mode_active_ && active_action_goal_ && + active_action_goal_->is_active()) { + auto wm_res = construct_result(true); + active_action_goal_->succeed(wm_res); + cleanup_mission_state(); + } + return; + } + + ReferenceFilterAction::Goal rf_goal; + rf_goal.waypoint = waypoints_[current_index_]; + rf_goal.convergence_threshold = convergence_threshold_; + + send_reference_filter_goal(rf_goal); +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER ACTION SERVER +// --------------------------------------------------------- + +rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal( + const rclcpp_action::GoalUUID& /*goal_uuid*/, + std::shared_ptr goal) { + if (active_action_goal_ && active_action_goal_->is_active()) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + } + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + ++mission_id_; + + waypoints_ = goal->waypoints; + current_index_ = 0; + persistent_action_mode_active_ = goal->persistent; + priority_mode_active_ = false; + has_reference_pose_ = false; + convergence_threshold_ = goal->convergence_threshold; + + if (waypoints_.empty() && !persistent_action_mode_active_) { + spdlog::warn( + "WaypointManager: received empty waypoint list and non-persistent " + "mode"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void WaypointManagerNode::handle_waypoint_accepted( + const std::shared_ptr goal_handle) { + spdlog::info("WaypointManager: action goal accepted"); + active_action_goal_ = goal_handle; + + send_next_reference_filter_goal(); +} + +rclcpp_action::CancelResponse WaypointManagerNode::handle_waypoint_cancel( + const std::shared_ptr /*goal_handle*/) { + spdlog::info("WaypointManagerAction: cancel requested"); + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER SERVICE SERVER +// --------------------------------------------------------- + +void WaypointManagerNode::handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response) { + if (!persistent_action_mode_active_ || !active_action_goal_ || + !active_action_goal_->is_active()) { + response->success = false; + return; + } + + if (priority_mode_active_ && !request->take_priority && + current_index_ < waypoints_.size()) { + response->success = false; + return; + } + + priority_mode_active_ = request->take_priority; + + if (request->overwrite_prior_waypoints) { + waypoints_ = request->waypoints; + current_index_ = 0; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + if (!waypoints_.empty()) { + send_next_reference_filter_goal(); + } + + response->success = true; + return; + } + + waypoints_.insert(waypoints_.end(), request->waypoints.begin(), + request->waypoints.end()); + + if (!active_reference_filter_goal_ && current_index_ < waypoints_.size()) { + send_next_reference_filter_goal(); + } + + response->success = true; +} + +// --------------------------------------------------------- +// REFERENCE FILTER ACTION CLIENT +// --------------------------------------------------------- + +void WaypointManagerNode::send_reference_filter_goal( + const ReferenceFilterAction::Goal& goal_msg) { + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + const std::uint64_t this_mission = mission_id_; + + rclcpp_action::Client::SendGoalOptions options; + + options.goal_response_callback = + [this, this_mission](ReferenceFilterGoalHandle::SharedPtr gh) { + if (!gh) { + spdlog::warn("ReferenceFilter goal rejected"); + return; + } + + if (this_mission == mission_id_) { + active_reference_filter_goal_ = gh; + } else { + spdlog::info( + "RF goal response for old mission, ignoring handle"); + } + }; + + options.feedback_callback = + [this, this_mission]( + ReferenceFilterGoalHandle::SharedPtr, + const std::shared_ptr fb) { + if (this_mission != mission_id_) { + return; + } + + latest_ref_feedback_ = *fb; + has_reference_pose_ = true; + + if (!active_action_goal_ || !active_action_goal_->is_active()) + return; + + geometry_msgs::msg::Pose robot_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + fb->reference); + + if (current_index_ < waypoints_.size()) { + auto wm_fb = std::make_shared(); + wm_fb->current_pose = robot_pose; + wm_fb->current_waypoint = waypoints_[current_index_]; + active_action_goal_->publish_feedback(wm_fb); + } + }; + + options + .result_callback = [this, this_mission]( + const ReferenceFilterGoalHandle::WrappedResult& + res) { + if (this_mission != mission_id_) { + spdlog::info( + "ReferenceFilter result received for old mission, ignoring."); + return; + } + + active_reference_filter_goal_.reset(); + + if (!active_action_goal_) { + spdlog::info( + "ReferenceFilter result received but no active WM goal"); + return; + } + + const bool wm_canceling = active_action_goal_->is_canceling(); + const bool wm_active = active_action_goal_->is_active(); + + switch (res.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + spdlog::info("ReferenceFilter goal reached waypoint"); + + if (wm_canceling) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + + cleanup_mission_state(); + } else { + current_index_++; + send_next_reference_filter_goal(); + } + break; + } + + case rclcpp_action::ResultCode::CANCELED: { + spdlog::info("ReferenceFilter goal cancelled"); + + if (wm_canceling && wm_active) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + cleanup_mission_state(); + } + break; + } + + case rclcpp_action::ResultCode::ABORTED: { + spdlog::warn("ReferenceFilter goal aborted unexpectedly"); + if (wm_active) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + cleanup_mission_state(); + } + break; + } + + default: + spdlog::error( + "ReferenceFilter goal returned unknown result code"); + break; + } + }; + + reference_filter_client_->async_send_goal(goal_msg, options); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(WaypointManagerNode) + +} // namespace vortex::mission diff --git a/mission/waypoint_manager/test/CMakeLists.txt b/mission/waypoint_manager/test/CMakeLists.txt new file mode 100644 index 000000000..d63481816 --- /dev/null +++ b/mission/waypoint_manager/test/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager_test) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +find_package(ament_cmake_ros REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) +endfunction() + +add_ros_isolated_launch_test(test_single_waypoint.py TIMEOUT 300) +add_ros_isolated_launch_test(test_service.py TIMEOUT 300) diff --git a/mission/waypoint_manager/test/test_service.py b/mission/waypoint_manager/test/test_service.py new file mode 100644 index 000000000..6bc827b97 --- /dev/null +++ b/mission/waypoint_manager/test/test_service.py @@ -0,0 +1,153 @@ +import os +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.action import ActionClient +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint +from vortex_msgs.srv import SendWaypoints + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerService(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _call_send_waypoints( + self, + waypoints, + switching_threshold=0.3, + overwrite=False, + take_priority=False, + timeout=5.0, + ): + client = self.node.create_client(SendWaypoints, '/orca/waypoint_addition') + assert client.wait_for_service(timeout_sec=5.0), ( + 'SendWaypoints service not available' + ) + + req = SendWaypoints.Request() + req.waypoints = list(waypoints) + req.switching_threshold = float(switching_threshold) + req.overwrite_prior_waypoints = bool(overwrite) + req.take_priority = bool(take_priority) + + fut = client.call_async(req) + rclpy.spin_until_future_complete(self.node, fut, timeout_sec=timeout) + assert fut.done(), 'Timed out waiting for SendWaypoints response' + return fut.result() + + def test_service_returns_false_when_no_persistent_action(self): + # When no persistent action is active, service should return false + wp = Waypoint() + wp.pose.position.x = 1.0 + wp.pose.position.y = 2.0 + wp.pose.position.z = 1.0 + + resp = self._call_send_waypoints([wp], overwrite=False, take_priority=False) + assert not resp.success, ( + 'Service should return false when no persistent action is active' + ) + + def test_priority_blocks_non_priority_calls(self): + # Send a persistent action goal so the waypoint manager is in persistent mode + action_client = ActionClient( + self.node, WaypointManager, '/orca/waypoint_manager' + ) + assert action_client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp1 = Waypoint() + wp1.pose.position.x = 0.0 + wp1.pose.position.y = 0.0 + wp1.pose.position.z = 1.0 + wp2 = Waypoint() + wp2.pose.position.x = 1.0 + wp2.pose.position.y = 1.0 + wp2.pose.position.z = 1.0 + + goal_msg.waypoints = [wp1, wp2] + goal_msg.persistent = True + goal_msg.convergence_threshold = 0.3 + + send_fut = action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self.node, send_fut, timeout_sec=20.0) + assert send_fut.done(), 'Timed out waiting for goal response' + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Call service to take priority + resp1 = self._call_send_waypoints([], overwrite=False, take_priority=True) + assert resp1.success, 'Priority service call should succeed' + + # Now a non-priority call while priority is active should be rejected + wp_extra = Waypoint() + wp_extra.pose.position.x = 2.0 + wp_extra.pose.position.y = 2.0 + wp_extra.pose.position.z = 1.0 + + resp2 = self._call_send_waypoints( + [wp_extra], overwrite=False, take_priority=False + ) + assert not resp2.success, ( + 'Non-priority call should be rejected while priority is active' + ) + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/mission/waypoint_manager/test/test_single_waypoint.py b/mission/waypoint_manager/test/test_single_waypoint.py new file mode 100644 index 000000000..35e02b79e --- /dev/null +++ b/mission/waypoint_manager/test/test_single_waypoint.py @@ -0,0 +1,147 @@ +import os +import time +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from rclpy.qos import qos_profile_sensor_data +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerAcceptsGoal(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _publish_fake_odom(self, x, y, z, duration_sec=5.0, rate_hz=10.0): + pub = self.node.create_publisher( + Odometry, + '/orca/odom', + qos_profile_sensor_data, + ) + + msg = Odometry() + msg.header.frame_id = 'odom' + msg.child_frame_id = 'base_link' + msg.pose.pose.position.x = x + msg.pose.pose.position.y = y + msg.pose.pose.position.z = z + + end_time = time.time() + duration_sec + period = 1.0 / rate_hz + + while time.time() < end_time: + msg.header.stamp = self.node.get_clock().now().to_msg() + pub.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(period) + + self.node.destroy_publisher(pub) + + def test_accepts_and_executes_goal(self): + client = ActionClient(self.node, WaypointManager, '/orca/waypoint_manager') + + assert client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp = Waypoint() + wp.pose.position.x = 0.0 + wp.pose.position.y = 0.0 + wp.pose.position.z = 1.0 + + goal_msg.waypoints = [wp] + goal_msg.persistent = False + goal_msg.convergence_threshold = 0.3 + + send_fut = client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete( + self.node, + send_fut, + timeout_sec=20.0, + ) + + assert send_fut.done(), 'Timed out waiting for goal response' + + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Publish fake odometry at the goal position + self._publish_fake_odom( + x=wp.pose.position.x, + y=wp.pose.position.y, + z=wp.pose.position.z, + duration_sec=5.0, + ) + + result_fut = goal_handle.get_result_async() + rclpy.spin_until_future_complete( + self.node, + result_fut, + timeout_sec=20.0, + ) + + assert result_fut.done(), 'Timed out waiting for result' + + result = result_fut.result().result + assert result.success, 'Waypoint execution failed' + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/tests/ros_node_tests/reference_filter_node_test.sh b/tests/ros_node_tests/reference_filter_node_test.sh old mode 100644 new mode 100755 index f9278d31c..8561bf190 --- a/tests/ros_node_tests/reference_filter_node_test.sh +++ b/tests/ros_node_tests/reference_filter_node_test.sh @@ -28,7 +28,10 @@ fi # Send action goal echo "Sending goal..." -ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint "{goal: {pose: {position: {x: 1.0}}}}" & +ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \ +"{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, mode: 0}}" & + +sleep 2 # Check if controller correctly publishes guidance echo "Waiting for guidance data..." diff --git a/tests/simulator_tests/waypoint_manager_test/check_goal.py b/tests/simulator_tests/waypoint_manager_test/check_goal.py new file mode 100644 index 000000000..89f4a023e --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/check_goal.py @@ -0,0 +1,103 @@ +import math +import os +import time + +import rclpy +import yaml +from geometry_msgs.msg import PoseWithCovarianceStamped +from rclpy.node import Node +from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from vortex_utils.python_utils import quat_to_euler + +best_effort_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.BEST_EFFORT, +) + +# Read goal from temp file +file_path = 'goal_pose.yaml' +with open(file_path) as f: + data = yaml.safe_load(f) + +# Remove temp file +os.remove(file_path) +print(f"Temp file {file_path} deleted") +goal_pos = data['pos'] +goal_ori = data['ori'] + +pos_tol = 0.6 # meters (match python test tolerance) +ori_tol = 0.5 # rad (loose) + + +class CheckGoalNode(Node): + def __init__(self): + super().__init__('check_goal_node') + self.pose_sub_ = self.create_subscription( + PoseWithCovarianceStamped, '/orca/pose', self.pose_callback, best_effort_qos + ) + + self.current_pose_: PoseWithCovarianceStamped = None + self.received_pose_: bool = False + + def pose_callback(self, msg: PoseWithCovarianceStamped): + self.current_pose_ = msg + self.received_pose_ = True + + +def main(args=None): + rclpy.init(args=args) + node = CheckGoalNode() + + print(f"Waiting for vehicle to reach goal: {goal_pos} with orientation {goal_ori}") + + start_time = time.time() + timeout = 240 # seconds; waypoint manager may take longer + + x = y = z = None + current_ori = (0.0, 0.0, 0.0) + + while rclpy.ok() and time.time() - start_time < timeout: + rclpy.spin_once(node) + if node.received_pose_: + x = node.current_pose_.pose.pose.position.x + y = node.current_pose_.pose.pose.position.y + z = node.current_pose_.pose.pose.position.z + q_w = node.current_pose_.pose.pose.orientation.w + q_x = node.current_pose_.pose.pose.orientation.x + q_y = node.current_pose_.pose.pose.orientation.y + q_z = node.current_pose_.pose.pose.orientation.z + current_ori = quat_to_euler(x=q_x, y=q_y, z=q_z, w=q_w) + dist = math.sqrt( + (goal_pos[0] - x) ** 2 + (goal_pos[1] - y) ** 2 + (goal_pos[2] - z) ** 2 + ) + + dist_ori = math.sqrt( + (goal_ori[0] - current_ori[0]) ** 2 + + (goal_ori[1] - current_ori[1]) ** 2 + + (goal_ori[2] - current_ori[2]) ** 2 + ) + + if dist < pos_tol and dist_ori < ori_tol: + print( + f"Vehicle reached final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + print(f"Final vehicle pose: ({x, y, z}), {current_ori}") + print(f"Euclidean error: {dist}, {dist_ori}") + rclpy.shutdown() + exit(0) + + print( + f"Vehicle did not reach final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + if x is not None: + print(f"Current_vehicle pose: ({x, y, z}), {current_ori}") + print( + f"Euclidean error: {dist if 'dist' in locals() else 'N/A'}, {dist_ori if 'dist_ori' in locals() else 'N/A'}" + ) + rclpy.shutdown() + exit(1) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/send_goal.py b/tests/simulator_tests/waypoint_manager_test/send_goal.py new file mode 100644 index 000000000..427bcb853 --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/send_goal.py @@ -0,0 +1,91 @@ +import random + +import rclpy +import yaml +from rclpy.action import ActionClient +from rclpy.node import Node +from vortex_msgs.action import ReferenceFilterWaypoint +from vortex_utils.python_utils import PoseData, euler_to_quat + + +def randomize_pose() -> PoseData: + pose: PoseData = PoseData() + pose.x = random.uniform(-10.0, 10.0) + pose.y = random.uniform(-10.0, 10.0) + pose.z = random.uniform(0.5, 3.0) + pose.roll = 0.0 + pose.pitch = random.uniform(-1.0, 1.0) + pose.yaw = random.uniform(-1.57, 1.57) + + return pose + + +class ReferenceFilterWaypointClient(Node): + def __init__(self): + super().__init__('reference_filter_waypoint_client') + + self._action_client = ActionClient( + self, ReferenceFilterWaypoint, '/orca/reference_filter' + ) + self.send_goal() + + def send_goal(self): + goal_pose = randomize_pose() + goal_msg = ReferenceFilterWaypoint.Goal() + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z + roll = goal_pose.roll + pitch = goal_pose.pitch + yaw = goal_pose.yaw + + quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) + + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] + + # Write goal pose to temp file + file_path = "goal_pose.yaml" + + data = { + "pos": [goal_pose.x, goal_pose.y, goal_pose.z], + "ori": [goal_pose.roll, goal_pose.pitch, goal_pose.yaw], + } + + with open(file_path, "w") as f: + yaml.safe_dump(data, f) + + # Send the goal asynchronously + self._action_client.wait_for_server(timeout_sec=10.0) + self.get_logger().info(f'Sending goal {goal_pose}...') + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result.success + self.get_logger().info(f'Goal result: {result}') + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + action_client = ReferenceFilterWaypointClient() + rclpy.spin(action_client) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/simulator_test.sh b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh new file mode 100755 index 000000000..1d726305f --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh @@ -0,0 +1,115 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + +# Get the directory of this script dynamically +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + # Safely kill any started PIDs (ignore empty values) + for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$FILTER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi + done + exit 1 +} +trap cleanup ERR + +setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +BAG_PID=$! +echo "Started bagging with PID: $BAG_PID" + +# Launch Stonefish Simulator +setsid ros2 launch stonefish_sim simulation.launch.py rendering:=false scenario:=orca_no_gpu & +SIM_PID=$! +echo "Launched simulator with PID: $SIM_PID" + +echo "Waiting for simulator to start..." +timeout 30s bash -c ' + while ! ros2 topic list | grep -q "/orca/odom"; do + sleep 1 + done || true' +echo "Simulator started" + +# Check for ROS errors in logs +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for odometry data +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Launch ORCA Simulation +setsid ros2 launch stonefish_sim orca_sim.launch.py & +ORCA_PID=$! +echo "Launched orca with PID: $ORCA_PID" + +setsid ros2 launch waypoint_manager waypoint_manager.launch.py & +WM_PID=$! +echo "Launched waypoint manager with PID: $WM_PID" + +echo "Waiting for sim interface to start..." +timeout 30s bash -c 'until ros2 topic list | grep -q "/orca/pose"; do sleep 1; done' +echo "Simulator started" + +# Check for ROS errors again +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for pose data +echo "Waiting for pose data..." +timeout 10s ros2 topic echo /orca/pose --once +echo "Got pose data" + +# Launch controller and reference filter +setsid ros2 launch auv_setup dp.launch.py & +CONTROLLER_PID=$! +echo "Launched controller and reference filter with PID: $CONTROLLER_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Set operation mode +echo "Turning off killswitch and setting operation mode to autonomous mode" +ros2 topic pub /orca/killswitch std_msgs/msg/Bool "{data: false}" -t 5 # Ensure the message arrives +ros2 topic pub /orca/operation_mode std_msgs/msg/String "{data: 'autonomous mode'}" -t 5 # Ensure the message arrives + +# Send waypoint goal +echo "Sending goal" +python3 "$SCRIPT_DIR/send_goal.py" + +# Check if goal reached +echo "Checking if goal reached" +python3 "$SCRIPT_DIR/check_goal.py" + +if [ $? -ne 0 ]; then + echo "Test failed: Vehicle did not reach final waypoint." + exit 1 +else + echo "Test passed: Vehicle reached final waypoint." +fi + +# Terminate processes (safely) +for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi +done + +echo "Test completed successfully." diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index bb04a7686..25cb3275d 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -33,19 +33,18 @@ def send_goal(self): goal_pose = randomize_pose() goal_msg = ReferenceFilterWaypoint.Goal() - goal_msg.goal.pose.position.x = goal_pose.x - goal_msg.goal.pose.position.y = goal_pose.y - goal_msg.goal.pose.position.z = goal_pose.z + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z roll = goal_pose.roll pitch = goal_pose.pitch yaw = goal_pose.yaw quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) - - goal_msg.goal.pose.orientation.x = quat[0] - goal_msg.goal.pose.orientation.y = quat[1] - goal_msg.goal.pose.orientation.z = quat[2] - goal_msg.goal.pose.orientation.w = quat[3] + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] # Write goal pose to temp file file_path = "goal_pose.yaml"