From 14a0ece7009e6d288419d7de71b10ee3551b5a03 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 26 Mar 2026 14:02:58 +0100 Subject: [PATCH 01/10] moved over pool_exploration --- .../pool_exploration/CMakeLists.txt | 96 +++++ .../subsea_docking/pool_exploration/README.md | 6 + .../config/pool_exploration.yaml | 55 +++ .../pool_exploration/pool_exploration.hpp | 283 +++++++++++++ .../pool_exploration/pool_exploration_ros.hpp | 274 ++++++++++++ .../include/pool_exploration/pool_grid.hpp | 60 +++ .../launch/pool_exploration.launch.py | 28 ++ .../pool_exploration/package.xml | 28 ++ .../pool_exploration/src/pool_exploration.cpp | 367 ++++++++++++++++ .../src/pool_exploration_ros.cpp | 398 ++++++++++++++++++ .../pool_exploration/src/pool_grid.cpp | 140 ++++++ .../pool_exploration/test/CMakeLists.txt | 20 + .../test/test_pool_exploration.cpp | 225 ++++++++++ .../pool_exploration/test/test_pool_map.cpp | 214 ++++++++++ 14 files changed, 2194 insertions(+) create mode 100644 mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt create mode 100644 mission/tacc/subsea_docking/pool_exploration/README.md create mode 100644 mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml create mode 100644 mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/launch/pool_exploration.launch.py create mode 100644 mission/tacc/subsea_docking/pool_exploration/package.xml create mode 100644 mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt create mode 100644 mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp create mode 100644 mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp diff --git a/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt new file mode 100644 index 0000000..ea47a87 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt @@ -0,0 +1,96 @@ +cmake_minimum_required(VERSION 3.8) +project(pool_exploration) + +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 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(message_filters REQUIRED) +find_package(visualization_msgs) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories(include) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +set(LIB_NAME ${PROJECT_NAME}_lib) +add_library(${LIB_NAME} SHARED + src/pool_exploration.cpp + src/pool_exploration_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + tf2_geometry_msgs + tf2_eigen + geometry_msgs + vortex_msgs + nav_msgs + vortex_utils + vortex_utils_ros + Eigen3 + spdlog + fmt + message_filters + #for testing: + visualization_msgs +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "PoolExplorationNode" + 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 + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/mission/tacc/subsea_docking/pool_exploration/README.md b/mission/tacc/subsea_docking/pool_exploration/README.md new file mode 100644 index 0000000..ab2c4ba --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/README.md @@ -0,0 +1,6 @@ +colcon build --packages-up-to pool_exploration --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1 + + +Utvide til venstre hjørne også + +Evt filtrere ut doble linjer tidligere diff --git a/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml b/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml new file mode 100644 index 0000000..e784a02 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Topics + line_sub_topic: "/line_detection/line_segments" + pose_sub_topic: "/pose" + sonar_info_sub_topic: "/moby/fls/sonar_info" + debug_topic: "/debug" + + # Publish + publish_rate_ms: 200 + + # Frames / TF + odom_frame: "moby/odom" + base_frame: "moby/base_link" + sonar_frame: "moby/sonar_link" + + # Corner/docking params + min_wall_distance_m: 0.5 + max_wall_distance_m: 10.0 + + far_wall_heading_angle_threshold: 1.6 # gammel? + parallel_heading_angle_threshold_rad: 0.4 + perpendicular_heading_angle_threshold_rad: 1.35 + + min_corner_angle_rad: 1.50 + max_corner_angle_rad: 1.70 + right_wall_offset_m: 2.5 #flytter langs høyre veggen + far_wall_offset_m: 4.0 #flytter langs far wall + + right_wall_max_y_m: 0.4 + far_wall_min_x_m: 0.5 + + right_dist: -0.1 # ENDRE NAVN ? + choose_right_corner: 1 + + # Waypoint + switching_threshold: 0.5 + overwrite_prior_waypoints: true + take_priority: true + +# FOR MAP LOGIC + map_pub_topic: "/map" + map_frame: "map" + + # Map / grid + size_x: 1000.0 + size_y: 1000.0 + resolution: 0.1 + frame_id: "map" + + # Map cells + unknown_cell: -1 + occupied_cell: 100 + + enu_to_ned: false \ No newline at end of file diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp new file mode 100644 index 0000000..af353dd --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp @@ -0,0 +1,283 @@ +#ifndef POOL_EXPLORATION_HPP +#define POOL_EXPLORATION_HPP + +#include +#include +#include +#include +#include + +namespace vortex::pool_exploration{ + +/** + * @brief Represents a candidate docking corner formed by a right wall and a far wall. + * + * A corner estimate consists of two detected wall segments and their intersection + * point in the planning frame. + */ +struct CornerEstimate { + /** @brief Line segment classified as the right wall. */ + vortex::utils::types::LineSegment2D right_wall; + + /** @brief Line segment classified as the far wall. */ + vortex::utils::types::LineSegment2D far_wall; + + /** @brief Intersection point between the two walls. */ + Eigen::Vector2f corner_point; +}; + +/** + * @brief Classification result for a detected wall segment. + */ +enum class WallClassification { + /** @brief Wall segment classified as the right wall. */ + RightWall, + + /** @brief Wall segment classified as the left wall. */ + LeftWall, + + /** @brief Wall segment classified as the far wall. */ + FarWall, + + /** @brief Wall segment does not satisfy any classification rule. */ + Rejected +}; + +/** + * @brief Convert a Point2D to an Eigen 2D vector. + * + * @param point Input point. + * @return Equivalent Eigen::Vector2f. + */ +inline Eigen::Vector2f to_eigen(const vortex::utils::types::Point2D point) { + return {point.x, point.y}; +} + +/** + * @brief Configuration parameters for the pool exploration planner. + * + * These parameters control wall classification, corner validation, and docking + * point estimation. + */ +struct PoolExplorationPlannerConfig { + /** @brief Minimum allowed distance from the drone to a candidate wall projection [m]. */ + float min_wall_distance_m; + + /** @brief Maximum allowed distance from the drone to a candidate wall projection [m]. */ + float max_wall_distance_m; + + // REMOVE THIS IF OTHER CODE WORKS, TO DO + float far_wall_heading_angle_threshold; + + /** @brief Maximum angle between heading and a wall for it to be considered parallel [rad]. */ + float parallel_heading_angle_threshold_rad; + + /** @brief Minimum angle between heading and a wall for it to be considered perpendicular [rad]. */ + float perpendicular_heading_angle_threshold_rad; + + /** @brief Minimum allowed angle between right wall and far wall for a valid corner [rad]. */ + float min_corner_angle_rad; + + /** @brief Maximum allowed angle between right wall and far wall for a valid corner [rad]. */ + float max_corner_angle_rad; + + // REMOVE THIS IF OTHER CODE WORKS, TO DO + float right_dist; + + /** @brief Maximum y-value used when checking whether a projection belongs to the right wall. */ + float right_wall_max_y_m; + + /** @brief Minimum x-value used when checking whether a projection belongs to the far wall. */ + float far_wall_min_x_m; + + /** @brief Offset from the right wall when estimating docking position [m]. */ + float right_wall_offset_m; + + /** @brief Offset from the far wall when estimating docking position [m]. */ + float far_wall_offset_m; + + /** @brief + * Selects whether to prefer the right-side corner in the mission logic. + */ + // HAS NOT BEEN IMPLEMENTED YET, TO DO + int choose_right_corner; +}; + +/** + * @brief Planner for detecting pool corners and estimating a docking position. + * + * The planner classifies detected line segments as candidate pool walls, combines + * compatible wall pairs into corner estimates, and computes a docking point from + * the selected corner. + */ +class PoolExplorationPlanner { +public: + /** + * @brief Construct a new planner instance. + * + * @param config Planner configuration parameters. + */ + PoolExplorationPlanner( + const PoolExplorationPlannerConfig& config); + + /** + * @brief Find all valid corner estimates from a set of detected line segments. + * + * Each line segment is first classified as a candidate wall. Candidate right walls + * and far walls are then paired, and their intersection is checked. A pair is + * accepted as a corner if the angle between the two walls lies within the configured + * corner-angle interval. + * + * @param lines Input line segments expressed in the planning frame. + * @param drone_pos Current drone position in the same frame as the lines. + * @param drone_heading Current drone heading [rad]. + * @return Vector of valid corner estimates. + */ + // EVT LAGE FOR VENSTRE VEGG, TO DO + std::vector find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& drone_pos, // sjekk at får inn pos og heading riktig? + float drone_heading) const; + + /** + * @brief Select the best corner estimate among all candidates. + * + * The current implementation selects the corner whose intersection point is + * closest to the drone. + * + * @param possible_corners All valid corner candidates. + * @param drone_pos Current drone position in the planning frame. + * @return Best corner estimate. + * + * @throws std::runtime_error if the input vector is empty. + // ENDRE UNNTAKSHÅNDTERING?, TO DO + */ + CornerEstimate select_best_corner( + const std::vector& possible_corners, + const Eigen::Vector2f& drone_pos) const; + + /** + * @brief Estimate a docking position from a validated corner estimate. + * + * The docking point is computed by offsetting the detected corner along the + * inward-facing normals of the right wall and far wall. + * + * @param estimated_corner Corner estimate containing the two wall segments and their intersection. + * @param drone_pos Current drone position in the same frame as the corner estimate. + * @return Estimated docking position in the planning frame. + */ + Eigen::Vector2f estimate_docking_position( + const CornerEstimate& estimated_corner, + const Eigen::Vector2f& drone_pos) const; + +private: + /** + * @brief Classify a line segment as a right wall, far wall, or rejected. + * + * A line is classified using: + * - the distance from the drone to the orthogonal projection on the line, + * - the position of that projection relative to the expected mission geometry, + * - and the unsigned angle between the wall direction and drone heading. + * + * Right-wall candidates must satisfy the configured right-side projection test + * and be approximately parallel to the drone heading. + * + * Far-wall candidates must satisfy the configured forward projection test + * and be approximately perpendicular to the drone heading. + * + * @param line Input line segment. + * @param drone_pos Current drone position. + * @param drone_heading Current drone heading [rad]. + * @return Wall classification result. + */ + WallClassification classify_wall( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos, + float drone_heading) const; + + /** + * @brief Project a point orthogonally onto the infinite line defined by a line segment. + * + * If the segment is degenerate, the first endpoint is returned. ANNEN HÅNDTERING? TO DO + * + * @param drone_pos Point to be projected. + * @param line Line segment defining the projection line. + * @return Projected point in the same frame. + */ + // PROJEKSJONSFORMELEN, TO DO + Eigen::Vector2f project_drone_onto_line( + const Eigen::Vector2f& drone_pos, // punktet som projiseres + const vortex::utils::types::LineSegment2D& line) const; + + /** + * @brief Compute the smallest unsigned angle between a wall segment and the drone heading. + * + * The wall direction is compared to the drone heading direction using the absolute + * dot product, making the result independent of the line endpoint ordering. + * + * The returned angle lies in [0, π/2]: + * - ≈ 0 rad → wall is parallel to heading + * - ≈ π/2 rad → wall is perpendicular to heading + * + * If the segment is degenerate (near-zero length), +∞ is returned. + * The dot product is clamped to [0, 1] to ensure numerical stability in acos(). + * + * @param line Wall segment. + * @param drone_heading Drone heading [rad]. + * @return Smallest unsigned angle between wall and heading [rad], or +∞ if degenerate. + */ + float angle_between_line_and_heading( + const vortex::utils::types::LineSegment2D& line, + float drone_heading) const; + + /** + * @brief Compute the intersection point of two infinite lines. + * + * The lines are defined by the endpoints of the two input line segments. + * The method returns false if the lines are parallel. + * + * @param line0 First line segment. + * @param line1 Second line segment. + * @param intersection_coordinates Output intersection point. + * @return true if the lines intersect. + * @return false if the lines are parallel. + */ + bool compute_line_intersection( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1, + Eigen::Vector2f& intersection_coordinates) const; + + /** + * @brief Compute the smallest angle between two line segments. + * + * The result is the unsigned smallest angle between the direction vectors of + * the two segments. + * + * @param line0 First line segment. + * @param line1 Second line segment. + * @return Angle between the two lines [rad]. + */ + float angle_between_lines( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1) const; + + /** + * @brief Compute the unit normal of a line pointing towards a reference point. + * + * This is used when computing a docking position offset from a detected wall. + * + * @param line Input line segment. + * @param drone_pos Reference point used to choose normal direction. + * @return Unit normal pointing towards the reference point. + */ + Eigen::Vector2f compute_normal_towards_point( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos) const; + + /** @brief Planner configuration. */ + PoolExplorationPlannerConfig config_; +}; + +} // namespace vortex::pool_exploration + +#endif // POOL_EXPLORATION_HPP diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp new file mode 100644 index 0000000..53cb4a5 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp @@ -0,0 +1,274 @@ +#ifndef POOL_EXPLORATION_ROS_HPP +#define POOL_EXPLORATION_ROS_HPP + +#include + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include +#include //til testing + +namespace vortex::pool_exploration{ + +/** + * @brief ROS 2 node for pool exploration and docking waypoint generation. + * + * This node provides the ROS interface for the pool exploration planner. + * It subscribes to detected sonar line segments, vehicle pose, and sonar + * image metadata, transforms the incoming line segments into the planning + * frame, estimates a valid pool corner through the planner, computes a + * docking position, and sends the resulting waypoint to the waypoint manager. + */ + +class PoolExplorationNode : public rclcpp::Node { + public: + /** + * @brief Construct a new PoolExplorationNode. + * + * The constructor initializes parameters, ROS interfaces, and the planner. + * + * @param options ROS node options. + */ + explicit PoolExplorationNode(const rclcpp::NodeOptions& options); + + /** @brief Default destructor. */ + ~PoolExplorationNode() = default; + + private: + // setup + + /** + * @brief Declare and load ROS parameters used by the node. + * + * This includes frame names, topic names, and waypoint-related parameters. + */ + void setup_parameters(); + + /** + * @brief Create subscribers, TF utilities, and service clients. + * + * This method initializes: + * - TF buffer and listener, + * - line, pose, and sonar-info subscriptions, + * - TF message filter for line detections, + * - waypoint service client. + */ + void setup_publishers_and_subscribers(); + + /** + * @brief Create and configure the pool exploration planner. + * + * Planner configuration values are loaded from ROS parameters and used to + * construct the internal planner instance. + */ + void setup_planner(); + + // callbacks + + /** + * @brief Callback for vehicle pose updates. + * + * Updates the internal vehicle state estimate used by the planner. + * + * @param msg Vehicle pose message with covariance. + */ + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg); + + /** + * @brief Callback for sonar image metadata updates. + * + * Stores the most recent sonar information needed to convert detected line + * endpoints from image pixels to metric sonar coordinates. + * + * @param msg Sonar metadata message. + */ + void sonar_info_callback( + const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg); + + /** + * @brief Callback for detected line segments. + * + * Triggered when a line segment message passes the TF message filter. The + * callback attempts to estimate a docking waypoint from the detected lines. + * + * @param msg Array of detected 2D line segments. + */ + void line_callback( + const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg); + + // helpers + + /** + * @brief Estimate and send a docking waypoint from detected line segments. + * + * The input segments are transformed into the planning frame, passed to the + * planner for corner detection, and then used to estimate a docking point. + * If a valid estimate is found, a waypoint is sent. + * + * @param msg Array of detected 2D line segments. + */ + void estimate_and_send_docking_waypoint(const vortex_msgs::msg::LineSegment2DArray& msg); + + /** + * @brief Transform sonar-detected 2D segments into a target frame. + * + * Each line endpoint is first converted from pixel coordinates to sonar + * metric coordinates using the latest sonar metadata. The resulting points + * are then transformed by the provided homogeneous transform. + * + * @param msg Input line segment array. + * @param T_target_src Homogeneous transform from source frame to target frame. + * @return Vector of transformed 2D line segments in the target frame. + */ + std::vector transform_segments_2d( + const vortex_msgs::msg::LineSegment2DArray& msg, + const Eigen::Matrix4f& T_target_src); + + /** + * @brief Send a docking waypoint to the waypoint manager. + * + * A single waypoint is created from the estimated docking position and sent + * through the SendWaypoints service. The request uses the configured waypoint + * control parameters. + * + * @param docking_estimate Estimated docking position in the planning frame. + */ + void send_docking_waypoint(const Eigen::Vector2f& docking_estimate); + + //TIL TESTING + void publish_docking_marker(const Eigen::Vector2f& docking); + + + /** @brief Topic name for detected sonar line segments. */ + std::string line_sub_topic_; + + /** @brief Topic name for vehicle pose estimates. */ + std::string pose_sub_topic_; + + /** @brief Topic name for sonar image metadata. */ + std::string sonar_info_sub_topic_; + + // TIL TESTING + std::string debug_topic_; + + + /** @brief Odometry frame used as planning/reference frame. */ + std::string odom_frame_; + + /** @brief Vehicle base frame name. */ + std::string base_frame_; + + /** @brief Sonar frame name. */ + std::string sonar_frame_; + + + /** @brief Distance threshold for switching to the next waypoint [m]. */ + float waypoint_switching_threshold_; + + /** @brief Whether newly sent waypoints should overwrite previously queued waypoints. */ + bool waypoint_overwrite_prior_; + + /** @brief Whether the sent waypoint should take priority in the waypoint manager. */ + bool waypoint_take_priority_; + + + /** @brief True once a docking waypoint has been sent. */ + bool waypoint_sent_{false}; + + /** @brief Latest known vehicle pose and heading used by the planner. */ + vortex::utils::types::PoseEuler drone_state_; + + /** @brief Most recent sonar metadata message. */ + vortex_msgs::msg::SonarInfo::ConstSharedPtr latest_sonar_info_; + + + /** @brief TF buffer used for transform lookup. */ + std::shared_ptr tf_buffer_; + + /** @brief TF listener attached to the TF buffer. */ + std::shared_ptr tf_listener_; + + + /** @brief Subscriber for line segment detections. */ + message_filters::Subscriber line_sub_; + + /** + * @brief TF-aware message filter for line segment detections. + * + * Ensures that line messages are only forwarded once the required transform + * into the target frame is available. + */ + std::shared_ptr> line_filter_; + + /** @brief Subscriber for vehicle pose updates. */ + rclcpp::Subscription::SharedPtr pose_sub_; + + /** @brief Subscriber for sonar metadata updates. */ + rclcpp::Subscription::SharedPtr sonar_info_sub_; + + + /** @brief Client for sending waypoint requests to the waypoint manager. */ + rclcpp::Client::SharedPtr waypoint_client_; + + + /** @brief Pool exploration planner used for corner detection and docking estimation. */ + std::unique_ptr planner_; + + // Debug visualization + rclcpp::Publisher::SharedPtr docking_marker_pub_; //til testing +}; + +// GRID LOGIKK TIL SENERE +# if 0 +#include + + private: + + std::string map_pub_topic_; + std::string map_frame_; + + std::shared_ptr tf_broadcaster_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + std::chrono::milliseconds pub_dt_{200}; + + void timer_callback(); + // Lager en transformasjon TF map->odom + geometry_msgs::msg::TransformStamped compute_map_odom_transform(); + + void publish_grid(); + + // Logikk til senere bruk: + void draw_segments_in_map_frame(const vortex_msgs::msg::LineSegment2DArray& msg); + + // publisher + rclcpp::Publisher::SharedPtr + map_pub_; + + // map + PoolExplorationPlanner map_; + +#endif +} // namespace vortex::pool_exploration + +#endif // POOL_EXPLORATION_ROS_HPP + diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp new file mode 100644 index 0000000..614db42 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp @@ -0,0 +1,60 @@ +#ifndef POOL_EXPLORATION_HPP +#define POOL_EXPLORATION_HPP + +#include +#include + +#include +#include + +#include +#include + +namespace vortex::pool_exploration{ +class PoolExplorationMap { +public: + // Konstruktør + PoolExplorationMap( + double size_x, + double size_y, + double resolution, + const std::string& frame_id); + + const nav_msgs::msg::OccupancyGrid& grid() const; + + /** + * @brief Draws a straight line on the occupancy grid using Bresenham's algorithm. + * Reference: https://www.youtube.com/watch?v=CceepU1vIKo + */ + // denne kalles fra ros noden + // fra map til celle (forklar mer hvordan) + void insert_line_in_grid(const std::vector& segments); // NB: har endret til utils type linesegment, har ikke asEigen funksjonen + +private: + //Occupancy grid + // Set all cells unknown, + // origin in center of map with no rotation + void initialize_grid(); + + //brukes i insertSegmentsMapFrame + // bresenham line algoritm (referanse) + void bresenham_line_algoritm(int x0, int y0, int x1, int y1); + + // brukes i bresenhamLineAlgoritm + // sjekke at koordinatene er innenfor mappet + void set_grid_cell(int x, int y, int value); + + nav_msgs::msg::OccupancyGrid grid_; + std::string frame_id; + double size_x_; + double size_y_; + double resolution_; + + int8_t occupied_cell_; + int8_t unknown_cell_; + +}; + +} // namespace vortex::pool_exploration + +#endif // POOL_EXPLORATION_HPP \ No newline at end of file diff --git a/mission/tacc/subsea_docking/pool_exploration/launch/pool_exploration.launch.py b/mission/tacc/subsea_docking/pool_exploration/launch/pool_exploration.launch.py new file mode 100644 index 0000000..4ace143 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/launch/pool_exploration.launch.py @@ -0,0 +1,28 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory("pool_exploration"), + "config", + "pool_exploration.yaml", + ) + + return LaunchDescription( + [ + Node( + package='pool_exploration', + executable='pool_exploration_node', + name='pool_exploration', + output='screen', + parameters=[ + config, + {'use_sim_time': True}, + ], + ), + ] + ) diff --git a/mission/tacc/subsea_docking/pool_exploration/package.xml b/mission/tacc/subsea_docking/pool_exploration/package.xml new file mode 100644 index 0000000..3856ba9 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/package.xml @@ -0,0 +1,28 @@ + + + + pool_exploration + 0.0.0 + TODO: Package description + sina + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + tf2_geometry_msgs + tf2_eigen + eigen + vortex_utils + vortex_utils_ros + vortex_msgs + rclcpp_components + message_filters + visualization_msgs + + + ament_cmake + + diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp new file mode 100644 index 0000000..493ce9b --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp @@ -0,0 +1,367 @@ +#include +#include +#include // til testing + +#include +#include +#include + +namespace vortex::pool_exploration{ + +PoolExplorationPlanner::PoolExplorationPlanner(const PoolExplorationPlannerConfig& config) + : config_(config) {} + +# if 1 +std::vector PoolExplorationPlanner::find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& drone_pos, + float drone_heading) const { + std::vector right_wall_candidates; + std::vector far_wall_candidates; + std::vector corner_estimates; + + for (const auto& line : lines) { + const WallClassification classification = classify_wall(line, drone_pos, drone_heading); + + if (classification == WallClassification::RightWall) { + right_wall_candidates.push_back(line); + } + else if (classification == WallClassification::FarWall) { + far_wall_candidates.push_back(line); + } + } + + spdlog::info("==== SUMMARY ===="); + spdlog::info("Right wall candidates: {}", right_wall_candidates.size()); + spdlog::info("Far wall candidates: {}", far_wall_candidates.size()); + + for (const auto& right_wall : right_wall_candidates) { + for (const auto& far_wall : far_wall_candidates) { + Eigen::Vector2f corner_intersection; + if (!compute_line_intersection(right_wall, far_wall, corner_intersection)) { + continue; + } + + const float wall_angle = angle_between_lines(right_wall, far_wall); + + if (wall_angle >= config_.min_corner_angle_rad && + wall_angle <= config_.max_corner_angle_rad) { + corner_estimates.push_back({right_wall, far_wall, corner_intersection}); + } + } + } + + return corner_estimates; +} +#endif + +CornerEstimate PoolExplorationPlanner::select_best_corner( + const std::vector& possible_corners, + const Eigen::Vector2f& drone_pos) const { + // if (possible_corners.empty()) {throw std::runtime_error("No candidate corners available"); } // UNNTAKSHÅNDTERING I ROS I STEDET + + float min_distance = std::numeric_limits::max(); + CornerEstimate best_corner = possible_corners.front(); + + for (const auto& corner : possible_corners) { + float distance = (corner.corner_point - drone_pos).squaredNorm(); + + if (distance < min_distance) { + min_distance = distance; + best_corner = corner; + } + } + return best_corner; +} + +Eigen::Vector2f PoolExplorationPlanner::estimate_docking_position( + const CornerEstimate& estimated_corner, + const Eigen::Vector2f& drone_pos) const { + Eigen::Vector2f right_normal = compute_normal_towards_point(estimated_corner.right_wall, drone_pos); + Eigen::Vector2f far_normal = compute_normal_towards_point(estimated_corner.far_wall, drone_pos); + + Eigen::Vector2f docking_estimate = estimated_corner.corner_point + + right_normal * config_.right_wall_offset_m + + far_normal * config_.far_wall_offset_m; + + return docking_estimate; +} + +WallClassification PoolExplorationPlanner::classify_wall( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos, + float drone_heading) const { + const Eigen::Vector2f p0 = to_eigen(line.p0); + const Eigen::Vector2f p1 = to_eigen(line.p1); + + const Eigen::Vector2f dir = p1 - p0; + if (dir.squaredNorm() < 1e-6f) { // ENDRE KRAV TIL LENGDE PÅ LINJE? + spdlog::info(" -> REJECTED: degenerate line"); + return WallClassification::Rejected; + } + // TIL LOGGING + spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", + p0.x(), p0.y(), p1.x(), p1.y()); + // + const Eigen::Vector2f projection = project_drone_onto_line(drone_pos, line); + const Eigen::Vector2f rel = projection - drone_pos; + + const float distance = rel.norm(); + // TIL LOGGING + spdlog::info(" Projection: ({:.2f}, {:.2f})", projection.x(), projection.y()); + spdlog::info(" Rel: ({:.2f}, {:.2f}), dist: {:.2f}", + rel.x(), rel.y(), distance); + // + if (distance < config_.min_wall_distance_m || + distance > config_.max_wall_distance_m) { + spdlog::info(" -> REJECTED by distance"); + return WallClassification::Rejected; + } + + const float heading_wall_angle = angle_between_line_and_heading(line, drone_heading); + spdlog::info(" Angle: {:.2f} rad ({:.1f} deg)", + heading_wall_angle, heading_wall_angle * 180.0 / M_PI); + + Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); + Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); + float forward_dist = rel.dot(forward); + float right_dist = rel.dot(right); + + // RIGHT WALL: projection has negative y-value in NED, wall is approximately parallel to heading + if (//projection.y() < config_.right_wall_max_y_m && + right_dist <0 && + heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { + spdlog::info(" -> Classified as RIGHT candidate"); + return WallClassification::RightWall; + } + + // FAR WALL: projection has positive x-value, wall is approximately perpendicular to heading + if (//projection.x() > config_.far_wall_min_x_m && + forward_dist > 0 && + heading_wall_angle > config_.perpendicular_heading_angle_threshold_rad) { + spdlog::info(" -> Classified as FAR candidate"); + return WallClassification::FarWall; + } + spdlog::info(" -> REJECTED by geometry"); + return WallClassification::Rejected; +} + +Eigen::Vector2f PoolExplorationPlanner::project_drone_onto_line( + const Eigen::Vector2f& drone_pos, + const vortex::utils::types::LineSegment2D& line) const { + const Eigen::Vector2f p0 = to_eigen(line.p0); + const Eigen::Vector2f p1 = to_eigen(line.p1); + + const Eigen::Vector2f line_direction = p1 - p0; + const float line_length_squared = line_direction.squaredNorm(); + + const float projection_parameter = (drone_pos - p0).dot(line_direction) / line_length_squared; + const Eigen::Vector2f projection_point = p0 + projection_parameter * line_direction; + + return projection_point; +} + +float PoolExplorationPlanner::angle_between_line_and_heading( + const vortex::utils::types::LineSegment2D& line, + float drone_heading) const { + Eigen::Vector2f wall_direction = to_eigen(line.p1) - to_eigen(line.p0); + const float wall_length = wall_direction.norm(); + + if (wall_length < 1e-6f) { + return std::numeric_limits::infinity(); + } + + wall_direction /= wall_length; + Eigen::Vector2f heading_direction( + std::cos(drone_heading), + std::sin(drone_heading)); + + float cos_heading_wall_angle = + std::abs(wall_direction.dot(heading_direction)); //rekkefølge p0 og p1 urelevant + cos_heading_wall_angle = std::clamp(cos_heading_wall_angle, 0.0f, 1.0f); // unngå små avvik + + return std::acos(cos_heading_wall_angle); +} + +bool PoolExplorationPlanner::compute_line_intersection( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1, + Eigen::Vector2f& intersection) const { + const Eigen::Vector2f p00 = to_eigen(line0.p0); + const Eigen::Vector2f p01 = to_eigen(line0.p1); + const Eigen::Vector2f p10 = to_eigen(line1.p0); + const Eigen::Vector2f p11 = to_eigen(line1.p1); + + float determinant = (p00.x()-p01.x())*(p10.y()-p11.y()) - (p00.y()-p01.y())*(p10.x()-p11.x()); + + if (std::abs(determinant) < 1e-6f) + return false; // parallelle linjer + + intersection.x() = ((p00.x()*p01.y()-p00.y()*p01.x())*(p10.x()-p11.x()) - + (p00.x()-p01.x())*(p10.x()*p11.y()-p10.y()*p11.x()))/determinant; + intersection.y() = ((p00.x()*p01.y()-p00.y()*p01.x())*(p10.y()-p11.y()) - + (p00.y()-p01.y())*(p10.x()*p11.y()-p10.y()*p11.x()))/determinant; + return true; +} + +float PoolExplorationPlanner::angle_between_lines( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1) const { + + Eigen::Vector2f line0_direction = + to_eigen(line0.p1) - to_eigen(line0.p0); + + Eigen::Vector2f line1_direction = + to_eigen(line1.p1) - to_eigen(line1.p0); + + const float line0_length = line0_direction.norm(); + const float line1_length = line1_direction.norm(); + + if (line0_length < 1e-6f || line1_length < 1e-6f) { + return std::numeric_limits::infinity(); + } + + line0_direction /= line0_length; + line1_direction /= line1_length; + + float cos_angle_between_lines = + std::abs(line0_direction.dot(line1_direction)); + + cos_angle_between_lines = + std::clamp(cos_angle_between_lines, 0.0f, 1.0f); + + return std::acos(cos_angle_between_lines); +} + +Eigen::Vector2f PoolExplorationPlanner::compute_normal_towards_point( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos) const +{ + Eigen::Vector2f p0 = to_eigen(line.p0); + Eigen::Vector2f p1 = to_eigen(line.p1); + + Eigen::Vector2f line_direction = p1 - p0; + + if (line_direction.x() < 0.0f || + (line_direction.x() == 0.0f && line_direction.y() < 0.0f)) { + std::swap(p0, p1); + line_direction = p1 - p0; + } + + const float line_length = line_direction.norm(); + if (line_length < 1e-6f) { + return Eigen::Vector2f::Zero(); + } + + line_direction /= line_length; + + Eigen::Vector2f normal(-line_direction.y(), line_direction.x()); + + const Eigen::Vector2f line_midpoint = 0.5f * (p0 + p1); + const Eigen::Vector2f midpoint_to_drone = drone_pos - line_midpoint; + + if (midpoint_to_drone.dot(normal) < 0.0f) { + normal = -normal; + } + + return normal; +} + +// klassifisere hjørne utfra relativ avstand til drone +# if 0 +std::vector PoolExplorationPlanner::find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& drone_pos, + float drone_heading) { + + std::vector right_candidates; + std::vector left_candidates; + std::vector far_candidates; + std::vector potential_corners; + + // Finner ut hva som er foran ogsånn? (?) + Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); + Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); // RIKTIG NED? + + for (const auto& line : lines) { + // const auto eigen_line = line.asEigen(); + const Eigen::Vector2f p0 = to_eigen(line.p0); + const Eigen::Vector2f p1 = to_eigen(line.p1); + + spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", + p0.x(), p0.y(), p1.x(), p1.y()); + + Eigen::Vector2f projection = project_point_onto_line(drone_pos, line); + Eigen::Vector2f rel = projection - drone_pos; + + float dist = rel.norm(); + float angle = angle_between_line_and_heading(line, drone_heading); + + float forward_dist = rel.dot(forward); + float right_dist = rel.dot(right); + + spdlog::info(" Projection: ({:.2f}, {:.2f})", + projection.x(), projection.y()); + + spdlog::info(" Rel: ({:.2f}, {:.2f}), dist: {:.2f}", + rel.x(), rel.y(), dist); + + spdlog::info(" Angle: {:.2f} rad ({:.1f} deg)", + angle, angle * 180.0 / M_PI); + + spdlog::info(" forward_dist: {:.2f}, right_dist: {:.2f}", + forward_dist, right_dist); + + spdlog::info("Angle: {:.2f}, right_dist: {:.2f}, forward_dist: {:.2f}", + angle, right_dist, forward_dist); + + if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle < config_.far_wall_heading_angle_threshold && + right_dist > config_.right_dist + ) { + spdlog::info(" → Classified as RIGHT candidate"); + right_candidates.push_back(line); + } + else if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle > config_.far_wall_heading_angle_threshold && + forward_dist > 0.0f + ) { + spdlog::info(" → Classified as FAR candidate"); + far_candidates.push_back(line); + } + + else if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle < config_.far_wall_heading_angle_threshold && + right_dist < 0.0f + ) { + spdlog::info(" → Classified as LEFT candidate"); + left_candidates.push_back(line); + } + else { + spdlog::info(" → REJECTED by angle"); + } + } + spdlog::info("==== SUMMARY ===="); + spdlog::info("Right candidates: {}", right_candidates.size()); + spdlog::info("Far candidates: {}", far_candidates.size()); + + for (const auto& right_wall : right_candidates) { + for (const auto& far_wall : far_candidates) { + Eigen::Vector2f intersection_coordinates{}; + bool intersect = compute_line_intersection(right_wall, far_wall, intersection_coordinates); + if (intersect){ + float wall_angle = angle_between_lines(right_wall, far_wall); + if (wall_angle >= config_.min_corner_angle_rad && wall_angle <= config_.max_corner_angle_rad) { + potential_corners.push_back({right_wall, far_wall, intersection_coordinates}); + } + } + } + } + return potential_corners; +} +#endif + +} // namespace vortex::pool_exploration diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp new file mode 100644 index 0000000..a5af8e0 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp @@ -0,0 +1,398 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include + + +#include +// #include +#include //ERSTATTE MED VORTEX? ^^^ +#include +#include +#include "pool_exploration/pool_exploration.hpp" + +namespace vortex::pool_exploration{ + +PoolExplorationNode::PoolExplorationNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("pool_exploration_node", options) { + setup_parameters(); + setup_publishers_and_subscribers(); + setup_planner(); + } + +void PoolExplorationNode::setup_parameters() +{ + // Frames + odom_frame_ = this->declare_parameter("odom_frame"); + base_frame_ = this->declare_parameter("base_frame"); + sonar_frame_ = this->declare_parameter("sonar_frame"); + + // Topics + line_sub_topic_ = this->declare_parameter("line_sub_topic"); + pose_sub_topic_ = this->declare_parameter("pose_sub_topic"); + sonar_info_sub_topic_ = this->declare_parameter("sonar_info_sub_topic"); + debug_topic_ = this->declare_parameter("debug_topic"); //For testing + + // Waypoint parameters from YAML + waypoint_switching_threshold_ = this->declare_parameter("switching_threshold"); + waypoint_overwrite_prior_ = this->declare_parameter("overwrite_prior_waypoints"); + waypoint_take_priority_ = this->declare_parameter("take_priority"); +} + +void PoolExplorationNode::setup_publishers_and_subscribers() { + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + const auto qos_sub = rclcpp::SensorDataQoS(); //standard for sensordata, ta inn vortex sin i stedet?? + auto sub_options = rclcpp::SubscriptionOptions(); + + line_sub_.subscribe( + this, line_sub_topic_, + qos_sub.get_rmw_qos_profile(), sub_options); + + pose_sub_ = this->create_subscription( + pose_sub_topic_, qos_sub, + std::bind(&PoolExplorationNode::pose_callback, this, std::placeholders::_1)); + + sonar_info_sub_ = this->create_subscription( + sonar_info_sub_topic_, qos_sub, + [this](const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg) { + this->sonar_info_callback(msg); + }); + + line_filter_ = std::make_shared>( + line_sub_, *tf_buffer_, odom_frame_, 10, + this->get_node_logging_interface(), + this->get_node_clock_interface()); + + line_filter_->registerCallback( + std::bind(&PoolExplorationNode::line_callback, this, std::placeholders::_1)); + + waypoint_client_ = this->create_client("/send_waypoints"); //endre /send_waypoints navnet + + docking_marker_pub_ = this->create_publisher(debug_topic_, 10); //til testing +} + +void PoolExplorationNode::setup_planner() { + PoolExplorationPlannerConfig config{}; + + config.min_wall_distance_m = this->declare_parameter("min_wall_distance_m"); + config.max_wall_distance_m = this->declare_parameter("max_wall_distance_m"); + config.far_wall_heading_angle_threshold = this->declare_parameter("far_wall_heading_angle_threshold"); + config.parallel_heading_angle_threshold_rad = this->declare_parameter("parallel_heading_angle_threshold_rad"); + config.perpendicular_heading_angle_threshold_rad = this->declare_parameter("perpendicular_heading_angle_threshold_rad"); + + config.min_corner_angle_rad = this->declare_parameter("min_corner_angle_rad"); + config.max_corner_angle_rad = this->declare_parameter("max_corner_angle_rad"); + config.right_dist = this->declare_parameter("right_dist"); + config.far_wall_min_x_m = this->declare_parameter("far_wall_min_x_m"); + config.right_wall_max_y_m = this->declare_parameter("right_wall_max_y_m"); + config.choose_right_corner = this->declare_parameter("choose_right_corner"); + config.right_wall_offset_m = this->declare_parameter("right_wall_offset_m"); + config.far_wall_offset_m = this->declare_parameter("far_wall_offset_m"); + + planner_ = std::make_unique(config); +} + +void PoolExplorationNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg) { + drone_state_.x = msg->pose.pose.position.x; + drone_state_.y = msg->pose.pose.position.y; + drone_state_.z = msg->pose.pose.position.z; + + Eigen::Quaterniond q; + tf2::fromMsg(msg->pose.pose.orientation, q); + Eigen::Vector3d rpy = vortex::utils::math::quat_to_euler(q); + drone_state_.yaw = rpy.z(); +} + +void PoolExplorationNode::sonar_info_callback( + const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg) { + latest_sonar_info_ = msg; +} + +void PoolExplorationNode::line_callback( + const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg) { + + estimate_and_send_docking_waypoint(*msg); + //drawSegmentsInMapFrame(*msg); +} + +void PoolExplorationNode::estimate_and_send_docking_waypoint( + const vortex_msgs::msg::LineSegment2DArray& msg) { + geometry_msgs::msg::TransformStamped tf_stamped; + + try { + tf_stamped = tf_buffer_->lookupTransform( + odom_frame_, msg.header.frame_id, msg.header.stamp); + } catch (const tf2::TransformException& ex) { + spdlog::warn("[PoolExploration] TF failed {} -> {}: {}", + msg.header.frame_id, odom_frame_, ex.what()); + return; + } + + const Eigen::Affine3d T = tf2::transformToEigen(tf_stamped.transform); + const Eigen::Matrix4f T_odom_src = T.matrix().cast(); + + auto segs = transform_segments_2d(msg, T_odom_src); //NB ENDRE NAVN <3 + + Eigen::Vector2f drone_pos = {drone_state_.x,drone_state_.y}; + float drone_heading = drone_state_.yaw; + + auto corners = planner_->find_corner_estimates( + segs, + drone_pos, + drone_heading + ); + + if (corners.empty()) { + spdlog::info("[PoolExploration] No valid corners -> no docking estimate"); + return; + } + + CornerEstimate best_corner = planner_->select_best_corner(corners, drone_pos); + + Eigen::Vector2f docking = planner_->estimate_docking_position(best_corner, drone_pos); + + publish_docking_marker(docking); //til testing + send_docking_waypoint(docking); + + spdlog::info("[PoolExploration] Docking estimate (odom): x={} y={}", + docking.x(), docking.y()); +} + +std::vector PoolExplorationNode::transform_segments_2d( + const vortex_msgs::msg::LineSegment2DArray& msg, + const Eigen::Matrix4f& T_target_src) +{ + std::vector segments; + segments.reserve(msg.lines.size()); + + if (!latest_sonar_info_) { + spdlog::warn("[PoolExploration] No sonar image info available yet"); + return segments; + } + + vortex::utils::types::SonarInfo sonar_info; + sonar_info.meters_per_pixel_x = latest_sonar_info_->meters_per_pixel_x; + sonar_info.meters_per_pixel_y = latest_sonar_info_->meters_per_pixel_y; + sonar_info.image_width = latest_sonar_info_->width; + sonar_info.image_height = latest_sonar_info_->height; + + for (const auto& line : msg.lines) { + const auto p0 = sonar_info.pixel_index_to_sonar_metric(line.p0.x, line.p0.y); + const auto p1 = sonar_info.pixel_index_to_sonar_metric(line.p1.x, line.p1.y); + + const Eigen::Vector4f p0_sonar( + static_cast(p0.y), + static_cast(p0.x), + 0.0f, + 1.0f + ); + const Eigen::Vector4f p1_sonar( + static_cast(p1.y), + static_cast(p1.x), + 0.0f, + 1.0f + ); + + const Eigen::Vector4f p0_target = T_target_src * p0_sonar; + const Eigen::Vector4f p1_target = T_target_src * p1_sonar; + + vortex::utils::types::LineSegment2D seg; + seg.p0 = {p0_target.x(), p0_target.y()}; + seg.p1 = {p1_target.x(), p1_target.y()}; + + spdlog::info("[PoolExploration] Line received: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", // til debugging + seg.p0.x, seg.p0.y, seg.p1.x, seg.p1.y); + + segments.push_back(seg); + } + return segments; +} + +void PoolExplorationNode::send_docking_waypoint(const Eigen::Vector2f& docking_estimate) +{ + if (waypoint_sent_) { + return; + } + + if (!waypoint_client_->service_is_ready()) { + spdlog::warn("Waypoint service not available"); + return; + } + + auto request = std::make_shared(); + vortex_msgs::msg::Waypoint wp; + + wp.pose.position.x = docking_estimate.x(); + wp.pose.position.y = docking_estimate.y(); + wp.pose.position.z = 0.0f; //skal være 0? + + wp.pose.orientation.x = 0.0f; // Riktig orientation? + wp.pose.orientation.y = 0.0f; + wp.pose.orientation.z = 0.0f; + wp.pose.orientation.w = 1.0f; + + request->waypoints.push_back(wp); + request->switching_threshold = waypoint_switching_threshold_; + request->overwrite_prior_waypoints = waypoint_overwrite_prior_; + request->take_priority = waypoint_take_priority_; + + waypoint_client_->async_send_request(request); + waypoint_sent_ = true; + spdlog::info("Docking waypoint sent to WaypointManager: x={} y={}", docking_estimate.x(), docking_estimate.y()); +} + +void PoolExplorationNode::publish_docking_marker(const Eigen::Vector2f& docking) //til testing +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = odom_frame_; + marker.header.stamp = this->now(); + + marker.ns = "docking_debug"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = docking.x(); + marker.pose.position.y = docking.y(); + marker.pose.position.z = 0.0; + + marker.pose.orientation.w = 1.0; + + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + docking_marker_pub_->publish(marker); +} + +// Grid logikk +# if 0 + +//INKLUDER DISSE NÅR SLÅES SAMMEN IGJEN +void PoolExplorationNode::setup_publishers_and_subscribers() { + //map_frame_ = this->declare_parameter("map_frame", "map"); //allerede definert + //pub_dt_ = std::chrono::milliseconds( + // this->declare_parameter("publish_rate_ms")); + + //this->declare_parameter("enu_to_ned", false); + + //const std::string map_pub_topic = + // this->declare_parameter("map_pub_topic", "/map"); + + //tf_broadcaster_ = std::make_shared(this); + //const auto map_to_odom_tf = compute_map_odom_transform(); //const? + //tf_broadcaster_->sendTransform(map_to_odom_tf); + + //const auto map_qos = rclcpp::QoS(rclcpp::KeepLast(1)) //bare siste kart + // .reliable() //meldinger må leveres + // .transient_local(); //subscribers fra siste melding + + //map_pub_ = this->create_publisher( + // map_pub_topic, + // map_qos); + //timer_ = this->create_wall_timer(pub_dt_, [this]() { this->timer_callback(); }); +} + +//Konstruktøren +PoolExplorationNode::PoolExplorationNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("pool_exploration_node", options), + size_x_(this->declare_parameter("size_x")), + size_y_(this->declare_parameter("size_y")), + resolution_(this->declare_parameter("resolution")), + map_(size_x_, size_y_, resolution_, map_frame_) { + setup_publishers_and_subscribers(); +} + +geometry_msgs::msg::TransformStamped PoolExplorationNode::compute_map_odom_transform() { + geometry_msgs::msg::TransformStamped map_to_odom; + map_to_odom.header.stamp = this->get_clock()->now(); + map_to_odom.header.frame_id = map_frame_; + map_to_odom.child_frame_id = odom_frame_; + + // dette blir i midten av kartet + map_to_odom.transform.translation.x = 0.0; + map_to_odom.transform.translation.y = 0.0; + map_to_odom.transform.translation.z = 0.0; + + if (this->get_parameter("enu_to_ned").as_bool()) { + tf2::Quaternion q1; + q1.setRPY(M_PI, 0.0, M_PI_2); + map_to_odom.transform.rotation.w = q1.w(); + map_to_odom.transform.rotation.x = q1.x(); + map_to_odom.transform.rotation.y = q1.y(); + map_to_odom.transform.rotation.z = q1.z(); + } else { + //Kjøres dersom vi er i ENU frame + tf2::Quaternion q2; + q2.setRPY(0.0, 0.0, 0.0); + map_to_odom.transform.rotation.w = q2.w(); + map_to_odom.transform.rotation.x = q2.x(); + map_to_odom.transform.rotation.y = q2.y(); + map_to_odom.transform.rotation.z = q2.z(); + } + + return map_to_odom; +} + +// VENTE MED DENNE LOGIKK TIL SENERE +void PoolExplorationNode::draw_segments_in_map_frame( + const vortex_msgs::msg::LineSegment2DArray& msg) +{ + geometry_msgs::msg::TransformStamped tf_stamped; + + try { + tf_stamped = tf_buffer_->lookupTransform( + map_frame_, msg.header.frame_id, msg.header.stamp); + } catch (const tf2::TransformException& ex) { + spdlog::warn("[PoolExploration] TF failed {} -> {}: {}", + msg.header.frame_id, map_frame_, ex.what()); + return; + } + + const Eigen::Affine3d T = tf2::transformToEigen(tf_stamped.transform); + const Eigen::Matrix4f T_map_src = T.matrix().cast(); + + //se nærmere på denne transform logikken + auto segs = transform_segments_2d(msg, T_map_src); + // bruker logikk fra pool_exploration.h + + map_.insert_line_in_grid(segs); + + spdlog::info("[PoolExploration] {} line segments drawn in map", segs.size()); +} +// LOGIKK FOR GRIDDET +void PoolExplorationNode::publish_grid() { + nav_msgs::msg::OccupancyGrid msg = map_.grid(); + + msg.header.frame_id = map_frame_; + msg.header.stamp = this->get_clock()->now(); + + map_pub_->publish(msg); +} + +void PoolExplorationNode::timer_callback() { + publish_grid(); +} +#endif + +RCLCPP_COMPONENTS_REGISTER_NODE(PoolExplorationNode) + +} // namespace vortex::pool_exploration diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp new file mode 100644 index 0000000..4025164 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp @@ -0,0 +1,140 @@ +#include +#include +#include +#include // til testing + +#include + +namespace vortex::pool_exploration{ + +// Lage egen config til griddet TO DO +PoolExplorationMap::PoolExplorationMap( + double size_x, + double size_y, + double resolution, + const std::string& frame_id) + : size_x_(size_x), + size_y_(size_y), + resolution_(resolution) { + grid_.header.frame_id = frame_id; + initialize_grid(); +} + +void PoolExplorationMap::initialize_grid() { + int width = static_cast(size_x_ / resolution_); + int height = static_cast(size_y_ / resolution_); + + grid_.info.resolution = resolution_; + grid_.info.width = width; + grid_.info.height = height; + + grid_.info.origin.position.x = -size_x_ / 2.0; + grid_.info.origin.position.y = -size_y_ / 2.0; + grid_.info.origin.orientation.w = 1.0; + + grid_.data.assign(width * height, unknown_cell_); +} + +const nav_msgs::msg::OccupancyGrid& PoolExplorationMap::grid() const { + return grid_; +} + +void PoolExplorationMap::insert_line_in_grid( + const std::vector& segments) { + for (const auto& seg : segments) { + + int x0 = static_cast((seg.p0.x - grid_.info.origin.position.x) / grid_.info.resolution); + int y0 = static_cast((seg.p0.y - grid_.info.origin.position.y) / grid_.info.resolution); + int x1 = static_cast((seg.p1.x - grid_.info.origin.position.x) / grid_.info.resolution); + int y1 = static_cast((seg.p1.y - grid_.info.origin.position.y) / grid_.info.resolution); + + bresenham_line_algoritm(x0, y0, x1, y1); + } +} + +void PoolExplorationMap::bresenham_line_algoritm( + int x0, int y0, int x1, int y1) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (!steep){ + if (x0 > x1){ + std::swap(x0, x1); + std::swap(y0,y1); + } + + int dx = x1 - x0; + int dy = y1 - y0; + + int dir = (dy <0) ? -1 : 1; + dy = std::abs(dy); + + int y = y0; + int p = 2 * dy - dx; + + for (int i = 0; i <= dx; i++) { + set_grid_cell(x0 + i, y, occupied_cell_); // må declare occupied_cell_ i ros senere + + if (p >= 0) { + y += dir; + p -= 2 * dx; + } + p += 2 * dy; + } + } else { + if (y0 > y1){ + std::swap(x0, x1); + std::swap(y0,y1); + } + + int dx = x1 - x0; + int dy = y1 - y0; + + int dir = (dx <0) ? -1 : 1; + dx = std::abs(dx); + + int x = x0; + int p = 2 * dx - dy; + + for (int i = 0; i <= dy; i++) { + set_grid_cell(x, y0 + i, occupied_cell_); + + if (p >= 0) { + x += dir; + p -= 2 * dy; + } + p += 2 * dx; + } + } +} + +void PoolExplorationMap::set_grid_cell( + int x, int y, int value) { + if (x < 0 || x >= static_cast(grid_.info.width) || + y < 0 || y >= static_cast(grid_.info.height)) { + return; + } + int index = y * grid_.info.width + x; + grid_.data[index] = value; +} + + +/* +//FUNKSJON BRUKT TIL TESTING, FJERNE SENERE:))? +void PoolExplorationPlanner::printGridToConsole() const{ + for (int y = grid_.info.height - 1; y >= 0; --y) { + for (int x = 0; x < grid_.info.width; ++x) { + + int index = y * grid_.info.width + x; + + if (grid_.data[index] == OCCUPIED) + std::cout << "X "; + else if (grid_.data[index] == -1) + std::cout << ". "; + else + std::cout << "? "; + } + std::cout << ::std::endl; + } +} +*/ + +} // namespace vortex::pool_exploration diff --git a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt new file mode 100644 index 0000000..20eb0f1 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt @@ -0,0 +1,20 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_pool_exploration + test_pool_exploration.cpp +) + +target_include_directories(test_pool_exploration PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include +) + +ament_target_dependencies(test_pool_exploration + rclcpp + nav_msgs + vortex_msgs + Eigen3 +) + +target_link_libraries(test_pool_exploration + ${PROJECT_NAME}_lib +) \ No newline at end of file diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp new file mode 100644 index 0000000..61e97ec --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp @@ -0,0 +1,225 @@ +#include + +#include +#include +#include + +#include +#include + +namespace vortex::pool_exploration::test { + +using vortex::utils::types::LineSegment2D; +using vortex::utils::types::Point2D; + +namespace { + +Point2D make_point(float x, float y) { + return Point2D{.x = x, .y = y}; +} + +LineSegment2D make_line(float x0, float y0, float x1, float y1) { + return LineSegment2D{ + .p0 = make_point(x0, y0), + .p1 = make_point(x1, y1), + }; +} + +PoolExplorationPlannerConfig make_config() { + PoolExplorationPlannerConfig config{}; + + // Avstander for gyldige vegger + config.min_wall_distance_m = 0.2f; + config.max_wall_distance_m = 10.0f; + + // Klassifisering + // Right wall: y må være negativ nok i NED + config.right_wall_max_y_m = -0.1f; + + // Far/front wall: x må være positiv nok + config.far_wall_min_x_m = 0.1f; + + // Vinkler + config.parallel_heading_angle_threshold_rad = 20.0f * static_cast(M_PI) / 180.0f; + config.perpendicular_heading_angle_threshold_rad = 70.0f * static_cast(M_PI) / 180.0f; + + // Hjørnevinkel + config.min_corner_angle_rad = 70.0f * static_cast(M_PI) / 180.0f; + config.max_corner_angle_rad = 110.0f * static_cast(M_PI) / 180.0f; + + // Docking-offset + config.right_wall_offset_m = 1.0f; + config.far_wall_offset_m = 1.5f; + + return config; +} + +} // namespace + +class PoolExplorationPlannerTest : public ::testing::Test { +protected: + PoolExplorationPlannerTest() + : config(make_config()), + planner(config) {} + + PoolExplorationPlannerConfig config; + PoolExplorationPlanner planner; +}; + +TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_FindsCornerFromPerpendicularWalls) { + // Drone i origo, heading langs +x + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // "Right wall" i NED: horisontal linje under drone (negativ y) + // "Front wall": vertikal linje foran drone (positiv x) + const LineSegment2D right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f); + const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const std::vector lines{right_wall, front_wall}; + + const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + + ASSERT_EQ(corners.size(), 1u); + + EXPECT_NEAR(corners[0].corner_point.x(), 5.0f, 1e-4f); + EXPECT_NEAR(corners[0].corner_point.y(), -2.0f, 1e-4f); +} + +TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsParallelWalls) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // Begge linjer omtrent parallelle med heading -> skal ikke gi hjørne + const LineSegment2D line0 = make_line(0.0f, -2.0f, 10.0f, -2.0f); + const LineSegment2D line1 = make_line(0.0f, -4.0f, 10.0f, -4.0f); + + const std::vector lines{line0, line1}; + + const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsWallsOutsideDistanceThreshold) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // Right wall er for langt unna + const LineSegment2D right_wall_too_far = make_line(0.0f, -20.0f, 10.0f, -20.0f); + const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const std::vector lines{right_wall_too_far, front_wall}; + + const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ReturnsClosestCorner) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + + CornerEstimate c0{ + .right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, -2.0f} + }; + + CornerEstimate c1{ + .right_wall = make_line(0.0f, -4.0f, 10.0f, -4.0f), + .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), + .corner_point = Eigen::Vector2f{8.0f, -4.0f} + }; + + const auto best = planner.select_best_corner({c1, c0}, drone_pos); + + EXPECT_NEAR(best.corner_point.x(), 5.0f, 1e-4f); + EXPECT_NEAR(best.corner_point.y(), -2.0f, 1e-4f); +} + +TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ThrowsOnEmptyInput) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + + EXPECT_THROW( + planner.select_best_corner({}, drone_pos), + std::runtime_error + ); +} + +TEST_F(PoolExplorationPlannerTest, EstimateDockingPosition_OffsetsFromCornerTowardDroneSideOfWalls) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + + // Hjørne i (5, -2) + // Right wall: y = -2 + // Front wall: x = 5 + // + // Med drone i (0,0): + // - normal fra right wall mot drone peker opp: (0, +1) + // - normal fra front wall mot drone peker venstre: (-1, 0) + // + // Forventet docking: + // (5, -2) + 1.0*(0,1) + 1.5*(-1,0) = (3.5, -1.0) + + CornerEstimate corner{ + .right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, -2.0f} + }; + + const Eigen::Vector2f docking = planner.estimate_docking_position(corner, drone_pos); + + EXPECT_NEAR(docking.x(), 3.5f, 1e-4f); + EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); +} + +TEST_F(PoolExplorationPlannerTest, FullPipeline_FindsCornerSelectsBestAndEstimatesDocking) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // Ett riktig hjørne nærme + const LineSegment2D right_wall_near = make_line(0.0f, -2.0f, 10.0f, -2.0f); + const LineSegment2D front_wall_near = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + // Ett riktig hjørne lenger unna + const LineSegment2D right_wall_far = make_line(0.0f, -4.0f, 10.0f, -4.0f); + const LineSegment2D front_wall_far = make_line(9.0f, -5.0f, 9.0f, 5.0f); + + // Litt støy som ikke skal bli brukt + const LineSegment2D noise = make_line(-5.0f, 3.0f, -2.0f, 3.0f); + + const std::vector lines{ + right_wall_near, front_wall_near, + right_wall_far, front_wall_far, + noise + }; + + const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + + ASSERT_GE(corners.size(), 2u); + + const auto best = planner.select_best_corner(corners, drone_pos); + const auto docking = planner.estimate_docking_position(best, drone_pos); + + EXPECT_NEAR(best.corner_point.x(), 5.0f, 1e-4f); + EXPECT_NEAR(best.corner_point.y(), -2.0f, 1e-4f); + + EXPECT_NEAR(docking.x(), 3.5f, 1e-4f); + EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); +} + +TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_AcceptsApproximatelyPerpendicularWalls) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // Litt skjeve vegger, men fortsatt innenfor tersklene + const LineSegment2D right_wall = make_line(0.0f, -2.0f, 10.0f, -1.5f); + const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.5f, 5.0f); + + const std::vector lines{right_wall, front_wall}; + + const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + + EXPECT_EQ(corners.size(), 1u); +} + +} // namespace vortex::pool_exploration::test \ No newline at end of file diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp new file mode 100644 index 0000000..34e0f77 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp @@ -0,0 +1,214 @@ +#include +#include +#include +#include +#include + + +using namespace vortex::pool_exploration; +/* +TEST(PoolExplorationTest, DrawLinesIdentityTransform) +{ + PoolExplorationMap map(10.0, 10.0, 1.0, "map"); + + // Lag et LineSegment2DArray med én linje + auto array_msg = std::make_shared(); + vortex_msgs::msg::LineSegment2D line; + line.p0.x = 1.0; + line.p0.y = 0.0; + line.p1.x = 4.0; + line.p1.y = 0.0; + array_msg->lines.push_back(line); + + // Identitet transform + Eigen::Matrix4f identity = Eigen::Matrix4f::Identity(); + + // Kjør funksjonen som tegner linjene + map.setLineSegmentInMapFrame(array_msg, identity); + + const auto& grid = map.grid(); + + // Map origin er -5,-5 + // Punkt (1,0) blir grid (6,5) + for (int x = 6; x <= 9; ++x) { + int index = 5 * grid.info.width + x; + EXPECT_EQ(grid.data[index], OCCUPIED); + } +} + + +TEST(PoolExplorationTest, DebugPrint) +{ + PoolExplorationMap map(10.0, 10.0, 1.0, "map"); + + map.bresenhamLineAlgoritm(2, 2, 8, 6); + + map.printGridToConsole(); // 👈 HER +} + +TEST(PoolExplorationTest, GridInitialization) +{ + PoolExplorationMap map(10.0, 10.0, 1.0, "map"); + + const auto& grid = map.grid(); + + EXPECT_EQ(grid.info.width, 10); + EXPECT_EQ(grid.info.height, 10); + EXPECT_EQ(grid.data.size(), 100); + + // Alle celler skal være -1 (unknown) + for (auto cell : grid.data) { + EXPECT_EQ(cell, -1); + } +} +/* +TEST(PoolExplorationTest, SetGridCellInsideBounds) +{ + PoolExplorationMap map(10.0, 10.0, 1.0, "map"); + + map.setGridCell(5, 5, OCCUPIED); + + const auto& grid = map.grid(); + int index = 5 * grid.info.width + 5; + + EXPECT_EQ(grid.data[index], OCCUPIED); +} + +TEST(PoolExplorationTest, BresenhamHorizontalLine) +{ + PoolExplorationMap map(10.0, 10.0, 1.0, "map"); + + map.bresenhamLineAlgoritm(2, 5, 7, 5); + + const auto& grid = map.grid(); + + for (int x = 2; x <= 7; ++x) { + int index = 5 * grid.info.width + x; + EXPECT_EQ(grid.data[index], OCCUPIED); + } +} + +TEST(PoolExplorationTest, MultipleLinesIdentityTransform) +{ + // Lag et større grid, f.eks. 20x20 meter med 1m oppløsning + PoolExplorationMap map(20.0, 20.0, 1.0, "map"); + + // Lag LineSegment2DArray-melding + auto array_msg = std::make_shared(); + + // Legg til flere linjer + vortex_msgs::msg::LineSegment2D line1; + line1.p0.x = 1.0; line1.p0.y = 0.0; + line1.p1.x = 4.0; line1.p1.y = 0.0; + array_msg->lines.push_back(line1); + + vortex_msgs::msg::LineSegment2D line2; + line2.p0.x = -3.0; line2.p0.y = 2.0; + line2.p1.x = 2.0; line2.p1.y = 5.0; + array_msg->lines.push_back(line2); + + vortex_msgs::msg::LineSegment2D line3; + line3.p0.x = -5.0; line3.p0.y = -5.0; + line3.p1.x = 5.0; line3.p1.y = -5.0; + array_msg->lines.push_back(line3); + + // Identitet transform + Eigen::Matrix4f identity = Eigen::Matrix4f::Identity(); + + // Tegn linjene + map.setLineSegmentInMapFrame(array_msg, identity); + + const auto& grid = map.grid(); + + // Enkel sjekk: se at start- og sluttpunkt for hver linje er satt til OCCUPIED + auto check_cell = [&](double x, double y) { + int gx = static_cast((x - grid.info.origin.position.x) / grid.info.resolution); + int gy = static_cast((y - grid.info.origin.position.y) / grid.info.resolution); + int index = gy * grid.info.width + gx; + EXPECT_EQ(grid.data[index], OCCUPIED); + }; + + // Linje 1 + check_cell(1.0, 0.0); + check_cell(4.0, 0.0); + + // Linje 2 + check_cell(-3.0, 2.0); + check_cell(2.0, 5.0); + + // Linje 3 + check_cell(-5.0, -5.0); + check_cell(5.0, -5.0); + + // Valgfritt: skriv grid til console for visuell inspeksjon + map.printGridToConsole(); +} + + +TEST(GeometryTest, ProjectPointToLine) { + PoolExplorationMap map(10, 10, 1.0, "map"); + + Eigen::Vector2f p0(0,0); + Eigen::Vector2f p1(4,0); + Eigen::Vector2f drone_pos(2, 3); + + Eigen::Vector2f projection = map.projectPointToLine(drone_pos, {p0,p1}); + + // Prosjekter punkt på x-akse linje => y skal bli 0 + EXPECT_FLOAT_EQ(projection.x(), 2.0f); + EXPECT_FLOAT_EQ(projection.y(), 0.0f); +} + +TEST(GeometryTest, LineAngleDifference) { + PoolExplorationMap map(10,10,1.0,"map"); + + Eigen::Vector2f p0(0,0); + Eigen::Vector2f p1(1,1); + float heading = 0.0f; // Drone peker mot x-akse + + float diff = map.lineAngleDifference({p0,p1}, heading); + + // Linje 45 grader mot x-aksen + EXPECT_NEAR(diff, M_PI/4, 1e-6); +} + +TEST(GeometryTest, LineIntersection) { + PoolExplorationMap map(10,10,1.0,"map"); + + Eigen::Vector2f intersection; + + // Kryssende linjer + std::pair line1{{0,0},{4,4}}; + std::pair line2{{0,4},{4,0}}; + + bool intersects = map.lineIntersection(line1, line2, intersection); + + EXPECT_TRUE(intersects); + EXPECT_NEAR(intersection.x(), 2.0f, 1e-6); + EXPECT_NEAR(intersection.y(), 2.0f, 1e-6); + + // Parallelle linjer + std::pair line3{{0,0},{1,0}}; + std::pair line4{{0,1},{1,1}}; + + intersects = map.lineIntersection(line3, line4, intersection); + EXPECT_FALSE(intersects); +} + +TEST(GeometryTest, AngleBetweenLines) { + PoolExplorationMap map(10,10,1.0,"map"); + + std::pair line1{{0,0},{1,0}}; + std::pair line2{{0,0},{0,1}}; + + float angle = map.angleBetweenLines(line1, line2); + + // Linjene er ortogonale + EXPECT_NEAR(angle, M_PI/2, 1e-6); + + // Samme linje => vinkel 0 + angle = map.angleBetweenLines(line1, line1); + EXPECT_NEAR(angle, 0.0, 1e-6); +} + +*/ \ No newline at end of file From 081852429bbfb83003bad63aa5fab480c3473ff2 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 26 Mar 2026 18:11:39 +0100 Subject: [PATCH 02/10] docking setup package --- .../subsea_docking_setup/CMakeLists.txt | 11 ++++++ .../config/line_detection_ransac.yaml | 24 +++++++++++++ .../launch/subsea_docking.launch.py | 35 +++++++++++++++++++ .../subsea_docking_setup/package.xml | 17 +++++++++ 4 files changed, 87 insertions(+) create mode 100644 mission/tacc/subsea_docking/subsea_docking_setup/CMakeLists.txt create mode 100644 mission/tacc/subsea_docking/subsea_docking_setup/config/line_detection_ransac.yaml create mode 100644 mission/tacc/subsea_docking/subsea_docking_setup/launch/subsea_docking.launch.py create mode 100644 mission/tacc/subsea_docking/subsea_docking_setup/package.xml diff --git a/mission/tacc/subsea_docking/subsea_docking_setup/CMakeLists.txt b/mission/tacc/subsea_docking/subsea_docking_setup/CMakeLists.txt new file mode 100644 index 0000000..7492075 --- /dev/null +++ b/mission/tacc/subsea_docking/subsea_docking_setup/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) +project(subsea_docking_setup) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/mission/tacc/subsea_docking/subsea_docking_setup/config/line_detection_ransac.yaml b/mission/tacc/subsea_docking/subsea_docking_setup/config/line_detection_ransac.yaml new file mode 100644 index 0000000..4f6a4bc --- /dev/null +++ b/mission/tacc/subsea_docking/subsea_docking_setup/config/line_detection_ransac.yaml @@ -0,0 +1,24 @@ +/**: + ros__parameters: + topic: + image_sub_topic: /forward_looking_sonar/display + line_segments_pub_topic: line_detection/line_segments + color_overlay_pub_topic: line_detection/visualization + boundary_debug_pub_topic: line_detection/boundary_image + boundary_overlay_pub_topic: line_detection/boundary_overlay + + boundary_detection: + threshold: 15 + step: 1.0 + num_rays: 50 + sample_side_length: 5 + angle: 150 + edge_detection: true + + ransac: + points_checked: 4 + inlier_threshold: 2.0 + min_remaining_points: 6 + min_inliers: 6 + + mode: "debug" # {"standard", "visualize", "debug"} diff --git a/mission/tacc/subsea_docking/subsea_docking_setup/launch/subsea_docking.launch.py b/mission/tacc/subsea_docking/subsea_docking_setup/launch/subsea_docking.launch.py new file mode 100644 index 0000000..30d958d --- /dev/null +++ b/mission/tacc/subsea_docking/subsea_docking_setup/launch/subsea_docking.launch.py @@ -0,0 +1,35 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('subsea_docking_setup') + + line_detection_config = os.path.join( + pkg_dir, 'config', 'line_detection_ransac.yaml' + ) + + container = ComposableNodeContainer( + name='subsea_docking_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='line_detection_ransac', + plugin='LineDetectionRansacNode', + name='line_detection_ransac', + parameters=[ + line_detection_config, + {'use_sim_time': False}, + ], + ), + ], + output='screen', + ) + + return LaunchDescription([container]) diff --git a/mission/tacc/subsea_docking/subsea_docking_setup/package.xml b/mission/tacc/subsea_docking/subsea_docking_setup/package.xml new file mode 100644 index 0000000..0f33633 --- /dev/null +++ b/mission/tacc/subsea_docking/subsea_docking_setup/package.xml @@ -0,0 +1,17 @@ + + + subsea_docking_setup + 0.0.1 + Launch and configuration package for subsea docking + todo + MIT + + ament_cmake + + line_detection_ransac + rclcpp_components + + + ament_cmake + + From 254dc233ac42b073e1a26489122eddc483c09ea1 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 26 Mar 2026 18:19:22 +0100 Subject: [PATCH 03/10] fix pre-commit. Replace waypoint with SendPose service --- .pre-commit-config.yaml | 2 +- .../pool_exploration/CMakeLists.txt | 2 +- .../subsea_docking/pool_exploration/README.md | 4 +- .../config/pool_exploration.yaml | 8 +- .../pool_exploration/pool_exploration.hpp | 194 +++++---- .../pool_exploration/pool_exploration_ros.hpp | 373 +++++++++--------- .../include/pool_exploration/pool_grid.hpp | 56 +-- .../pool_exploration/src/pool_exploration.cpp | 193 ++++----- .../src/pool_exploration_ros.cpp | 255 ++++++------ .../pool_exploration/src/pool_grid.cpp | 74 ++-- .../pool_exploration/test/CMakeLists.txt | 2 +- .../test/test_pool_exploration.cpp | 103 ++--- .../pool_exploration/test/test_pool_map.cpp | 26 +- 13 files changed, 661 insertions(+), 631 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 17684f5..753ea16 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -113,7 +113,7 @@ repos: rev: v2.4.1 hooks: - id: codespell - args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU'] + args: ['--write-changes', '--ignore-words-list=theses,fom,NED,ENU,Alle'] ci: autoupdate_schedule: quarterly diff --git a/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt index ea47a87..9814ddd 100644 --- a/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt +++ b/mission/tacc/subsea_docking/pool_exploration/CMakeLists.txt @@ -60,7 +60,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC fmt message_filters #for testing: - visualization_msgs + visualization_msgs ) rclcpp_components_register_node( diff --git a/mission/tacc/subsea_docking/pool_exploration/README.md b/mission/tacc/subsea_docking/pool_exploration/README.md index ab2c4ba..8ddf81c 100644 --- a/mission/tacc/subsea_docking/pool_exploration/README.md +++ b/mission/tacc/subsea_docking/pool_exploration/README.md @@ -1,6 +1,6 @@ colcon build --packages-up-to pool_exploration --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -Utvide til venstre hjørne også +Utvide til venstre hjørne også -Evt filtrere ut doble linjer tidligere +Evt filtrere ut double linjer tidligere diff --git a/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml b/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml index e784a02..9e15e0e 100644 --- a/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml +++ b/mission/tacc/subsea_docking/pool_exploration/config/pool_exploration.yaml @@ -2,7 +2,7 @@ ros__parameters: # Topics line_sub_topic: "/line_detection/line_segments" - pose_sub_topic: "/pose" + pose_sub_topic: "/pose" sonar_info_sub_topic: "/moby/fls/sonar_info" debug_topic: "/debug" @@ -32,14 +32,14 @@ right_dist: -0.1 # ENDRE NAVN ? choose_right_corner: 1 - + # Waypoint switching_threshold: 0.5 overwrite_prior_waypoints: true take_priority: true # FOR MAP LOGIC - map_pub_topic: "/map" + map_pub_topic: "/map" map_frame: "map" # Map / grid @@ -52,4 +52,4 @@ unknown_cell: -1 occupied_cell: 100 - enu_to_ned: false \ No newline at end of file + enu_to_ned: false diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp index af353dd..ebdd703 100644 --- a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp @@ -1,19 +1,20 @@ -#ifndef POOL_EXPLORATION_HPP -#define POOL_EXPLORATION_HPP +#ifndef POOL_EXPLORATION__POOL_EXPLORATION_HPP_ +#define POOL_EXPLORATION__POOL_EXPLORATION_HPP_ -#include -#include #include #include +#include +#include #include -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { /** - * @brief Represents a candidate docking corner formed by a right wall and a far wall. + * @brief Represents a candidate docking corner formed by a right wall and a far + * wall. * - * A corner estimate consists of two detected wall segments and their intersection - * point in the planning frame. + * A corner estimate consists of two detected wall segments and their + * intersection point in the planning frame. */ struct CornerEstimate { /** @brief Line segment classified as the right wall. */ @@ -60,45 +61,54 @@ inline Eigen::Vector2f to_eigen(const vortex::utils::types::Point2D point) { * point estimation. */ struct PoolExplorationPlannerConfig { - /** @brief Minimum allowed distance from the drone to a candidate wall projection [m]. */ - float min_wall_distance_m; + /** @brief Minimum allowed distance from the drone to a candidate wall + * projection [m]. */ + float min_wall_distance_m; - /** @brief Maximum allowed distance from the drone to a candidate wall projection [m]. */ + /** @brief Maximum allowed distance from the drone to a candidate wall + * projection [m]. */ float max_wall_distance_m; // REMOVE THIS IF OTHER CODE WORKS, TO DO - float far_wall_heading_angle_threshold; + float far_wall_heading_angle_threshold; - /** @brief Maximum angle between heading and a wall for it to be considered parallel [rad]. */ + /** @brief Maximum angle between heading and a wall for it to be considered + * parallel [rad]. */ float parallel_heading_angle_threshold_rad; - /** @brief Minimum angle between heading and a wall for it to be considered perpendicular [rad]. */ + /** @brief Minimum angle between heading and a wall for it to be considered + * perpendicular [rad]. */ float perpendicular_heading_angle_threshold_rad; - /** @brief Minimum allowed angle between right wall and far wall for a valid corner [rad]. */ + /** @brief Minimum allowed angle between right wall and far wall for a valid + * corner [rad]. */ float min_corner_angle_rad; - /** @brief Maximum allowed angle between right wall and far wall for a valid corner [rad]. */ + /** @brief Maximum allowed angle between right wall and far wall for a valid + * corner [rad]. */ float max_corner_angle_rad; // REMOVE THIS IF OTHER CODE WORKS, TO DO - float right_dist; + float right_dist; - /** @brief Maximum y-value used when checking whether a projection belongs to the right wall. */ + /** @brief Maximum y-value used when checking whether a projection belongs + * to the right wall. */ float right_wall_max_y_m; - /** @brief Minimum x-value used when checking whether a projection belongs to the far wall. */ + /** @brief Minimum x-value used when checking whether a projection belongs + * to the far wall. */ float far_wall_min_x_m; - /** @brief Offset from the right wall when estimating docking position [m]. */ + /** @brief Offset from the right wall when estimating docking position [m]. + */ float right_wall_offset_m; /** @brief Offset from the far wall when estimating docking position [m]. */ float far_wall_offset_m; - /** @brief - * Selects whether to prefer the right-side corner in the mission logic. - */ + /** @brief + * Selects whether to prefer the right-side corner in the mission logic. + */ // HAS NOT BEEN IMPLEMENTED YET, TO DO int choose_right_corner; }; @@ -106,27 +116,27 @@ struct PoolExplorationPlannerConfig { /** * @brief Planner for detecting pool corners and estimating a docking position. * - * The planner classifies detected line segments as candidate pool walls, combines - * compatible wall pairs into corner estimates, and computes a docking point from - * the selected corner. + * The planner classifies detected line segments as candidate pool walls, + * combines compatible wall pairs into corner estimates, and computes a docking + * point from the selected corner. */ class PoolExplorationPlanner { -public: + public: /** * @brief Construct a new planner instance. * * @param config Planner configuration parameters. */ - PoolExplorationPlanner( - const PoolExplorationPlannerConfig& config); + explicit PoolExplorationPlanner(const PoolExplorationPlannerConfig& config); /** - * @brief Find all valid corner estimates from a set of detected line segments. + * @brief Find all valid corner estimates from a set of detected line + * segments. * - * Each line segment is first classified as a candidate wall. Candidate right walls - * and far walls are then paired, and their intersection is checked. A pair is - * accepted as a corner if the angle between the two walls lies within the configured - * corner-angle interval. + * Each line segment is first classified as a candidate wall. Candidate + * right walls and far walls are then paired, and their intersection is + * checked. A pair is accepted as a corner if the angle between the two + * walls lies within the configured corner-angle interval. * * @param lines Input line segments expressed in the planning frame. * @param drone_pos Current drone position in the same frame as the lines. @@ -134,9 +144,10 @@ class PoolExplorationPlanner { * @return Vector of valid corner estimates. */ // EVT LAGE FOR VENSTRE VEGG, TO DO - std::vector find_corner_estimates( - const std::vector& lines, - const Eigen::Vector2f& drone_pos, // sjekk at får inn pos og heading riktig? + std::vector find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& + drone_pos, // sjekk at får inn pos og heading riktig? float drone_heading) const; /** @@ -149,10 +160,10 @@ class PoolExplorationPlanner { * @param drone_pos Current drone position in the planning frame. * @return Best corner estimate. * - * @throws std::runtime_error if the input vector is empty. + * @throws std::runtime_error if the input vector is empty. // ENDRE UNNTAKSHÅNDTERING?, TO DO */ - CornerEstimate select_best_corner( + CornerEstimate select_best_corner( const std::vector& possible_corners, const Eigen::Vector2f& drone_pos) const; @@ -162,43 +173,48 @@ class PoolExplorationPlanner { * The docking point is computed by offsetting the detected corner along the * inward-facing normals of the right wall and far wall. * - * @param estimated_corner Corner estimate containing the two wall segments and their intersection. - * @param drone_pos Current drone position in the same frame as the corner estimate. + * @param estimated_corner Corner estimate containing the two wall segments + * and their intersection. + * @param drone_pos Current drone position in the same frame as the corner + * estimate. * @return Estimated docking position in the planning frame. */ Eigen::Vector2f estimate_docking_position( const CornerEstimate& estimated_corner, const Eigen::Vector2f& drone_pos) const; -private: + private: /** - * @brief Classify a line segment as a right wall, far wall, or rejected. - * - * A line is classified using: - * - the distance from the drone to the orthogonal projection on the line, - * - the position of that projection relative to the expected mission geometry, - * - and the unsigned angle between the wall direction and drone heading. - * - * Right-wall candidates must satisfy the configured right-side projection test - * and be approximately parallel to the drone heading. - * - * Far-wall candidates must satisfy the configured forward projection test - * and be approximately perpendicular to the drone heading. - * + * @brief Classify a line segment as a right wall, far wall, or rejected. + * + * A line is classified using: + * - the distance from the drone to the orthogonal projection on the line, + * - the position of that projection relative to the expected mission + * geometry, + * - and the unsigned angle between the wall direction and drone heading. + * + * Right-wall candidates must satisfy the configured right-side projection + * test and be approximately parallel to the drone heading. + * + * Far-wall candidates must satisfy the configured forward projection test + * and be approximately perpendicular to the drone heading. + * * @param line Input line segment. * @param drone_pos Current drone position. * @param drone_heading Current drone heading [rad]. * @return Wall classification result. */ WallClassification classify_wall( - const vortex::utils::types::LineSegment2D& line, - const Eigen::Vector2f& drone_pos, - float drone_heading) const; + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos, + float drone_heading) const; /** - * @brief Project a point orthogonally onto the infinite line defined by a line segment. + * @brief Project a point orthogonally onto the infinite line defined by a + * line segment. * - * If the segment is degenerate, the first endpoint is returned. ANNEN HÅNDTERING? TO DO + * If the segment is degenerate, the first endpoint is returned. ANNEN + * HÅNDTERING? TO DO * * @param drone_pos Point to be projected. * @param line Line segment defining the projection line. @@ -206,28 +222,32 @@ class PoolExplorationPlanner { */ // PROJEKSJONSFORMELEN, TO DO Eigen::Vector2f project_drone_onto_line( - const Eigen::Vector2f& drone_pos, // punktet som projiseres + const Eigen::Vector2f& drone_pos, // punktet some projiseres const vortex::utils::types::LineSegment2D& line) const; /** - * @brief Compute the smallest unsigned angle between a wall segment and the drone heading. - * - * The wall direction is compared to the drone heading direction using the absolute - * dot product, making the result independent of the line endpoint ordering. - * - * The returned angle lies in [0, π/2]: - * - ≈ 0 rad → wall is parallel to heading - * - ≈ π/2 rad → wall is perpendicular to heading - * - * If the segment is degenerate (near-zero length), +∞ is returned. - * The dot product is clamped to [0, 1] to ensure numerical stability in acos(). - * - * @param line Wall segment. - * @param drone_heading Drone heading [rad]. - * @return Smallest unsigned angle between wall and heading [rad], or +∞ if degenerate. - */ + * @brief Compute the smallest unsigned angle between a wall segment and the + * drone heading. + * + * The wall direction is compared to the drone heading direction using the + * absolute dot product, making the result independent of the line endpoint + * ordering. + * + * The returned angle lies in [0, π/2]: + * - ≈ 0 rad → wall is parallel to heading + * - ≈ π/2 rad → wall is perpendicular to heading + * + * If the segment is degenerate (near-zero length), +∞ is returned. + * The dot product is clamped to [0, 1] to ensure numerical stability in + * acos(). + * + * @param line Wall segment. + * @param drone_heading Drone heading [rad]. + * @return Smallest unsigned angle between wall and heading [rad], or +∞ if + * degenerate. + */ float angle_between_line_and_heading( - const vortex::utils::types::LineSegment2D& line, + const vortex::utils::types::LineSegment2D& line, float drone_heading) const; /** @@ -242,16 +262,16 @@ class PoolExplorationPlanner { * @return true if the lines intersect. * @return false if the lines are parallel. */ - bool compute_line_intersection( + bool compute_line_intersection( const vortex::utils::types::LineSegment2D& line0, const vortex::utils::types::LineSegment2D& line1, Eigen::Vector2f& intersection_coordinates) const; - + /** * @brief Compute the smallest angle between two line segments. * - * The result is the unsigned smallest angle between the direction vectors of - * the two segments. + * The result is the unsigned smallest angle between the direction vectors + * of the two segments. * * @param line0 First line segment. * @param line1 Second line segment. @@ -262,16 +282,18 @@ class PoolExplorationPlanner { const vortex::utils::types::LineSegment2D& line1) const; /** - * @brief Compute the unit normal of a line pointing towards a reference point. + * @brief Compute the unit normal of a line pointing towards a reference + * point. * - * This is used when computing a docking position offset from a detected wall. + * This is used when computing a docking position offset from a detected + * wall. * * @param line Input line segment. * @param drone_pos Reference point used to choose normal direction. * @return Unit normal pointing towards the reference point. */ Eigen::Vector2f compute_normal_towards_point( - const vortex::utils::types::LineSegment2D& line, + const vortex::utils::types::LineSegment2D& line, const Eigen::Vector2f& drone_pos) const; /** @brief Planner configuration. */ @@ -280,4 +302,4 @@ class PoolExplorationPlanner { } // namespace vortex::pool_exploration -#endif // POOL_EXPLORATION_HPP +#endif // POOL_EXPLORATION__POOL_EXPLORATION_HPP_ diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp index 53cb4a5..c6d45c1 100644 --- a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp @@ -1,7 +1,9 @@ -#ifndef POOL_EXPLORATION_ROS_HPP -#define POOL_EXPLORATION_ROS_HPP +#ifndef POOL_EXPLORATION__POOL_EXPLORATION_ROS_HPP_ +#define POOL_EXPLORATION__POOL_EXPLORATION_ROS_HPP_ #include +#include +#include #include @@ -17,14 +19,14 @@ #include #include -#include +#include -#include +#include #include -#include //til testing +#include // til testing -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { /** * @brief ROS 2 node for pool exploration and docking waypoint generation. @@ -35,176 +37,164 @@ namespace vortex::pool_exploration{ * frame, estimates a valid pool corner through the planner, computes a * docking position, and sends the resulting waypoint to the waypoint manager. */ - + class PoolExplorationNode : public rclcpp::Node { - public: - /** - * @brief Construct a new PoolExplorationNode. - * - * The constructor initializes parameters, ROS interfaces, and the planner. - * - * @param options ROS node options. - */ - explicit PoolExplorationNode(const rclcpp::NodeOptions& options); - - /** @brief Default destructor. */ - ~PoolExplorationNode() = default; + public: + /** + * @brief Construct a new PoolExplorationNode. + * + * The constructor initializes parameters, ROS interfaces, and the planner. + * + * @param options ROS node options. + */ + explicit PoolExplorationNode(const rclcpp::NodeOptions& options); - private: - // setup - - /** - * @brief Declare and load ROS parameters used by the node. - * - * This includes frame names, topic names, and waypoint-related parameters. - */ - void setup_parameters(); - - /** - * @brief Create subscribers, TF utilities, and service clients. - * - * This method initializes: - * - TF buffer and listener, - * - line, pose, and sonar-info subscriptions, - * - TF message filter for line detections, - * - waypoint service client. - */ - void setup_publishers_and_subscribers(); - - /** - * @brief Create and configure the pool exploration planner. - * - * Planner configuration values are loaded from ROS parameters and used to - * construct the internal planner instance. - */ - void setup_planner(); - - // callbacks - - /** - * @brief Callback for vehicle pose updates. - * - * Updates the internal vehicle state estimate used by the planner. - * - * @param msg Vehicle pose message with covariance. - */ - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg); - - /** - * @brief Callback for sonar image metadata updates. - * - * Stores the most recent sonar information needed to convert detected line - * endpoints from image pixels to metric sonar coordinates. - * - * @param msg Sonar metadata message. - */ - void sonar_info_callback( - const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg); - - /** - * @brief Callback for detected line segments. - * - * Triggered when a line segment message passes the TF message filter. The - * callback attempts to estimate a docking waypoint from the detected lines. - * - * @param msg Array of detected 2D line segments. - */ - void line_callback( - const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg); - - // helpers - - /** - * @brief Estimate and send a docking waypoint from detected line segments. - * - * The input segments are transformed into the planning frame, passed to the - * planner for corner detection, and then used to estimate a docking point. - * If a valid estimate is found, a waypoint is sent. - * - * @param msg Array of detected 2D line segments. - */ - void estimate_and_send_docking_waypoint(const vortex_msgs::msg::LineSegment2DArray& msg); - - /** - * @brief Transform sonar-detected 2D segments into a target frame. - * - * Each line endpoint is first converted from pixel coordinates to sonar - * metric coordinates using the latest sonar metadata. The resulting points - * are then transformed by the provided homogeneous transform. - * - * @param msg Input line segment array. - * @param T_target_src Homogeneous transform from source frame to target frame. - * @return Vector of transformed 2D line segments in the target frame. - */ - std::vector transform_segments_2d( - const vortex_msgs::msg::LineSegment2DArray& msg, - const Eigen::Matrix4f& T_target_src); - - /** - * @brief Send a docking waypoint to the waypoint manager. - * - * A single waypoint is created from the estimated docking position and sent - * through the SendWaypoints service. The request uses the configured waypoint - * control parameters. - * - * @param docking_estimate Estimated docking position in the planning frame. - */ - void send_docking_waypoint(const Eigen::Vector2f& docking_estimate); - - //TIL TESTING - void publish_docking_marker(const Eigen::Vector2f& docking); - - - /** @brief Topic name for detected sonar line segments. */ - std::string line_sub_topic_; - - /** @brief Topic name for vehicle pose estimates. */ - std::string pose_sub_topic_; - - /** @brief Topic name for sonar image metadata. */ - std::string sonar_info_sub_topic_; - - // TIL TESTING - std::string debug_topic_; - - - /** @brief Odometry frame used as planning/reference frame. */ - std::string odom_frame_; - - /** @brief Vehicle base frame name. */ - std::string base_frame_; - - /** @brief Sonar frame name. */ - std::string sonar_frame_; - - - /** @brief Distance threshold for switching to the next waypoint [m]. */ - float waypoint_switching_threshold_; - - /** @brief Whether newly sent waypoints should overwrite previously queued waypoints. */ - bool waypoint_overwrite_prior_; - - /** @brief Whether the sent waypoint should take priority in the waypoint manager. */ - bool waypoint_take_priority_; - - - /** @brief True once a docking waypoint has been sent. */ - bool waypoint_sent_{false}; - - /** @brief Latest known vehicle pose and heading used by the planner. */ - vortex::utils::types::PoseEuler drone_state_; - - /** @brief Most recent sonar metadata message. */ - vortex_msgs::msg::SonarInfo::ConstSharedPtr latest_sonar_info_; - - - /** @brief TF buffer used for transform lookup. */ - std::shared_ptr tf_buffer_; - - /** @brief TF listener attached to the TF buffer. */ - std::shared_ptr tf_listener_; + /** @brief Default destructor. */ + ~PoolExplorationNode() = default; + + private: + // setup + + /** + * @brief Declare and load ROS parameters used by the node. + * + * This includes frame names, topic names, and waypoint-related parameters. + */ + void setup_parameters(); + + /** + * @brief Create subscribers, TF utilities, and service clients. + * + * This method initializes: + * - TF buffer and listener, + * - line, pose, and sonar-info subscriptions, + * - TF message filter for line detections, + * - waypoint service client. + */ + void setup_publishers_and_subscribers(); + + /** + * @brief Create and configure the pool exploration planner. + * + * Planner configuration values are loaded from ROS parameters and used to + * construct the internal planner instance. + */ + void setup_planner(); + + // callbacks + + /** + * @brief Callback for vehicle pose updates. + * + * Updates the internal vehicle state estimate used by the planner. + * + * @param msg Vehicle pose message with covariance. + */ + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& + msg); + + /** + * @brief Callback for sonar image metadata updates. + * + * Stores the most recent sonar information needed to convert detected line + * endpoints from image pixels to metric sonar coordinates. + * + * @param msg Sonar metadata message. + */ + void sonar_info_callback( + const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg); + + /** + * @brief Callback for detected line segments. + * + * Triggered when a line segment message passes the TF message filter. The + * callback attempts to estimate a docking waypoint from the detected lines. + * + * @param msg Array of detected 2D line segments. + */ + void line_callback( + const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg); + // helpers + + /** + * @brief Estimate and send a docking waypoint from detected line segments. + * + * The input segments are transformed into the planning frame, passed to the + * planner for corner detection, and then used to estimate a docking point. + * If a valid estimate is found, a waypoint is sent. + * + * @param msg Array of detected 2D line segments. + */ + void estimate_and_send_docking_waypoint( + const vortex_msgs::msg::LineSegment2DArray& msg); + + /** + * @brief Transform sonar-detected 2D segments into a target frame. + * + * Each line endpoint is first converted from pixel coordinates to sonar + * metric coordinates using the latest sonar metadata. The resulting points + * are then transformed by the provided homogeneous transform. + * + * @param msg Input line segment array. + * @param T_target_src Homogeneous transform from source frame to target + * frame. + * @return Vector of transformed 2D line segments in the target frame. + */ + std::vector transform_segments_2d( + const vortex_msgs::msg::LineSegment2DArray& msg, + const Eigen::Matrix4f& T_target_src); + + /** + * @brief Send a docking waypoint to the waypoint manager. + * + * A single waypoint is created from the estimated docking position and sent + * through the SendWaypoints service. The request uses the configured + * waypoint control parameters. + * + * @param docking_estimate Estimated docking position in the planning frame. + */ + void send_docking_waypoint(const Eigen::Vector2f& docking_estimate); + + // TIL TESTING + void publish_docking_marker(const Eigen::Vector2f& docking); + + /** @brief Topic name for detected sonar line segments. */ + std::string line_sub_topic_; + + /** @brief Topic name for vehicle pose estimates. */ + std::string pose_sub_topic_; + + /** @brief Topic name for sonar image metadata. */ + std::string sonar_info_sub_topic_; + + // TIL TESTING + std::string debug_topic_; + + /** @brief Odometry frame used as planning/reference frame. */ + std::string odom_frame_; + + /** @brief Vehicle base frame name. */ + std::string base_frame_; + + /** @brief Sonar frame name. */ + std::string sonar_frame_; + + /** @brief True once a docking pose has been sent. */ + bool waypoint_sent_{false}; + + /** @brief Latest known vehicle pose and heading used by the planner. */ + vortex::utils::types::PoseEuler drone_state_; + + /** @brief Most recent sonar metadata message. */ + vortex_msgs::msg::SonarInfo::ConstSharedPtr latest_sonar_info_; + + /** @brief TF buffer used for transform lookup. */ + std::shared_ptr tf_buffer_; + + /** @brief TF listener attached to the TF buffer. */ + std::shared_ptr tf_listener_; /** @brief Subscriber for line segment detections. */ message_filters::Subscriber line_sub_; @@ -215,37 +205,41 @@ class PoolExplorationNode : public rclcpp::Node { * Ensures that line messages are only forwarded once the required transform * into the target frame is available. */ - std::shared_ptr> line_filter_; + std::shared_ptr< + tf2_ros::MessageFilter> + line_filter_; /** @brief Subscriber for vehicle pose updates. */ - rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; /** @brief Subscriber for sonar metadata updates. */ - rclcpp::Subscription::SharedPtr sonar_info_sub_; - - - /** @brief Client for sending waypoint requests to the waypoint manager. */ - rclcpp::Client::SharedPtr waypoint_client_; + rclcpp::Subscription::SharedPtr + sonar_info_sub_; + /** @brief Client for sending the docking pose estimate. */ + rclcpp::Client::SharedPtr send_pose_client_; - /** @brief Pool exploration planner used for corner detection and docking estimation. */ + /** @brief Pool exploration planner used for corner detection and docking + * estimation. */ std::unique_ptr planner_; // Debug visualization - rclcpp::Publisher::SharedPtr docking_marker_pub_; //til testing + rclcpp::Publisher::SharedPtr + docking_marker_pub_; // til testing }; // GRID LOGIKK TIL SENERE -# if 0 -#include +#if 0 +#include private: - + std::string map_pub_topic_; std::string map_frame_; std::shared_ptr tf_broadcaster_; - + // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -254,14 +248,14 @@ class PoolExplorationNode : public rclcpp::Node { void timer_callback(); // Lager en transformasjon TF map->odom geometry_msgs::msg::TransformStamped compute_map_odom_transform(); - + void publish_grid(); // Logikk til senere bruk: void draw_segments_in_map_frame(const vortex_msgs::msg::LineSegment2DArray& msg); // publisher - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr map_pub_; // map @@ -270,5 +264,4 @@ class PoolExplorationNode : public rclcpp::Node { #endif } // namespace vortex::pool_exploration -#endif // POOL_EXPLORATION_ROS_HPP - +#endif // POOL_EXPLORATION__POOL_EXPLORATION_ROS_HPP_ diff --git a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp index 614db42..821d2fb 100644 --- a/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp @@ -1,43 +1,46 @@ -#ifndef POOL_EXPLORATION_HPP -#define POOL_EXPLORATION_HPP +#ifndef POOL_EXPLORATION__POOL_GRID_HPP_ +#define POOL_EXPLORATION__POOL_GRID_HPP_ #include #include -#include #include +#include +#include #include #include -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { class PoolExplorationMap { -public: - // Konstruktør - PoolExplorationMap( - double size_x, - double size_y, - double resolution, - const std::string& frame_id); + public: + // Konstruktør + PoolExplorationMap(double size_x, + double size_y, + double resolution, + const std::string& frame_id); const nav_msgs::msg::OccupancyGrid& grid() const; - /** - * @brief Draws a straight line on the occupancy grid using Bresenham's algorithm. - * Reference: https://www.youtube.com/watch?v=CceepU1vIKo - */ + /** + * @brief Draws a straight line on the occupancy grid using Bresenham's + * algorithm. Reference: https://www.youtube.com/watch?v=CceepU1vIKo + */ // denne kalles fra ros noden - // fra map til celle (forklar mer hvordan) - void insert_line_in_grid(const std::vector& segments); // NB: har endret til utils type linesegment, har ikke asEigen funksjonen - -private: - //Occupancy grid - // Set all cells unknown, - // origin in center of map with no rotation + // fra map til cell (forklar mer hvordan) + void insert_line_in_grid( + const std::vector& + segments); // NB: har endret til utils type linesegment, har ikke + // asEigen funksjonen + + private: + // Occupancy grid + // Set all cells unknown, + // origin in center of map with no rotation void initialize_grid(); - //brukes i insertSegmentsMapFrame - // bresenham line algoritm (referanse) + // brukes i insertSegmentsMapFrame + // bresenham line algorithm (referanse) void bresenham_line_algoritm(int x0, int y0, int x1, int y1); // brukes i bresenhamLineAlgoritm @@ -50,11 +53,10 @@ class PoolExplorationMap { double size_y_; double resolution_; - int8_t occupied_cell_; + int8_t occupied_cell_; int8_t unknown_cell_; - }; } // namespace vortex::pool_exploration -#endif // POOL_EXPLORATION_HPP \ No newline at end of file +#endif // POOL_EXPLORATION__POOL_GRID_HPP_ diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp index 493ce9b..c6f36de 100644 --- a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp @@ -1,17 +1,18 @@ +#include // til testing #include #include -#include // til testing +#include #include #include -#include -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { -PoolExplorationPlanner::PoolExplorationPlanner(const PoolExplorationPlannerConfig& config) +PoolExplorationPlanner::PoolExplorationPlanner( + const PoolExplorationPlannerConfig& config) : config_(config) {} -# if 1 +#if 1 std::vector PoolExplorationPlanner::find_corner_estimates( const std::vector& lines, const Eigen::Vector2f& drone_pos, @@ -21,12 +22,12 @@ std::vector PoolExplorationPlanner::find_corner_estimates( std::vector corner_estimates; for (const auto& line : lines) { - const WallClassification classification = classify_wall(line, drone_pos, drone_heading); + const WallClassification classification = + classify_wall(line, drone_pos, drone_heading); if (classification == WallClassification::RightWall) { right_wall_candidates.push_back(line); - } - else if (classification == WallClassification::FarWall) { + } else if (classification == WallClassification::FarWall) { far_wall_candidates.push_back(line); } } @@ -38,7 +39,8 @@ std::vector PoolExplorationPlanner::find_corner_estimates( for (const auto& right_wall : right_wall_candidates) { for (const auto& far_wall : far_wall_candidates) { Eigen::Vector2f corner_intersection; - if (!compute_line_intersection(right_wall, far_wall, corner_intersection)) { + if (!compute_line_intersection(right_wall, far_wall, + corner_intersection)) { continue; } @@ -46,7 +48,8 @@ std::vector PoolExplorationPlanner::find_corner_estimates( if (wall_angle >= config_.min_corner_angle_rad && wall_angle <= config_.max_corner_angle_rad) { - corner_estimates.push_back({right_wall, far_wall, corner_intersection}); + corner_estimates.push_back( + {right_wall, far_wall, corner_intersection}); } } } @@ -58,7 +61,8 @@ std::vector PoolExplorationPlanner::find_corner_estimates( CornerEstimate PoolExplorationPlanner::select_best_corner( const std::vector& possible_corners, const Eigen::Vector2f& drone_pos) const { - // if (possible_corners.empty()) {throw std::runtime_error("No candidate corners available"); } // UNNTAKSHÅNDTERING I ROS I STEDET + // if (possible_corners.empty()) {throw std::runtime_error("No candidate + // corners available"); } // UNNTAKSHÅNDTERING I ROS I STEDET float min_distance = std::numeric_limits::max(); CornerEstimate best_corner = possible_corners.front(); @@ -77,12 +81,15 @@ CornerEstimate PoolExplorationPlanner::select_best_corner( Eigen::Vector2f PoolExplorationPlanner::estimate_docking_position( const CornerEstimate& estimated_corner, const Eigen::Vector2f& drone_pos) const { - Eigen::Vector2f right_normal = compute_normal_towards_point(estimated_corner.right_wall, drone_pos); - Eigen::Vector2f far_normal = compute_normal_towards_point(estimated_corner.far_wall, drone_pos); + Eigen::Vector2f right_normal = + compute_normal_towards_point(estimated_corner.right_wall, drone_pos); + Eigen::Vector2f far_normal = + compute_normal_towards_point(estimated_corner.far_wall, drone_pos); - Eigen::Vector2f docking_estimate = estimated_corner.corner_point - + right_normal * config_.right_wall_offset_m - + far_normal * config_.far_wall_offset_m; + Eigen::Vector2f docking_estimate = + estimated_corner.corner_point + + right_normal * config_.right_wall_offset_m + + far_normal * config_.far_wall_offset_m; return docking_estimate; } @@ -95,22 +102,23 @@ WallClassification PoolExplorationPlanner::classify_wall( const Eigen::Vector2f p1 = to_eigen(line.p1); const Eigen::Vector2f dir = p1 - p0; - if (dir.squaredNorm() < 1e-6f) { // ENDRE KRAV TIL LENGDE PÅ LINJE? + if (dir.squaredNorm() < 1e-6f) { // ENDRE KRAV TIL LENGDE PÅ LINJE? spdlog::info(" -> REJECTED: degenerate line"); return WallClassification::Rejected; } // TIL LOGGING - spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", - p0.x(), p0.y(), p1.x(), p1.y()); + spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", p0.x(), p0.y(), + p1.x(), p1.y()); // const Eigen::Vector2f projection = project_drone_onto_line(drone_pos, line); const Eigen::Vector2f rel = projection - drone_pos; const float distance = rel.norm(); // TIL LOGGING - spdlog::info(" Projection: ({:.2f}, {:.2f})", projection.x(), projection.y()); - spdlog::info(" Rel: ({:.2f}, {:.2f}), dist: {:.2f}", - rel.x(), rel.y(), distance); + spdlog::info(" Projection: ({:.2f}, {:.2f})", projection.x(), + projection.y()); + spdlog::info(" Rel: ({:.2f}, {:.2f}), dist: {:.2f}", rel.x(), rel.y(), + distance); // if (distance < config_.min_wall_distance_m || distance > config_.max_wall_distance_m) { @@ -118,27 +126,31 @@ WallClassification PoolExplorationPlanner::classify_wall( return WallClassification::Rejected; } - const float heading_wall_angle = angle_between_line_and_heading(line, drone_heading); - spdlog::info(" Angle: {:.2f} rad ({:.1f} deg)", - heading_wall_angle, heading_wall_angle * 180.0 / M_PI); + const float heading_wall_angle = + angle_between_line_and_heading(line, drone_heading); + spdlog::info(" Angle: {:.2f} rad ({:.1f} deg)", heading_wall_angle, + heading_wall_angle * 180.0 / M_PI); Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); float forward_dist = rel.dot(forward); - float right_dist = rel.dot(right); + float right_dist = rel.dot(right); - // RIGHT WALL: projection has negative y-value in NED, wall is approximately parallel to heading - if (//projection.y() < config_.right_wall_max_y_m && - right_dist <0 && + // RIGHT WALL: projection has negative y-value in NED, wall is approximately + // parallel to heading + if ( // projection.y() < config_.right_wall_max_y_m && + right_dist < 0 && heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as RIGHT candidate"); return WallClassification::RightWall; } - // FAR WALL: projection has positive x-value, wall is approximately perpendicular to heading - if (//projection.x() > config_.far_wall_min_x_m && + // FAR WALL: projection has positive x-value, wall is approximately + // perpendicular to heading + if ( // projection.x() > config_.far_wall_min_x_m && forward_dist > 0 && - heading_wall_angle > config_.perpendicular_heading_angle_threshold_rad) { + heading_wall_angle > + config_.perpendicular_heading_angle_threshold_rad) { spdlog::info(" -> Classified as FAR candidate"); return WallClassification::FarWall; } @@ -146,7 +158,7 @@ WallClassification PoolExplorationPlanner::classify_wall( return WallClassification::Rejected; } -Eigen::Vector2f PoolExplorationPlanner::project_drone_onto_line( +Eigen::Vector2f PoolExplorationPlanner::project_drone_onto_line( const Eigen::Vector2f& drone_pos, const vortex::utils::types::LineSegment2D& line) const { const Eigen::Vector2f p0 = to_eigen(line.p0); @@ -155,14 +167,16 @@ Eigen::Vector2f PoolExplorationPlanner::project_drone_onto_line( const Eigen::Vector2f line_direction = p1 - p0; const float line_length_squared = line_direction.squaredNorm(); - const float projection_parameter = (drone_pos - p0).dot(line_direction) / line_length_squared; - const Eigen::Vector2f projection_point = p0 + projection_parameter * line_direction; + const float projection_parameter = + (drone_pos - p0).dot(line_direction) / line_length_squared; + const Eigen::Vector2f projection_point = + p0 + projection_parameter * line_direction; return projection_point; } -float PoolExplorationPlanner::angle_between_line_and_heading( - const vortex::utils::types::LineSegment2D& line, +float PoolExplorationPlanner::angle_between_line_and_heading( + const vortex::utils::types::LineSegment2D& line, float drone_heading) const { Eigen::Vector2f wall_direction = to_eigen(line.p1) - to_eigen(line.p0); const float wall_length = wall_direction.norm(); @@ -172,18 +186,18 @@ float PoolExplorationPlanner::angle_between_line_and_heading( } wall_direction /= wall_length; - Eigen::Vector2f heading_direction( - std::cos(drone_heading), - std::sin(drone_heading)); + Eigen::Vector2f heading_direction(std::cos(drone_heading), + std::sin(drone_heading)); - float cos_heading_wall_angle = - std::abs(wall_direction.dot(heading_direction)); //rekkefølge p0 og p1 urelevant - cos_heading_wall_angle = std::clamp(cos_heading_wall_angle, 0.0f, 1.0f); // unngå små avvik + float cos_heading_wall_angle = std::abs(wall_direction.dot( + heading_direction)); // rekkefølge p0 og p1 urelevant + cos_heading_wall_angle = + std::clamp(cos_heading_wall_angle, 0.0f, 1.0f); // unngå små avvik return std::acos(cos_heading_wall_angle); } -bool PoolExplorationPlanner::compute_line_intersection( +bool PoolExplorationPlanner::compute_line_intersection( const vortex::utils::types::LineSegment2D& line0, const vortex::utils::types::LineSegment2D& line1, Eigen::Vector2f& intersection) const { @@ -192,27 +206,29 @@ bool PoolExplorationPlanner::compute_line_intersection( const Eigen::Vector2f p10 = to_eigen(line1.p0); const Eigen::Vector2f p11 = to_eigen(line1.p1); - float determinant = (p00.x()-p01.x())*(p10.y()-p11.y()) - (p00.y()-p01.y())*(p10.x()-p11.x()); + float determinant = (p00.x() - p01.x()) * (p10.y() - p11.y()) - + (p00.y() - p01.y()) * (p10.x() - p11.x()); - if (std::abs(determinant) < 1e-6f) - return false; // parallelle linjer + if (std::abs(determinant) < 1e-6f) + return false; // parallelle linjer - intersection.x() = ((p00.x()*p01.y()-p00.y()*p01.x())*(p10.x()-p11.x()) - - (p00.x()-p01.x())*(p10.x()*p11.y()-p10.y()*p11.x()))/determinant; - intersection.y() = ((p00.x()*p01.y()-p00.y()*p01.x())*(p10.y()-p11.y()) - - (p00.y()-p01.y())*(p10.x()*p11.y()-p10.y()*p11.x()))/determinant; + intersection.x() = + ((p00.x() * p01.y() - p00.y() * p01.x()) * (p10.x() - p11.x()) - + (p00.x() - p01.x()) * (p10.x() * p11.y() - p10.y() * p11.x())) / + determinant; + intersection.y() = + ((p00.x() * p01.y() - p00.y() * p01.x()) * (p10.y() - p11.y()) - + (p00.y() - p01.y()) * (p10.x() * p11.y() - p10.y() * p11.x())) / + determinant; return true; } float PoolExplorationPlanner::angle_between_lines( const vortex::utils::types::LineSegment2D& line0, - const vortex::utils::types::LineSegment2D& line1) const { - - Eigen::Vector2f line0_direction = - to_eigen(line0.p1) - to_eigen(line0.p0); + const vortex::utils::types::LineSegment2D& line1) const { + Eigen::Vector2f line0_direction = to_eigen(line0.p1) - to_eigen(line0.p0); - Eigen::Vector2f line1_direction = - to_eigen(line1.p1) - to_eigen(line1.p0); + Eigen::Vector2f line1_direction = to_eigen(line1.p1) - to_eigen(line1.p0); const float line0_length = line0_direction.norm(); const float line1_length = line1_direction.norm(); @@ -227,16 +243,14 @@ float PoolExplorationPlanner::angle_between_lines( float cos_angle_between_lines = std::abs(line0_direction.dot(line1_direction)); - cos_angle_between_lines = - std::clamp(cos_angle_between_lines, 0.0f, 1.0f); + cos_angle_between_lines = std::clamp(cos_angle_between_lines, 0.0f, 1.0f); return std::acos(cos_angle_between_lines); } Eigen::Vector2f PoolExplorationPlanner::compute_normal_towards_point( const vortex::utils::types::LineSegment2D& line, - const Eigen::Vector2f& drone_pos) const -{ + const Eigen::Vector2f& drone_pos) const { Eigen::Vector2f p0 = to_eigen(line.p0); Eigen::Vector2f p1 = to_eigen(line.p1); @@ -267,11 +281,11 @@ Eigen::Vector2f PoolExplorationPlanner::compute_normal_towards_point( return normal; } -// klassifisere hjørne utfra relativ avstand til drone -# if 0 +// klassifisere hjørne utfra relative avstand til drone +#if 0 std::vector PoolExplorationPlanner::find_corner_estimates( const std::vector& lines, - const Eigen::Vector2f& drone_pos, + const Eigen::Vector2f& drone_pos, float drone_heading) { std::vector right_candidates; @@ -279,22 +293,22 @@ std::vector PoolExplorationPlanner::find_corner_estimates( std::vector far_candidates; std::vector potential_corners; - // Finner ut hva som er foran ogsånn? (?) + // Finner ut hva some er foran ogsånn? (?) Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); - Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); // RIKTIG NED? + Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); // RIKTIG NED? for (const auto& line : lines) { // const auto eigen_line = line.asEigen(); const Eigen::Vector2f p0 = to_eigen(line.p0); const Eigen::Vector2f p1 = to_eigen(line.p1); - + spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", p0.x(), p0.y(), p1.x(), p1.y()); Eigen::Vector2f projection = project_point_onto_line(drone_pos, line); Eigen::Vector2f rel = projection - drone_pos; - float dist = rel.norm(); + float dist = rel.norm(); float angle = angle_between_line_and_heading(line, drone_heading); float forward_dist = rel.dot(forward); @@ -315,33 +329,26 @@ std::vector PoolExplorationPlanner::find_corner_estimates( spdlog::info("Angle: {:.2f}, right_dist: {:.2f}, forward_dist: {:.2f}", angle, right_dist, forward_dist); - if (dist >= config_.min_wall_distance_m && - dist <= config_.max_wall_distance_m && - angle < config_.far_wall_heading_angle_threshold && - right_dist > config_.right_dist - ) { + if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle < config_.far_wall_heading_angle_threshold && + right_dist > config_.right_dist) { spdlog::info(" → Classified as RIGHT candidate"); right_candidates.push_back(line); - } - else if (dist >= config_.min_wall_distance_m && - dist <= config_.max_wall_distance_m && - angle > config_.far_wall_heading_angle_threshold && - forward_dist > 0.0f - ) { + } else if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle > config_.far_wall_heading_angle_threshold && + forward_dist > 0.0f) { spdlog::info(" → Classified as FAR candidate"); far_candidates.push_back(line); - } - - else if (dist >= config_.min_wall_distance_m && - dist <= config_.max_wall_distance_m && - angle < config_.far_wall_heading_angle_threshold && - right_dist < 0.0f - ) { - spdlog::info(" → Classified as LEFT candidate"); + } else if (dist >= config_.min_wall_distance_m && + dist <= config_.max_wall_distance_m && + angle < config_.far_wall_heading_angle_threshold && + right_dist < 0.0f) { + spdlog::info(" → Classified as LEFT candidate"); left_candidates.push_back(line); - } - else { - spdlog::info(" → REJECTED by angle"); + } else { + spdlog::info(" → REJECTED by angle"); } } spdlog::info("==== SUMMARY ===="); @@ -359,9 +366,9 @@ std::vector PoolExplorationPlanner::find_corner_estimates( } } } - } - return potential_corners; -} + } + return potential_corners; +} #endif } // namespace vortex::pool_exploration diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp index a5af8e0..1a89e7a 100644 --- a/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp @@ -3,31 +3,29 @@ #include #include +#include +#include #include #include -#include #include -#include - #include // #include -#include //ERSTATTE MED VORTEX? ^^^ +#include +#include // ERSTATTE MED VORTEX? ^^^ #include -#include #include "pool_exploration/pool_exploration.hpp" -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { PoolExplorationNode::PoolExplorationNode(const rclcpp::NodeOptions& options) : rclcpp::Node("pool_exploration_node", options) { - setup_parameters(); - setup_publishers_and_subscribers(); - setup_planner(); - } + setup_parameters(); + setup_publishers_and_subscribers(); + setup_planner(); +} -void PoolExplorationNode::setup_parameters() -{ +void PoolExplorationNode::setup_parameters() { // Frames odom_frame_ = this->declare_parameter("odom_frame"); base_frame_ = this->declare_parameter("base_frame"); @@ -36,13 +34,10 @@ void PoolExplorationNode::setup_parameters() // Topics line_sub_topic_ = this->declare_parameter("line_sub_topic"); pose_sub_topic_ = this->declare_parameter("pose_sub_topic"); - sonar_info_sub_topic_ = this->declare_parameter("sonar_info_sub_topic"); - debug_topic_ = this->declare_parameter("debug_topic"); //For testing - - // Waypoint parameters from YAML - waypoint_switching_threshold_ = this->declare_parameter("switching_threshold"); - waypoint_overwrite_prior_ = this->declare_parameter("overwrite_prior_waypoints"); - waypoint_take_priority_ = this->declare_parameter("take_priority"); + sonar_info_sub_topic_ = + this->declare_parameter("sonar_info_sub_topic"); + debug_topic_ = + this->declare_parameter("debug_topic"); // For testing } void PoolExplorationNode::setup_publishers_and_subscribers() { @@ -52,16 +47,19 @@ void PoolExplorationNode::setup_publishers_and_subscribers() { tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - const auto qos_sub = rclcpp::SensorDataQoS(); //standard for sensordata, ta inn vortex sin i stedet?? + const auto qos_sub = + rclcpp::SensorDataQoS(); // standard for sensordata, ta inn vortex sin + // i stedet?? auto sub_options = rclcpp::SubscriptionOptions(); - line_sub_.subscribe( - this, line_sub_topic_, - qos_sub.get_rmw_qos_profile(), sub_options); + line_sub_.subscribe(this, line_sub_topic_, qos_sub.get_rmw_qos_profile(), + sub_options); - pose_sub_ = this->create_subscription( + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( pose_sub_topic_, qos_sub, - std::bind(&PoolExplorationNode::pose_callback, this, std::placeholders::_1)); + std::bind(&PoolExplorationNode::pose_callback, this, + std::placeholders::_1)); sonar_info_sub_ = this->create_subscription( sonar_info_sub_topic_, qos_sub, @@ -69,36 +67,52 @@ void PoolExplorationNode::setup_publishers_and_subscribers() { this->sonar_info_callback(msg); }); - line_filter_ = std::make_shared>( - line_sub_, *tf_buffer_, odom_frame_, 10, - this->get_node_logging_interface(), - this->get_node_clock_interface()); + line_filter_ = std::make_shared< + tf2_ros::MessageFilter>( + line_sub_, *tf_buffer_, odom_frame_, 10, + this->get_node_logging_interface(), this->get_node_clock_interface()); - line_filter_->registerCallback( - std::bind(&PoolExplorationNode::line_callback, this, std::placeholders::_1)); + line_filter_->registerCallback(std::bind( + &PoolExplorationNode::line_callback, this, std::placeholders::_1)); - waypoint_client_ = this->create_client("/send_waypoints"); //endre /send_waypoints navnet + send_pose_client_ = this->create_client( + "/pool_exploration/docking_pose"); - docking_marker_pub_ = this->create_publisher(debug_topic_, 10); //til testing + docking_marker_pub_ = + this->create_publisher( + debug_topic_, 10); // til testing } -void PoolExplorationNode::setup_planner() { +void PoolExplorationNode::setup_planner() { PoolExplorationPlannerConfig config{}; - config.min_wall_distance_m = this->declare_parameter("min_wall_distance_m"); - config.max_wall_distance_m = this->declare_parameter("max_wall_distance_m"); - config.far_wall_heading_angle_threshold = this->declare_parameter("far_wall_heading_angle_threshold"); - config.parallel_heading_angle_threshold_rad = this->declare_parameter("parallel_heading_angle_threshold_rad"); - config.perpendicular_heading_angle_threshold_rad = this->declare_parameter("perpendicular_heading_angle_threshold_rad"); - - config.min_corner_angle_rad = this->declare_parameter("min_corner_angle_rad"); - config.max_corner_angle_rad = this->declare_parameter("max_corner_angle_rad"); + config.min_wall_distance_m = + this->declare_parameter("min_wall_distance_m"); + config.max_wall_distance_m = + this->declare_parameter("max_wall_distance_m"); + config.far_wall_heading_angle_threshold = + this->declare_parameter("far_wall_heading_angle_threshold"); + config.parallel_heading_angle_threshold_rad = + this->declare_parameter("parallel_heading_angle_threshold_rad"); + config.perpendicular_heading_angle_threshold_rad = + this->declare_parameter( + "perpendicular_heading_angle_threshold_rad"); + + config.min_corner_angle_rad = + this->declare_parameter("min_corner_angle_rad"); + config.max_corner_angle_rad = + this->declare_parameter("max_corner_angle_rad"); config.right_dist = this->declare_parameter("right_dist"); - config.far_wall_min_x_m = this->declare_parameter("far_wall_min_x_m"); - config.right_wall_max_y_m = this->declare_parameter("right_wall_max_y_m"); - config.choose_right_corner = this->declare_parameter("choose_right_corner"); - config.right_wall_offset_m = this->declare_parameter("right_wall_offset_m"); - config.far_wall_offset_m = this->declare_parameter("far_wall_offset_m"); + config.far_wall_min_x_m = + this->declare_parameter("far_wall_min_x_m"); + config.right_wall_max_y_m = + this->declare_parameter("right_wall_max_y_m"); + config.choose_right_corner = + this->declare_parameter("choose_right_corner"); + config.right_wall_offset_m = + this->declare_parameter("right_wall_offset_m"); + config.far_wall_offset_m = + this->declare_parameter("far_wall_offset_m"); planner_ = std::make_unique(config); } @@ -122,9 +136,8 @@ void PoolExplorationNode::sonar_info_callback( void PoolExplorationNode::line_callback( const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg) { - estimate_and_send_docking_waypoint(*msg); - //drawSegmentsInMapFrame(*msg); + // drawSegmentsInMapFrame(*msg); } void PoolExplorationNode::estimate_and_send_docking_waypoint( @@ -143,40 +156,40 @@ void PoolExplorationNode::estimate_and_send_docking_waypoint( const Eigen::Affine3d T = tf2::transformToEigen(tf_stamped.transform); const Eigen::Matrix4f T_odom_src = T.matrix().cast(); - auto segs = transform_segments_2d(msg, T_odom_src); //NB ENDRE NAVN <3 + auto segs = transform_segments_2d(msg, T_odom_src); // NB ENDRE NAVN <3 - Eigen::Vector2f drone_pos = {drone_state_.x,drone_state_.y}; + Eigen::Vector2f drone_pos = {drone_state_.x, drone_state_.y}; float drone_heading = drone_state_.yaw; - auto corners = planner_->find_corner_estimates( - segs, - drone_pos, - drone_heading - ); + auto corners = + planner_->find_corner_estimates(segs, drone_pos, drone_heading); if (corners.empty()) { - spdlog::info("[PoolExploration] No valid corners -> no docking estimate"); + spdlog::info( + "[PoolExploration] No valid corners -> no docking estimate"); return; } - CornerEstimate best_corner = planner_->select_best_corner(corners, drone_pos); + CornerEstimate best_corner = + planner_->select_best_corner(corners, drone_pos); - Eigen::Vector2f docking = planner_->estimate_docking_position(best_corner, drone_pos); + Eigen::Vector2f docking = + planner_->estimate_docking_position(best_corner, drone_pos); - publish_docking_marker(docking); //til testing + publish_docking_marker(docking); // til testing send_docking_waypoint(docking); spdlog::info("[PoolExploration] Docking estimate (odom): x={} y={}", docking.x(), docking.y()); } -std::vector PoolExplorationNode::transform_segments_2d( +std::vector +PoolExplorationNode::transform_segments_2d( const vortex_msgs::msg::LineSegment2DArray& msg, - const Eigen::Matrix4f& T_target_src) -{ + const Eigen::Matrix4f& T_target_src) { std::vector segments; segments.reserve(msg.lines.size()); - + if (!latest_sonar_info_) { spdlog::warn("[PoolExploration] No sonar image info available yet"); return segments; @@ -185,25 +198,19 @@ std::vector PoolExplorationNode::transform_ vortex::utils::types::SonarInfo sonar_info; sonar_info.meters_per_pixel_x = latest_sonar_info_->meters_per_pixel_x; sonar_info.meters_per_pixel_y = latest_sonar_info_->meters_per_pixel_y; - sonar_info.image_width = latest_sonar_info_->width; + sonar_info.image_width = latest_sonar_info_->width; sonar_info.image_height = latest_sonar_info_->height; for (const auto& line : msg.lines) { - const auto p0 = sonar_info.pixel_index_to_sonar_metric(line.p0.x, line.p0.y); - const auto p1 = sonar_info.pixel_index_to_sonar_metric(line.p1.x, line.p1.y); - - const Eigen::Vector4f p0_sonar( - static_cast(p0.y), - static_cast(p0.x), - 0.0f, - 1.0f - ); - const Eigen::Vector4f p1_sonar( - static_cast(p1.y), - static_cast(p1.x), - 0.0f, - 1.0f - ); + const auto p0 = + sonar_info.pixel_index_to_sonar_metric(line.p0.x, line.p0.y); + const auto p1 = + sonar_info.pixel_index_to_sonar_metric(line.p1.x, line.p1.y); + + const Eigen::Vector4f p0_sonar(static_cast(p0.y), + static_cast(p0.x), 0.0f, 1.0f); + const Eigen::Vector4f p1_sonar(static_cast(p1.y), + static_cast(p1.x), 0.0f, 1.0f); const Eigen::Vector4f p0_target = T_target_src * p0_sonar; const Eigen::Vector4f p1_target = T_target_src * p1_sonar; @@ -212,48 +219,44 @@ std::vector PoolExplorationNode::transform_ seg.p0 = {p0_target.x(), p0_target.y()}; seg.p1 = {p1_target.x(), p1_target.y()}; - spdlog::info("[PoolExploration] Line received: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", // til debugging - seg.p0.x, seg.p0.y, seg.p1.x, seg.p1.y); + spdlog::info( + "[PoolExploration] Line received: ({:.2f}, {:.2f}) -> ({:.2f}, " + "{:.2f})", // til debugging + seg.p0.x, seg.p0.y, seg.p1.x, seg.p1.y); segments.push_back(seg); } return segments; } -void PoolExplorationNode::send_docking_waypoint(const Eigen::Vector2f& docking_estimate) -{ +void PoolExplorationNode::send_docking_waypoint( + const Eigen::Vector2f& docking_estimate) { if (waypoint_sent_) { return; } - if (!waypoint_client_->service_is_ready()) { - spdlog::warn("Waypoint service not available"); + if (!send_pose_client_->service_is_ready()) { + spdlog::warn("[PoolExploration] SendPose service not available"); return; } - auto request = std::make_shared(); - vortex_msgs::msg::Waypoint wp; - - wp.pose.position.x = docking_estimate.x(); - wp.pose.position.y = docking_estimate.y(); - wp.pose.position.z = 0.0f; //skal være 0? - - wp.pose.orientation.x = 0.0f; // Riktig orientation? - wp.pose.orientation.y = 0.0f; - wp.pose.orientation.z = 0.0f; - wp.pose.orientation.w = 1.0f; + auto request = std::make_shared(); - request->waypoints.push_back(wp); - request->switching_threshold = waypoint_switching_threshold_; - request->overwrite_prior_waypoints = waypoint_overwrite_prior_; - request->take_priority = waypoint_take_priority_; + request->pose.header.stamp = this->now(); + request->pose.header.frame_id = odom_frame_; + request->pose.pose.position.x = docking_estimate.x(); + request->pose.pose.position.y = docking_estimate.y(); + request->pose.pose.position.z = 0.0; + request->pose.pose.orientation.w = 1.0; - waypoint_client_->async_send_request(request); + send_pose_client_->async_send_request(request); waypoint_sent_ = true; - spdlog::info("Docking waypoint sent to WaypointManager: x={} y={}", docking_estimate.x(), docking_estimate.y()); + spdlog::info("[PoolExploration] Docking pose sent: x={} y={}", + docking_estimate.x(), docking_estimate.y()); } -void PoolExplorationNode::publish_docking_marker(const Eigen::Vector2f& docking) //til testing +void PoolExplorationNode::publish_docking_marker( + const Eigen::Vector2f& docking) // til testing { visualization_msgs::msg::Marker marker; @@ -284,34 +287,34 @@ void PoolExplorationNode::publish_docking_marker(const Eigen::Vector2f& docking) } // Grid logikk -# if 0 +#if 0 -//INKLUDER DISSE NÅR SLÅES SAMMEN IGJEN +// INKLUDER DISSE NÅR SLÅES SAMMEN IGJEN void PoolExplorationNode::setup_publishers_and_subscribers() { - //map_frame_ = this->declare_parameter("map_frame", "map"); //allerede definert - //pub_dt_ = std::chrono::milliseconds( - // this->declare_parameter("publish_rate_ms")); + // map_frame_ = this->declare_parameter("map_frame", "map"); + // pub_dt_ = std::chrono::milliseconds( + // this->declare_parameter("publish_rate_ms")); - //this->declare_parameter("enu_to_ned", false); + // this->declare_parameter("enu_to_ned", false); - //const std::string map_pub_topic = - // this->declare_parameter("map_pub_topic", "/map"); + // const std::string map_pub_topic = + // this->declare_parameter("map_pub_topic", "/map"); - //tf_broadcaster_ = std::make_shared(this); - //const auto map_to_odom_tf = compute_map_odom_transform(); //const? - //tf_broadcaster_->sendTransform(map_to_odom_tf); + // tf_broadcaster_ = std::make_shared(this); + // const auto map_to_odom_tf = compute_map_odom_transform(); + // tf_broadcaster_->sendTransform(map_to_odom_tf); - //const auto map_qos = rclcpp::QoS(rclcpp::KeepLast(1)) //bare siste kart - // .reliable() //meldinger må leveres - // .transient_local(); //subscribers fra siste melding + // const auto map_qos = rclcpp::QoS(rclcpp::KeepLast(1)) + // .reliable() + // .transient_local(); - //map_pub_ = this->create_publisher( - // map_pub_topic, - // map_qos); - //timer_ = this->create_wall_timer(pub_dt_, [this]() { this->timer_callback(); }); + // map_pub_ = this->create_publisher( + // map_pub_topic, + // map_qos); + // timer_ = this->create_wall_timer(pub_dt_, [this]() { this->timer_callback(); }); } -//Konstruktøren +// Konstruktøren PoolExplorationNode::PoolExplorationNode(const rclcpp::NodeOptions& options) : rclcpp::Node("pool_exploration_node", options), size_x_(this->declare_parameter("size_x")), @@ -340,7 +343,7 @@ geometry_msgs::msg::TransformStamped PoolExplorationNode::compute_map_odom_trans map_to_odom.transform.rotation.y = q1.y(); map_to_odom.transform.rotation.z = q1.z(); } else { - //Kjøres dersom vi er i ENU frame + // Kjøres dersom vi er i ENU frame tf2::Quaternion q2; q2.setRPY(0.0, 0.0, 0.0); map_to_odom.transform.rotation.w = q2.w(); @@ -370,7 +373,7 @@ void PoolExplorationNode::draw_segments_in_map_frame( const Eigen::Affine3d T = tf2::transformToEigen(tf_stamped.transform); const Eigen::Matrix4f T_map_src = T.matrix().cast(); - //se nærmere på denne transform logikken + // se nærmere på denne transform logikken auto segs = transform_segments_2d(msg, T_map_src); // bruker logikk fra pool_exploration.h @@ -380,7 +383,7 @@ void PoolExplorationNode::draw_segments_in_map_frame( } // LOGIKK FOR GRIDDET void PoolExplorationNode::publish_grid() { - nav_msgs::msg::OccupancyGrid msg = map_.grid(); + nav_msgs::msg::OccupancyGrid msg = map_.grid(); msg.header.frame_id = map_frame_; msg.header.stamp = this->get_clock()->now(); diff --git a/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp b/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp index 4025164..eb4756d 100644 --- a/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp @@ -1,36 +1,33 @@ +#include // til testing #include #include #include -#include // til testing #include -namespace vortex::pool_exploration{ +namespace vortex::pool_exploration { // Lage egen config til griddet TO DO -PoolExplorationMap::PoolExplorationMap( - double size_x, - double size_y, - double resolution, - const std::string& frame_id) - : size_x_(size_x), - size_y_(size_y), - resolution_(resolution) { +PoolExplorationMap::PoolExplorationMap(double size_x, + double size_y, + double resolution, + const std::string& frame_id) + : size_x_(size_x), size_y_(size_y), resolution_(resolution) { grid_.header.frame_id = frame_id; initialize_grid(); } void PoolExplorationMap::initialize_grid() { - int width = static_cast(size_x_ / resolution_); + int width = static_cast(size_x_ / resolution_); int height = static_cast(size_y_ / resolution_); grid_.info.resolution = resolution_; - grid_.info.width = width; - grid_.info.height = height; + grid_.info.width = width; + grid_.info.height = height; grid_.info.origin.position.x = -size_x_ / 2.0; grid_.info.origin.position.y = -size_y_ / 2.0; - grid_.info.origin.orientation.w = 1.0; + grid_.info.origin.orientation.w = 1.0; grid_.data.assign(width * height, unknown_cell_); } @@ -42,36 +39,43 @@ const nav_msgs::msg::OccupancyGrid& PoolExplorationMap::grid() const { void PoolExplorationMap::insert_line_in_grid( const std::vector& segments) { for (const auto& seg : segments) { - - int x0 = static_cast((seg.p0.x - grid_.info.origin.position.x) / grid_.info.resolution); - int y0 = static_cast((seg.p0.y - grid_.info.origin.position.y) / grid_.info.resolution); - int x1 = static_cast((seg.p1.x - grid_.info.origin.position.x) / grid_.info.resolution); - int y1 = static_cast((seg.p1.y - grid_.info.origin.position.y) / grid_.info.resolution); + int x0 = static_cast((seg.p0.x - grid_.info.origin.position.x) / + grid_.info.resolution); + int y0 = static_cast((seg.p0.y - grid_.info.origin.position.y) / + grid_.info.resolution); + int x1 = static_cast((seg.p1.x - grid_.info.origin.position.x) / + grid_.info.resolution); + int y1 = static_cast((seg.p1.y - grid_.info.origin.position.y) / + grid_.info.resolution); bresenham_line_algoritm(x0, y0, x1, y1); } } -void PoolExplorationMap::bresenham_line_algoritm( - int x0, int y0, int x1, int y1) { +void PoolExplorationMap::bresenham_line_algoritm(int x0, + int y0, + int x1, + int y1) { bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); - if (!steep){ - if (x0 > x1){ - std::swap(x0, x1); - std::swap(y0,y1); + if (!steep) { + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); } int dx = x1 - x0; int dy = y1 - y0; - int dir = (dy <0) ? -1 : 1; + int dir = (dy < 0) ? -1 : 1; dy = std::abs(dy); int y = y0; int p = 2 * dy - dx; for (int i = 0; i <= dx; i++) { - set_grid_cell(x0 + i, y, occupied_cell_); // må declare occupied_cell_ i ros senere + set_grid_cell( + x0 + i, y, + occupied_cell_); // må declare occupied_cell_ i ros senere if (p >= 0) { y += dir; @@ -80,15 +84,15 @@ void PoolExplorationMap::bresenham_line_algoritm( p += 2 * dy; } } else { - if (y0 > y1){ - std::swap(x0, x1); - std::swap(y0,y1); + if (y0 > y1) { + std::swap(x0, x1); + std::swap(y0, y1); } int dx = x1 - x0; int dy = y1 - y0; - int dir = (dx <0) ? -1 : 1; + int dir = (dx < 0) ? -1 : 1; dx = std::abs(dx); int x = x0; @@ -106,16 +110,14 @@ void PoolExplorationMap::bresenham_line_algoritm( } } -void PoolExplorationMap::set_grid_cell( - int x, int y, int value) { - if (x < 0 || x >= static_cast(grid_.info.width) || - y < 0 || y >= static_cast(grid_.info.height)) { +void PoolExplorationMap::set_grid_cell(int x, int y, int value) { + if (x < 0 || x >= static_cast(grid_.info.width) || y < 0 || + y >= static_cast(grid_.info.height)) { return; } int index = y * grid_.info.width + x; grid_.data[index] = value; } - /* //FUNKSJON BRUKT TIL TESTING, FJERNE SENERE:))? diff --git a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt index 20eb0f1..854aaa0 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt +++ b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt @@ -17,4 +17,4 @@ ament_target_dependencies(test_pool_exploration target_link_libraries(test_pool_exploration ${PROJECT_NAME}_lib -) \ No newline at end of file +) diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp index 61e97ec..9e4b501 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp @@ -1,8 +1,8 @@ #include #include -#include #include +#include #include #include @@ -33,15 +33,17 @@ PoolExplorationPlannerConfig make_config() { config.max_wall_distance_m = 10.0f; // Klassifisering - // Right wall: y må være negativ nok i NED + // Right wall: y må være negative nok i NED config.right_wall_max_y_m = -0.1f; - // Far/front wall: x må være positiv nok + // Far/front wall: x må være positive nok config.far_wall_min_x_m = 0.1f; // Vinkler - config.parallel_heading_angle_threshold_rad = 20.0f * static_cast(M_PI) / 180.0f; - config.perpendicular_heading_angle_threshold_rad = 70.0f * static_cast(M_PI) / 180.0f; + config.parallel_heading_angle_threshold_rad = + 20.0f * static_cast(M_PI) / 180.0f; + config.perpendicular_heading_angle_threshold_rad = + 70.0f * static_cast(M_PI) / 180.0f; // Hjørnevinkel config.min_corner_angle_rad = 70.0f * static_cast(M_PI) / 180.0f; @@ -57,28 +59,28 @@ PoolExplorationPlannerConfig make_config() { } // namespace class PoolExplorationPlannerTest : public ::testing::Test { -protected: - PoolExplorationPlannerTest() - : config(make_config()), - planner(config) {} + protected: + PoolExplorationPlannerTest() : config(make_config()), planner(config) {} PoolExplorationPlannerConfig config; PoolExplorationPlanner planner; }; -TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_FindsCornerFromPerpendicularWalls) { +TEST_F(PoolExplorationPlannerTest, + FindCornerEstimates_FindsCornerFromPerpendicularWalls) { // Drone i origo, heading langs +x const Eigen::Vector2f drone_pos{0.0f, 0.0f}; const float drone_heading = 0.0f; - // "Right wall" i NED: horisontal linje under drone (negativ y) - // "Front wall": vertikal linje foran drone (positiv x) + // "Right wall" i NED: horizontal linje under drone (negative y) + // "Front wall": vertical linje foran drone (positive x) const LineSegment2D right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f); const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); const std::vector lines{right_wall, front_wall}; - const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + const auto corners = + planner.find_corner_estimates(lines, drone_pos, drone_heading); ASSERT_EQ(corners.size(), 1u); @@ -96,22 +98,26 @@ TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsParallelWalls) { const std::vector lines{line0, line1}; - const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + const auto corners = + planner.find_corner_estimates(lines, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } -TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsWallsOutsideDistanceThreshold) { +TEST_F(PoolExplorationPlannerTest, + FindCornerEstimates_RejectsWallsOutsideDistanceThreshold) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; const float drone_heading = 0.0f; // Right wall er for langt unna - const LineSegment2D right_wall_too_far = make_line(0.0f, -20.0f, 10.0f, -20.0f); + const LineSegment2D right_wall_too_far = + make_line(0.0f, -20.0f, 10.0f, -20.0f); const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); const std::vector lines{right_wall_too_far, front_wall}; - const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + const auto corners = + planner.find_corner_estimates(lines, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } @@ -119,17 +125,13 @@ TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsWallsOutsideDistan TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ReturnsClosestCorner) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - CornerEstimate c0{ - .right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), - .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), - .corner_point = Eigen::Vector2f{5.0f, -2.0f} - }; + CornerEstimate c0{.right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, -2.0f}}; - CornerEstimate c1{ - .right_wall = make_line(0.0f, -4.0f, 10.0f, -4.0f), - .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), - .corner_point = Eigen::Vector2f{8.0f, -4.0f} - }; + CornerEstimate c1{.right_wall = make_line(0.0f, -4.0f, 10.0f, -4.0f), + .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), + .corner_point = Eigen::Vector2f{8.0f, -4.0f}}; const auto best = planner.select_best_corner({c1, c0}, drone_pos); @@ -140,13 +142,11 @@ TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ReturnsClosestCorner) { TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ThrowsOnEmptyInput) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - EXPECT_THROW( - planner.select_best_corner({}, drone_pos), - std::runtime_error - ); + EXPECT_THROW(planner.select_best_corner({}, drone_pos), std::runtime_error); } -TEST_F(PoolExplorationPlannerTest, EstimateDockingPosition_OffsetsFromCornerTowardDroneSideOfWalls) { +TEST_F(PoolExplorationPlannerTest, + EstimateDockingPosition_OffsetsFromCornerTowardDroneSideOfWalls) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; // Hjørne i (5, -2) @@ -154,25 +154,25 @@ TEST_F(PoolExplorationPlannerTest, EstimateDockingPosition_OffsetsFromCornerTowa // Front wall: x = 5 // // Med drone i (0,0): - // - normal fra right wall mot drone peker opp: (0, +1) - // - normal fra front wall mot drone peker venstre: (-1, 0) + // - normal fra right wall not drone peker opp: (0, +1) + // - normal fra front wall not drone peker venstre: (-1, 0) // // Forventet docking: // (5, -2) + 1.0*(0,1) + 1.5*(-1,0) = (3.5, -1.0) - CornerEstimate corner{ - .right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), - .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), - .corner_point = Eigen::Vector2f{5.0f, -2.0f} - }; + CornerEstimate corner{.right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, -2.0f}}; - const Eigen::Vector2f docking = planner.estimate_docking_position(corner, drone_pos); + const Eigen::Vector2f docking = + planner.estimate_docking_position(corner, drone_pos); EXPECT_NEAR(docking.x(), 3.5f, 1e-4f); EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); } -TEST_F(PoolExplorationPlannerTest, FullPipeline_FindsCornerSelectsBestAndEstimatesDocking) { +TEST_F(PoolExplorationPlannerTest, + FullPipeline_FindsCornerSelectsBestAndEstimatesDocking) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; const float drone_heading = 0.0f; @@ -184,16 +184,15 @@ TEST_F(PoolExplorationPlannerTest, FullPipeline_FindsCornerSelectsBestAndEstimat const LineSegment2D right_wall_far = make_line(0.0f, -4.0f, 10.0f, -4.0f); const LineSegment2D front_wall_far = make_line(9.0f, -5.0f, 9.0f, 5.0f); - // Litt støy som ikke skal bli brukt + // Litt støy some ikke skal bli brukt const LineSegment2D noise = make_line(-5.0f, 3.0f, -2.0f, 3.0f); - const std::vector lines{ - right_wall_near, front_wall_near, - right_wall_far, front_wall_far, - noise - }; + const std::vector lines{right_wall_near, front_wall_near, + right_wall_far, front_wall_far, + noise}; - const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + const auto corners = + planner.find_corner_estimates(lines, drone_pos, drone_heading); ASSERT_GE(corners.size(), 2u); @@ -207,7 +206,8 @@ TEST_F(PoolExplorationPlannerTest, FullPipeline_FindsCornerSelectsBestAndEstimat EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); } -TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_AcceptsApproximatelyPerpendicularWalls) { +TEST_F(PoolExplorationPlannerTest, + FindCornerEstimates_AcceptsApproximatelyPerpendicularWalls) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; const float drone_heading = 0.0f; @@ -217,9 +217,10 @@ TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_AcceptsApproximatelyPerpe const std::vector lines{right_wall, front_wall}; - const auto corners = planner.find_corner_estimates(lines, drone_pos, drone_heading); + const auto corners = + planner.find_corner_estimates(lines, drone_pos, drone_heading); EXPECT_EQ(corners.size(), 1u); } -} // namespace vortex::pool_exploration::test \ No newline at end of file +} // namespace vortex::pool_exploration::test diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp index 34e0f77..a2759fb 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp @@ -1,11 +1,11 @@ #include -#include -#include #include #include +#include +#include - -using namespace vortex::pool_exploration; +using vortex::pool_exploration::PoolExplorationPlanner; +using vortex::pool_exploration::PoolExplorationPlannerConfig; /* TEST(PoolExplorationTest, DrawLinesIdentityTransform) { @@ -23,7 +23,7 @@ TEST(PoolExplorationTest, DrawLinesIdentityTransform) // Identitet transform Eigen::Matrix4f identity = Eigen::Matrix4f::Identity(); - // Kjør funksjonen som tegner linjene + // Kjør funksjonen some tegner linjene map.setLineSegmentInMapFrame(array_msg, identity); const auto& grid = map.grid(); @@ -56,7 +56,7 @@ TEST(PoolExplorationTest, GridInitialization) EXPECT_EQ(grid.info.height, 10); EXPECT_EQ(grid.data.size(), 100); - // Alle celler skal være -1 (unknown) + // Alle cellar skal være -1 (unknown) for (auto cell : grid.data) { EXPECT_EQ(cell, -1); } @@ -120,10 +120,10 @@ TEST(PoolExplorationTest, MultipleLinesIdentityTransform) const auto& grid = map.grid(); - // Enkel sjekk: se at start- og sluttpunkt for hver linje er satt til OCCUPIED - auto check_cell = [&](double x, double y) { - int gx = static_cast((x - grid.info.origin.position.x) / grid.info.resolution); - int gy = static_cast((y - grid.info.origin.position.y) / grid.info.resolution); + // Enkel sjekk: se at start- og sluttpunkt for hver linje er satt til +OCCUPIED auto check_cell = [&](double x, double y) { int gx = +static_cast((x - grid.info.origin.position.x) / grid.info.resolution); int +gy = static_cast((y - grid.info.origin.position.y) / grid.info.resolution); int index = gy * grid.info.width + gx; EXPECT_EQ(grid.data[index], OCCUPIED); }; @@ -164,11 +164,11 @@ TEST(GeometryTest, LineAngleDifference) { Eigen::Vector2f p0(0,0); Eigen::Vector2f p1(1,1); - float heading = 0.0f; // Drone peker mot x-akse + float heading = 0.0f; // Drone peker not x-akse float diff = map.lineAngleDifference({p0,p1}, heading); - // Linje 45 grader mot x-aksen + // Linje 45 grader not x-aksen EXPECT_NEAR(diff, M_PI/4, 1e-6); } @@ -211,4 +211,4 @@ TEST(GeometryTest, AngleBetweenLines) { EXPECT_NEAR(angle, 0.0, 1e-6); } -*/ \ No newline at end of file +*/ From 08aca1eb55dfb79751dbaf8f44d58fa634c04bb7 Mon Sep 17 00:00:00 2001 From: Sina Date: Tue, 31 Mar 2026 16:44:39 +0200 Subject: [PATCH 04/10] separated package from mapping --- .../docking_position_estimator/CMakeLists.txt | 90 ++++++ .../config/docking_position_estimator.yaml | 36 +++ .../docking_position_estimator.hpp | 293 ++++++++++++++++++ .../docking_position_estimator_ros.hpp | 233 ++++++++++++++ .../launch/pool_exploration.launch.py | 28 ++ .../docking_position_estimator/package.xml | 28 ++ .../src/docking_position_estimator.cpp | 282 +++++++++++++++++ .../src/docking_position_estimator_ros.cpp | 286 +++++++++++++++++ .../test/CMakeLists.txt | 20 ++ .../test/test_docking_position_estimator.cpp | 288 +++++++++++++++++ 10 files changed, 1584 insertions(+) create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/CMakeLists.txt create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/launch/pool_exploration.launch.py create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/package.xml create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/test/CMakeLists.txt create mode 100644 mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp diff --git a/mission/tacc/subsea_docking/docking_position_estimator/CMakeLists.txt b/mission/tacc/subsea_docking/docking_position_estimator/CMakeLists.txt new file mode 100644 index 0000000..37f183a --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.8) +project(docking_position_estimator) + +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 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(message_filters REQUIRED) +find_package(visualization_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories(include) + +set(LIB_NAME ${PROJECT_NAME}_lib) +add_library(${LIB_NAME} SHARED + src/docking_position_estimator.cpp + src/docking_position_estimator_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + tf2_geometry_msgs + tf2_eigen + geometry_msgs + vortex_msgs + nav_msgs + vortex_utils + vortex_utils_ros + Eigen3 + spdlog + fmt + message_filters + #for testing: + visualization_msgs +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::docking_position_estimator::DockingPositionEstimatorNode" + 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 + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml new file mode 100644 index 0000000..11e57bc --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml @@ -0,0 +1,36 @@ +/**: + ros__parameters: + # Topics + line_sub_topic: "/line_detection/line_segments" + pose_sub_topic: "/pose" + sonar_info_sub_topic: "/moby/fls/sonar_info" + debug_topic: "/debug" + + # Publish + # publish_rate_ms: 200 #not used? + + # Frames / TF + odom_frame: "moby/odom" + # base_frame: "moby/base_link" # not used + # sonar_frame: "moby/sonar_link" # not used + + # Corner/docking params + min_wall_distance_m: 0.5 + max_wall_distance_m: 10.0 + + parallel_heading_angle_threshold_rad: 0.4 + perpendicular_heading_angle_threshold_rad: 1.35 + min_corner_angle_rad: 1.50 + max_corner_angle_rad: 1.70 + right_wall_offset_m: 2.5 #offset along right wall + far_wall_offset_m: 4.0 #offset along far wall + + right_wall_max_y_m: 0.4 + far_wall_min_x_m: 0.5 + + # choose_right_corner: 1 #not used yet + + # Waypoint (not used now) + switching_threshold: 0.5 + overwrite_prior_waypoints: true + take_priority: true \ No newline at end of file diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp new file mode 100644 index 0000000..1be6df7 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -0,0 +1,293 @@ +#ifndef DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ +#define DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ + +#include +#include +#include +#include +#include + +namespace vortex::docking_position_estimator { + +/** + * @brief Represents a candidate docking corner formed by a right wall and a far + * wall. + * + * A corner estimate consists of two detected wall segments and their + * intersection point in the reference frame. + */ +struct CornerEstimate { + /** @brief Line segment classified as the right wall. */ + vortex::utils::types::LineSegment2D right_wall; + + /** @brief Line segment classified as the far wall. */ + vortex::utils::types::LineSegment2D far_wall; + + /** @brief Intersection point between the two walls. */ + Eigen::Vector2f corner_point; +}; + +/** + * @brief Classification result for a detected wall segment. + */ +enum class WallClassification { + /** @brief Wall segment classified as the right wall. */ + RightWall, + + /** @brief Wall segment classified as the left wall. (for future use)*/ + LeftWall, + + /** @brief Wall segment classified as the far wall. */ + FarWall, + + /** @brief Wall segment does not satisfy any classification rule. */ + Rejected +}; + +/** + * @brief Convert a Point2D to an Eigen 2D vector. + * + * @param point Input point. + * @return Equivalent Eigen::Vector2f. + */ +inline Eigen::Vector2f to_eigen(const vortex::utils::types::Point2D point) { + return {point.x, point.y}; +} + +/** + * @brief Configuration parameters for the docking position estimator. + * + * These parameters control wall classification, corner validation, and docking + * point estimation. + */ +struct DockingPositionEstimatorConfig { + /** @brief Minimum allowed distance from the drone to a candidate wall + * projection [m]. */ + float min_wall_distance_m; + + /** @brief Maximum allowed distance from the drone to a candidate wall + * projection [m]. */ + float max_wall_distance_m; + + /** @brief Maximum angle between heading and a wall for it to be considered + * parallel [rad]. */ + float parallel_heading_angle_threshold_rad; + + /** @brief Minimum angle between heading and a wall for it to be considered + * perpendicular [rad]. */ + float perpendicular_heading_angle_threshold_rad; + + /** @brief Minimum allowed angle between right wall and far wall for a valid + * corner [rad]. */ + float min_corner_angle_rad; + + /** @brief Maximum allowed angle between right wall and far wall for a valid + * corner [rad]. */ + float max_corner_angle_rad; + + /** @brief Maximum y-value used when checking whether a projection belongs + * to the right wall. */ + float right_wall_max_y_m; + + /** @brief Minimum x-value used when checking whether a projection belongs + * to the far wall. */ + float far_wall_min_x_m; + + /** @brief Offset from the right wall when estimating docking position [m]. + */ + float right_wall_offset_m; + + /** @brief Offset from the far wall when estimating docking position [m]. */ + float far_wall_offset_m; + + /** @brief + * Selects whether to prefer the right-side corner in the mission logic. + */ + + // int choose_right_corner; // not used yet +}; + +/** + * @brief Estimator for detecting wall corners and estimating a docking position. + * + * The estimator classifies detected line segments as candidate pool walls, + * combines compatible wall pairs into corner estimates, and computes a docking + * point from the selected corner. + */ +class DockingPositionEstimator { + public: + /** + * @brief Construct a new estimator instance. + * + * @param config Estimator configuration parameters. + */ + explicit DockingPositionEstimator(const DockingPositionEstimatorConfig& config); + + /** + * @brief Find all valid corner estimates from a set of detected line + * segments. + * + * Each line segment is first classified as a candidate wall. Candidate + * right walls and far walls are then paired, and their intersection is + * checked. A pair is accepted as a corner if the angle between the two + * walls lies within the configured corner-angle interval. + * + * @param lines Input line segments expressed in the reference frame. + * @param drone_pos Current drone position in the same frame as the lines. + * @param drone_heading Current drone heading [rad]. + * @return Vector of valid corner estimates. + */ + // EVT LAGE FOR VENSTRE VEGG, TO DO + std::vector find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& + drone_pos, // sjekk at får inn pos og heading riktig? + float drone_heading) const; + + /** + * @brief Select the best corner estimate among all candidates. + * + * The current implementation selects the corner whose intersection point is + * closest to the drone. + * + * @param possible_corners All valid corner candidates. + * @param drone_pos Current drone position in the reference frame. + * @return Best corner estimate. + */ + CornerEstimate select_best_corner( + const std::vector& possible_corners, + const Eigen::Vector2f& drone_pos) const; + + /** + * @brief Estimate a docking position from a validated corner estimate. + * + * The docking point is computed by offsetting the detected corner along the + * inward-facing normals of the right wall and far wall. + * + * @param estimated_corner Corner estimate containing the two wall segments + * and their intersection. + * @param drone_pos Current drone position in the same frame as the corner + * estimate. + * @return Estimated docking position in the reference frame. + */ + Eigen::Vector2f estimate_docking_position( + const CornerEstimate& estimated_corner, + const Eigen::Vector2f& drone_pos) const; + + private: + /** + * @brief Classify a line segment as a right wall, far wall, or rejected. + * + * A line is classified using: + * - the distance from the drone to the orthogonal projection on the line, + * - the position of that projection relative to the expected mission + * geometry, + * - and the unsigned angle between the wall direction and drone heading. + * + * Right-wall candidates must satisfy the configured right-side projection + * test and be approximately parallel to the drone heading. + * + * Far-wall candidates must satisfy the configured forward projection test + * and be approximately perpendicular to the drone heading. + * + * @param line Input line segment. + * @param drone_pos Current drone position. + * @param drone_heading Current drone heading [rad]. + * @return Wall classification result. + */ + WallClassification classify_wall( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos, + float drone_heading) const; + + /** + * @brief Project a point orthogonally onto the infinite line defined by a + * line segment. + * + * @param drone_pos Point to be projected. + * @param line Line segment defining the projection line. + * @return Projected point in the same frame. + */ + // PROJEKSJONSFORMELEN, TO DO + Eigen::Vector2f project_drone_onto_line( + const Eigen::Vector2f& drone_pos, // punktet some projiseres + const vortex::utils::types::LineSegment2D& line) const; + + /** + * @brief Compute the smallest unsigned angle between a wall segment and the + * drone heading. + * + * The wall direction is compared to the drone heading direction using the + * absolute dot product, making the result independent of the line endpoint + * ordering. + * + * The returned angle lies in [0, π/2]: + * - ≈ 0 rad → wall is parallel to heading + * - ≈ π/2 rad → wall is perpendicular to heading + * + * If the segment is degenerate (near-zero length), +∞ is returned. + * The dot product is clamped to [0, 1] to ensure numerical stability in + * acos(). + * + * @param line Wall segment. + * @param drone_heading Drone heading [rad]. + * @return Smallest unsigned angle between wall and heading [rad], or +∞ if + * degenerate. + */ + float angle_between_line_and_heading( + const vortex::utils::types::LineSegment2D& line, + float drone_heading) const; + + /** + * @brief Compute the intersection point of two infinite lines. + * + * The lines are defined by the endpoints of the two input line segments. + * The method returns false if the lines are parallel. + * + * @param line0 First line segment. + * @param line1 Second line segment. + * @param intersection_coordinates Output intersection point. + * @return true if the lines intersect. + * @return false if the lines are parallel. + */ + bool compute_line_intersection( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1, + Eigen::Vector2f& intersection_coordinates) const; + + /** + * @brief Compute the smallest angle between two line segments. + * + * The result is the unsigned smallest angle between the direction vectors + * of the two segments. + * + * @param line0 First line segment. + * @param line1 Second line segment. + * @return Angle between the two lines [rad]. + */ + float angle_between_lines( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1) const; + + /** + * @brief Compute the unit normal of a line pointing towards a reference + * point. + * + * This is used when computing a docking position offset from a detected + * wall. + * + * @param line Input line segment. + * @param drone_pos Reference point used to choose normal direction. + * @return Unit normal pointing towards the reference point. + */ + Eigen::Vector2f compute_normal_towards_point( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos) const; + + /** @brief Estimator configuration. */ + DockingPositionEstimatorConfig config_; +}; + +} // namespace vortex::docking_position_estimator + +#endif // DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp new file mode 100644 index 0000000..142c9d4 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp @@ -0,0 +1,233 @@ +#ifndef DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_ROS_HPP_ +#define DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_ROS_HPP_ + +#include +#include +#include + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include +#include // til testing + +namespace vortex::docking_position_estimator { + +/** +* @brief ROS 2 node for estimating a docking position from sonar line detections. +* +* This node provides the ROS interface for the docking position estimator. +* It subscribes to detected sonar line segments, drone pose, and sonar +* image metadata, transforms the incoming line segments into the odometry +* frame, and classifies them as candidate walls relative to the drone. +* +* From the classified walls, the node estimates corner candidates by +* intersecting right-wall and far-wall segments. The most suitable corner +* is selected, and a docking position is computed by offsetting from the +* corner along the wall normals. +* +* If a valid docking estimate is found, it is sent through the SendPose service. +*/ +class DockingPositionEstimatorNode : public rclcpp::Node { + public: + /** + * @brief Construct a new DockingPositionEstimatorNode. + * + * The constructor initializes parameters, ROS interfaces, and the estimator. + * + * @param options ROS node options. + */ + explicit DockingPositionEstimatorNode(const rclcpp::NodeOptions& options); + + /** @brief Default destructor. */ + ~DockingPositionEstimatorNode() = default; + + private: + // setup + + /** + * @brief Declare and load ROS parameters used by the node. + * + * This includes frame names, topic names, and waypoint-related parameters. + */ + void setup_parameters(); + + /** + * @brief Create subscribers, TF utilities, and service clients. + * + * This method initializes: + * - TF buffer and listener, + * - line, pose, and sonar-info subscriptions, + * - TF message filter for line detections, + * - docking pose service client. + */ + void setup_publishers_and_subscribers(); + + /** + * @brief Create and configure the docking position estimator. + * + * Estimator configuration values are loaded from ROS parameters and used to + * construct the internal estimator instance. + */ + void setup_estimator(); + + // callbacks + + /** + * @brief Callback for drone pose updates. + * + * Updates the internal drone state estimate used by the estimator. + * + * @param msg Drone pose message with covariance. + */ + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& + msg); + + /** + * @brief Callback for sonar image metadata updates. + * + * Stores the most recent sonar information needed to convert detected line + * endpoints from image pixels to metric sonar coordinates. + * + * @param msg Sonar metadata message. + */ + void sonar_info_callback( + const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg); + + /** + * @brief Callback for detected line segments. + * + * Triggered when a line segment message passes the TF message filter. The + * callback attempts to estimate a docking position from the detected lines. + * + * @param msg Array of detected 2D line segments. + */ + void line_callback( + const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg); + + // helpers + + /** + * @brief Estimate and send a docking position from detected line segments. + * + * The input segments are transformed into the odometry frame, passed to the + * estimator for wall classification and corner detection, and then used to + * compute a docking position. If a valid estimate is found, it is sent + * through the SendPose service. + * + * @param msg Array of detected 2D line segments. + */ + void estimate_and_send_docking_waypoint( + const vortex_msgs::msg::LineSegment2DArray& msg); + + /** + * @brief Transform sonar-detected 2D segments into a target frame. + * + * Each line endpoint is first converted from pixel coordinates to sonar + * metric coordinates using the latest sonar metadata. The resulting points + * are then transformed by the provided homogeneous transform. + * + * @param msg Input line segment array. + * @param T_target_src Homogeneous transform from source frame to target + * frame. + * @return Vector of transformed 2D line segments in the target frame. + */ + std::vector transform_segments_2d( + const vortex_msgs::msg::LineSegment2DArray& msg, + const Eigen::Matrix4f& T_target_src); + + /** + * @brief Send a docking position estimate through the SendPose service. + * + * A single pose is created from the estimated docking position and sent + * in the odometry frame. + * + * @param docking_estimate Estimated docking position in the odometry frame. + */ + void send_docking_waypoint(const Eigen::Vector2f& docking_estimate); + + // TIL TESTING + void publish_docking_marker(const Eigen::Vector2f& docking); + + /** @brief Topic name for detected sonar line segments. */ + std::string line_sub_topic_; + + /** @brief Topic name for vehicle pose estimates. */ + std::string pose_sub_topic_; + + /** @brief Topic name for sonar image metadata. */ + std::string sonar_info_sub_topic_; + + // TIL TESTING + std::string debug_topic_; + + /** @brief Odometry frame used as reference frame for estimation. */ + std::string odom_frame_; + + /** @brief True once a docking pose has been sent. */ + bool waypoint_sent_{false}; + + /** @brief Latest known vehicle pose and heading used by the estimator. */ + vortex::utils::types::PoseEuler drone_state_; + + /** @brief Most recent sonar metadata message. */ + vortex_msgs::msg::SonarInfo::ConstSharedPtr latest_sonar_info_; + + /** @brief TF buffer used for transform lookup. */ + std::shared_ptr tf_buffer_; + + /** @brief TF listener attached to the TF buffer. */ + std::shared_ptr tf_listener_; + + /** @brief Subscriber for line segment detections. */ + message_filters::Subscriber line_sub_; + + /** + * @brief TF-aware message filter for line segment detections. + * + * Ensures that line messages are only forwarded once the required transform + * into the target frame is available. + */ + std::shared_ptr< + tf2_ros::MessageFilter> + line_filter_; + + /** @brief Subscriber for vehicle pose updates. */ + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + /** @brief Subscriber for sonar metadata updates. */ + rclcpp::Subscription::SharedPtr + sonar_info_sub_; + + /** @brief Client for sending the docking pose estimate. */ + rclcpp::Client::SharedPtr send_pose_client_; + + /** @brief Docking position estimator used for corner detection and docking + * estimation. */ + std::unique_ptr estimator_; + + // Debug visualization + rclcpp::Publisher::SharedPtr + docking_marker_pub_; // til testing +}; + +} // namespace vortex::docking_position_estimator + +#endif // DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_ROS_HPP_ diff --git a/mission/tacc/subsea_docking/docking_position_estimator/launch/pool_exploration.launch.py b/mission/tacc/subsea_docking/docking_position_estimator/launch/pool_exploration.launch.py new file mode 100644 index 0000000..4ace143 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/launch/pool_exploration.launch.py @@ -0,0 +1,28 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory("pool_exploration"), + "config", + "pool_exploration.yaml", + ) + + return LaunchDescription( + [ + Node( + package='pool_exploration', + executable='pool_exploration_node', + name='pool_exploration', + output='screen', + parameters=[ + config, + {'use_sim_time': True}, + ], + ), + ] + ) diff --git a/mission/tacc/subsea_docking/docking_position_estimator/package.xml b/mission/tacc/subsea_docking/docking_position_estimator/package.xml new file mode 100644 index 0000000..2b69c71 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/package.xml @@ -0,0 +1,28 @@ + + + + docking_position_estimator + 0.0.0 + TODO: Package description + sina + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + tf2_geometry_msgs + tf2_eigen + eigen + vortex_utils + vortex_utils_ros + vortex_msgs + rclcpp_components + message_filters + visualization_msgs + + + ament_cmake + + diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp new file mode 100644 index 0000000..47ab2c0 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -0,0 +1,282 @@ +#include // til testing +// #include +#include + +// #include +#include +#include + +namespace vortex::docking_position_estimator { + +DockingPositionEstimator::DockingPositionEstimator( + const DockingPositionEstimatorConfig& config) + : config_(config) {} + +std::vector DockingPositionEstimator::find_corner_estimates( + const std::vector& lines, + const Eigen::Vector2f& drone_pos, + float drone_heading) const { + std::vector right_wall_candidates; + std::vector far_wall_candidates; + std::vector corner_estimates; + + for (const auto& line : lines) { + const WallClassification classification = + classify_wall(line, drone_pos, drone_heading); + + if (classification == WallClassification::RightWall) { + right_wall_candidates.push_back(line); + } else if (classification == WallClassification::FarWall) { + far_wall_candidates.push_back(line); + } + } + + spdlog::info("==== SUMMARY ===="); + spdlog::info("Right wall candidates: {}", right_wall_candidates.size()); + spdlog::info("Far wall candidates: {}", far_wall_candidates.size()); + + for (const auto& right_wall : right_wall_candidates) { + for (const auto& far_wall : far_wall_candidates) { + Eigen::Vector2f corner_intersection; + if (!compute_line_intersection(right_wall, far_wall, + corner_intersection)) { + continue; + } + + const float wall_angle = angle_between_lines(right_wall, far_wall); + + if (wall_angle >= config_.min_corner_angle_rad && + wall_angle <= config_.max_corner_angle_rad) { + corner_estimates.push_back( + {right_wall, far_wall, corner_intersection}); + } + } + } + + return corner_estimates; +} + +CornerEstimate DockingPositionEstimator::select_best_corner( + const std::vector& possible_corners, + const Eigen::Vector2f& drone_pos) const { + // if (possible_corners.empty()) {throw std::runtime_error("No candidate + // corners available"); } // UNNTAKSHÅNDTERING I ROS I STEDET + + float min_distance = std::numeric_limits::max(); + CornerEstimate best_corner = possible_corners.front(); + + for (const auto& corner : possible_corners) { + float distance = (corner.corner_point - drone_pos).squaredNorm(); + + if (distance < min_distance) { + min_distance = distance; + best_corner = corner; + } + } + return best_corner; +} + +Eigen::Vector2f DockingPositionEstimator::estimate_docking_position( + const CornerEstimate& estimated_corner, + const Eigen::Vector2f& drone_pos) const { + Eigen::Vector2f right_normal = + compute_normal_towards_point(estimated_corner.right_wall, drone_pos); + Eigen::Vector2f far_normal = + compute_normal_towards_point(estimated_corner.far_wall, drone_pos); + + Eigen::Vector2f docking_estimate = + estimated_corner.corner_point + + right_normal * config_.right_wall_offset_m + + far_normal * config_.far_wall_offset_m; + + return docking_estimate; +} + +WallClassification DockingPositionEstimator::classify_wall( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos, + float drone_heading) const { + const Eigen::Vector2f p0 = to_eigen(line.p0); + const Eigen::Vector2f p1 = to_eigen(line.p1); + + const Eigen::Vector2f dir = p1 - p0; + if (dir.squaredNorm() < 1e-6f) { // ENDRE KRAV TIL LENGDE PÅ LINJE? + spdlog::info(" -> REJECTED: degenerate line"); + return WallClassification::Rejected; + } + // TIL LOGGING + spdlog::info("Line: ({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})", p0.x(), p0.y(), + p1.x(), p1.y()); + // + const Eigen::Vector2f projection = project_drone_onto_line(drone_pos, line); + const Eigen::Vector2f rel = projection - drone_pos; + + const float distance = rel.norm(); + // TIL LOGGING + spdlog::info(" Projection: ({:.2f}, {:.2f})", projection.x(), + projection.y()); + spdlog::info(" Rel: ({:.2f}, {:.2f}), dist: {:.2f}", rel.x(), rel.y(), + distance); + // + if (distance < config_.min_wall_distance_m || + distance > config_.max_wall_distance_m) { + spdlog::info(" -> REJECTED by distance"); + return WallClassification::Rejected; + } + + const float heading_wall_angle = + angle_between_line_and_heading(line, drone_heading); + spdlog::info(" Angle: {:.2f} rad ({:.1f} deg)", heading_wall_angle, + heading_wall_angle * 180.0 / M_PI); + + Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); + Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); + float forward_dist = rel.dot(forward); + float right_dist = rel.dot(right); + + // RIGHT WALL: projection has negative y-value in NED, wall is approximately + // parallel to heading + if ( // projection.y() < config_.right_wall_max_y_m && + right_dist < 0 && //USIKKER PÅ DENNE? TO DO + heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { + spdlog::info(" -> Classified as RIGHT candidate"); + return WallClassification::RightWall; + } + + // FAR WALL: projection has positive x-value, wall is approximately + // perpendicular to heading + if ( // projection.x() > config_.far_wall_min_x_m && + forward_dist > 0 && + heading_wall_angle > + config_.perpendicular_heading_angle_threshold_rad) { + spdlog::info(" -> Classified as FAR candidate"); + return WallClassification::FarWall; + } + spdlog::info(" -> REJECTED by geometry"); + return WallClassification::Rejected; +} + +Eigen::Vector2f DockingPositionEstimator::project_drone_onto_line( + const Eigen::Vector2f& drone_pos, + const vortex::utils::types::LineSegment2D& line) const { + const Eigen::Vector2f p0 = to_eigen(line.p0); + const Eigen::Vector2f p1 = to_eigen(line.p1); + + const Eigen::Vector2f line_direction = p1 - p0; + const float line_length_squared = line_direction.squaredNorm(); + + const float projection_parameter = + (drone_pos - p0).dot(line_direction) / line_length_squared; + const Eigen::Vector2f projection_point = + p0 + projection_parameter * line_direction; + + return projection_point; +} + +float DockingPositionEstimator::angle_between_line_and_heading( + const vortex::utils::types::LineSegment2D& line, + float drone_heading) const { + Eigen::Vector2f wall_direction = to_eigen(line.p1) - to_eigen(line.p0); + const float wall_length = wall_direction.norm(); + + if (wall_length < 1e-6f) { + return std::numeric_limits::infinity(); + } + + wall_direction /= wall_length; + Eigen::Vector2f heading_direction(std::cos(drone_heading), + std::sin(drone_heading)); + + float cos_heading_wall_angle = std::abs(wall_direction.dot( + heading_direction)); // rekkefølge p0 og p1 urelevant + cos_heading_wall_angle = + std::clamp(cos_heading_wall_angle, 0.0f, 1.0f); // unngå små avvik + + return std::acos(cos_heading_wall_angle); +} + +bool DockingPositionEstimator::compute_line_intersection( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1, + Eigen::Vector2f& intersection) const { + const Eigen::Vector2f p00 = to_eigen(line0.p0); + const Eigen::Vector2f p01 = to_eigen(line0.p1); + const Eigen::Vector2f p10 = to_eigen(line1.p0); + const Eigen::Vector2f p11 = to_eigen(line1.p1); + + float determinant = (p00.x() - p01.x()) * (p10.y() - p11.y()) - + (p00.y() - p01.y()) * (p10.x() - p11.x()); + + if (std::abs(determinant) < 1e-6f) + return false; // parallelle linjer + + intersection.x() = + ((p00.x() * p01.y() - p00.y() * p01.x()) * (p10.x() - p11.x()) - + (p00.x() - p01.x()) * (p10.x() * p11.y() - p10.y() * p11.x())) / + determinant; + intersection.y() = + ((p00.x() * p01.y() - p00.y() * p01.x()) * (p10.y() - p11.y()) - + (p00.y() - p01.y()) * (p10.x() * p11.y() - p10.y() * p11.x())) / + determinant; + return true; +} + +float DockingPositionEstimator::angle_between_lines( + const vortex::utils::types::LineSegment2D& line0, + const vortex::utils::types::LineSegment2D& line1) const { + Eigen::Vector2f line0_direction = to_eigen(line0.p1) - to_eigen(line0.p0); + + Eigen::Vector2f line1_direction = to_eigen(line1.p1) - to_eigen(line1.p0); + + const float line0_length = line0_direction.norm(); + const float line1_length = line1_direction.norm(); + + if (line0_length < 1e-6f || line1_length < 1e-6f) { + return std::numeric_limits::infinity(); + } + + line0_direction /= line0_length; + line1_direction /= line1_length; + + float cos_angle_between_lines = + std::abs(line0_direction.dot(line1_direction)); + + cos_angle_between_lines = std::clamp(cos_angle_between_lines, 0.0f, 1.0f); + + return std::acos(cos_angle_between_lines); +} + +Eigen::Vector2f DockingPositionEstimator::compute_normal_towards_point( + const vortex::utils::types::LineSegment2D& line, + const Eigen::Vector2f& drone_pos) const { + Eigen::Vector2f p0 = to_eigen(line.p0); + Eigen::Vector2f p1 = to_eigen(line.p1); + + Eigen::Vector2f line_direction = p1 - p0; + + if (line_direction.x() < 0.0f || + (line_direction.x() == 0.0f && line_direction.y() < 0.0f)) { + std::swap(p0, p1); + line_direction = p1 - p0; + } + + const float line_length = line_direction.norm(); + if (line_length < 1e-6f) { + return Eigen::Vector2f::Zero(); + } + + line_direction /= line_length; + + Eigen::Vector2f normal(-line_direction.y(), line_direction.x()); + + const Eigen::Vector2f line_midpoint = 0.5f * (p0 + p1); + const Eigen::Vector2f midpoint_to_drone = drone_pos - line_midpoint; + + if (midpoint_to_drone.dot(normal) < 0.0f) { + normal = -normal; + } + + return normal; +} + +} // namespace vortex::docking_position_estimator diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp new file mode 100644 index 0000000..e0f61d4 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -0,0 +1,286 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +// #include +#include +#include // ERSTATTE MED VORTEX? ^^^ +#include +#include "docking_position_estimator/docking_position_estimator.hpp" + +namespace vortex::docking_position_estimator { + +DockingPositionEstimatorNode::DockingPositionEstimatorNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("docking_position_estimator_node", options) { + setup_parameters(); + setup_publishers_and_subscribers(); + setup_estimator(); +} + +void DockingPositionEstimatorNode::setup_parameters() { + // Frames + odom_frame_ = this->declare_parameter("odom_frame"); + + // Topics + line_sub_topic_ = this->declare_parameter("line_sub_topic"); + pose_sub_topic_ = this->declare_parameter("pose_sub_topic"); + sonar_info_sub_topic_ = + this->declare_parameter("sonar_info_sub_topic"); + debug_topic_ = + this->declare_parameter("debug_topic"); // For testing +} + +void DockingPositionEstimatorNode::setup_publishers_and_subscribers() { + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + const auto qos_sub = + rclcpp::SensorDataQoS(); // standard for sensordata, ta inn vortex sin + // i stedet?? + auto sub_options = rclcpp::SubscriptionOptions(); + + line_sub_.subscribe(this, line_sub_topic_, qos_sub.get_rmw_qos_profile(), + sub_options); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_sub_topic_, qos_sub, + std::bind(&DockingPositionEstimatorNode::pose_callback, this, + std::placeholders::_1)); + + sonar_info_sub_ = this->create_subscription( + sonar_info_sub_topic_, qos_sub, + [this](const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg) { + this->sonar_info_callback(msg); + }); + + line_filter_ = std::make_shared< + tf2_ros::MessageFilter>( + line_sub_, *tf_buffer_, odom_frame_, 10, + this->get_node_logging_interface(), this->get_node_clock_interface()); + + line_filter_->registerCallback(std::bind( + &DockingPositionEstimatorNode::line_callback, this, std::placeholders::_1)); + + send_pose_client_ = this->create_client( + "/docking_position_estimator/docking_pose"); + + docking_marker_pub_ = + this->create_publisher( + debug_topic_, 10); // til testing +} + +void DockingPositionEstimatorNode::setup_estimator() { + DockingPositionEstimatorConfig config{}; + + config.min_wall_distance_m = + this->declare_parameter("min_wall_distance_m"); + config.max_wall_distance_m = + this->declare_parameter("max_wall_distance_m"); + config.parallel_heading_angle_threshold_rad = + this->declare_parameter("parallel_heading_angle_threshold_rad"); + config.perpendicular_heading_angle_threshold_rad = + this->declare_parameter( + "perpendicular_heading_angle_threshold_rad"); + + config.min_corner_angle_rad = + this->declare_parameter("min_corner_angle_rad"); + config.max_corner_angle_rad = + this->declare_parameter("max_corner_angle_rad"); + config.far_wall_min_x_m = + this->declare_parameter("far_wall_min_x_m"); + config.right_wall_max_y_m = + this->declare_parameter("right_wall_max_y_m"); + // config.choose_right_corner = this->declare_parameter("choose_right_corner"); //not used yet + config.right_wall_offset_m = + this->declare_parameter("right_wall_offset_m"); + config.far_wall_offset_m = + this->declare_parameter("far_wall_offset_m"); + + estimator_ = std::make_unique(config); +} + +void DockingPositionEstimatorNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg) { + drone_state_.x = msg->pose.pose.position.x; + drone_state_.y = msg->pose.pose.position.y; + drone_state_.z = msg->pose.pose.position.z; + + Eigen::Quaterniond q; + tf2::fromMsg(msg->pose.pose.orientation, q); + Eigen::Vector3d rpy = vortex::utils::math::quat_to_euler(q); + drone_state_.yaw = rpy.z(); +} + +void DockingPositionEstimatorNode::sonar_info_callback( + const vortex_msgs::msg::SonarInfo::ConstSharedPtr& msg) { + latest_sonar_info_ = msg; +} + +void DockingPositionEstimatorNode::line_callback( + const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg) { + estimate_and_send_docking_waypoint(*msg); + // drawSegmentsInMapFrame(*msg); +} + +void DockingPositionEstimatorNode::estimate_and_send_docking_waypoint( + const vortex_msgs::msg::LineSegment2DArray& msg) { + geometry_msgs::msg::TransformStamped tf_stamped; + + try { + tf_stamped = tf_buffer_->lookupTransform( + odom_frame_, msg.header.frame_id, msg.header.stamp); + } catch (const tf2::TransformException& ex) { + spdlog::warn("[PoolExploration] TF failed {} -> {}: {}", + msg.header.frame_id, odom_frame_, ex.what()); + return; + } + + const Eigen::Affine3d T = tf2::transformToEigen(tf_stamped.transform); + const Eigen::Matrix4f T_odom_src = T.matrix().cast(); + + auto segs = transform_segments_2d(msg, T_odom_src); // NB ENDRE NAVN <3 + + Eigen::Vector2f drone_pos = {drone_state_.x, drone_state_.y}; + float drone_heading = drone_state_.yaw; + + auto corners = + estimator_->find_corner_estimates(segs, drone_pos, drone_heading); + + if (corners.empty()) { + spdlog::info( + "[PoolExploration] No valid corners -> no docking estimate"); + return; + } + + CornerEstimate best_corner = + estimator_->select_best_corner(corners, drone_pos); + + Eigen::Vector2f docking = + estimator_->estimate_docking_position(best_corner, drone_pos); + + publish_docking_marker(docking); // til testing + send_docking_waypoint(docking); + + spdlog::info("[PoolExploration] Docking estimate (odom): x={} y={}", + docking.x(), docking.y()); +} + +std::vector +DockingPositionEstimatorNode::transform_segments_2d( + const vortex_msgs::msg::LineSegment2DArray& msg, + const Eigen::Matrix4f& T_target_src) { + std::vector segments; + segments.reserve(msg.lines.size()); + + if (!latest_sonar_info_) { + spdlog::warn("[PoolExploration] No sonar image info available yet"); + return segments; + } + + vortex::utils::types::SonarInfo sonar_info; + sonar_info.meters_per_pixel_x = latest_sonar_info_->meters_per_pixel_x; + sonar_info.meters_per_pixel_y = latest_sonar_info_->meters_per_pixel_y; + sonar_info.image_width = latest_sonar_info_->width; + sonar_info.image_height = latest_sonar_info_->height; + + for (const auto& line : msg.lines) { + const auto p0 = + sonar_info.pixel_index_to_sonar_metric(line.p0.x, line.p0.y); + const auto p1 = + sonar_info.pixel_index_to_sonar_metric(line.p1.x, line.p1.y); + + const Eigen::Vector4f p0_sonar(static_cast(p0.y), + static_cast(p0.x), 0.0f, 1.0f); + const Eigen::Vector4f p1_sonar(static_cast(p1.y), + static_cast(p1.x), 0.0f, 1.0f); + + const Eigen::Vector4f p0_target = T_target_src * p0_sonar; + const Eigen::Vector4f p1_target = T_target_src * p1_sonar; + + vortex::utils::types::LineSegment2D seg; + seg.p0 = {p0_target.x(), p0_target.y()}; + seg.p1 = {p1_target.x(), p1_target.y()}; + + spdlog::info( + "[PoolExploration] Line received: ({:.2f}, {:.2f}) -> ({:.2f}, " + "{:.2f})", // til debugging + seg.p0.x, seg.p0.y, seg.p1.x, seg.p1.y); + + segments.push_back(seg); + } + return segments; +} + +void DockingPositionEstimatorNode::send_docking_waypoint( + const Eigen::Vector2f& docking_estimate) { + if (waypoint_sent_) { + return; + } + + if (!send_pose_client_->service_is_ready()) { + spdlog::warn("[PoolExploration] SendPose service not available"); + return; + } + + auto request = std::make_shared(); + + request->pose.header.stamp = this->now(); + request->pose.header.frame_id = odom_frame_; + request->pose.pose.position.x = docking_estimate.x(); + request->pose.pose.position.y = docking_estimate.y(); + request->pose.pose.position.z = 0.0; + request->pose.pose.orientation.w = 1.0; + + send_pose_client_->async_send_request(request); + waypoint_sent_ = true; + spdlog::info("[PoolExploration] Docking pose sent: x={} y={}", + docking_estimate.x(), docking_estimate.y()); +} + +void DockingPositionEstimatorNode::publish_docking_marker( + const Eigen::Vector2f& docking) // til testing +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = odom_frame_; + marker.header.stamp = this->now(); + + marker.ns = "docking_debug"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = docking.x(); + marker.pose.position.y = docking.y(); + marker.pose.position.z = 0.0; + + marker.pose.orientation.w = 1.0; + + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + docking_marker_pub_->publish(marker); +} + +RCLCPP_COMPONENTS_REGISTER_NODE( + vortex::docking_position_estimator::DockingPositionEstimatorNode) + +} // namespace vortex::docking_position_estimator diff --git a/mission/tacc/subsea_docking/docking_position_estimator/test/CMakeLists.txt b/mission/tacc/subsea_docking/docking_position_estimator/test/CMakeLists.txt new file mode 100644 index 0000000..4d1b68b --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/test/CMakeLists.txt @@ -0,0 +1,20 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_docking_position_estimator + test_docking_position_estimator.cpp +) + +target_include_directories(test_docking_position_estimator PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include +) + +ament_target_dependencies(test_docking_position_estimator + rclcpp + nav_msgs + vortex_msgs + Eigen3 +) + +target_link_libraries(test_docking_position_estimator + ${PROJECT_NAME}_lib +) diff --git a/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp new file mode 100644 index 0000000..b19a762 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp @@ -0,0 +1,288 @@ +#include + +#include +#include +#include + +#include +#include + +namespace vortex::docking_position_estimator::test { + +using vortex::utils::types::LineSegment2D; +using vortex::utils::types::Point2D; + +namespace { + +constexpr float kEps = 1e-4f; + +Point2D make_point(float x, float y) { + return Point2D{.x = x, .y = y}; +} + +LineSegment2D make_line(float x0, float y0, float x1, float y1) { + return LineSegment2D{ + .p0 = make_point(x0, y0), + .p1 = make_point(x1, y1), + }; +} + +DockingPositionEstimatorConfig make_config() { + DockingPositionEstimatorConfig config{}; + + config.min_wall_distance_m = 0.2f; + config.max_wall_distance_m = 10.0f; + + // Merk: disse brukes ikke i classify_wall() akkurat nå, + // men settes likevel for komplett config. + config.right_wall_max_y_m = 0.0f; + config.far_wall_min_x_m = 0.0f; + + config.parallel_heading_angle_threshold_rad = + 20.0f * static_cast(M_PI) / 180.0f; + config.perpendicular_heading_angle_threshold_rad = + 70.0f * static_cast(M_PI) / 180.0f; + + config.min_corner_angle_rad = + 70.0f * static_cast(M_PI) / 180.0f; + config.max_corner_angle_rad = + 110.0f * static_cast(M_PI) / 180.0f; + + config.right_wall_offset_m = 1.0f; + config.far_wall_offset_m = 1.5f; + + return config; +} + +} // namespace + +class DockingPositionEstimatorTest : public ::testing::Test { + protected: + DockingPositionEstimatorTest() + : config(make_config()), estimator(config) {} + + DockingPositionEstimatorConfig config; + DockingPositionEstimator estimator; +}; + +// ----------------------------------------------------------------------------- +// NED expectations: +// x = North, y = East +// +// heading = 0 rad -> facing North +// forward = +x +// right = +y +// +// heading = pi/2 -> facing East +// forward = +y +// right = +x +// ----------------------------------------------------------------------------- + + +TEST_F(DockingPositionEstimatorTest, + FacingNorth_RightWallToEast_AndFarWallAhead_ProducesCorner) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; // Facing north in NED + + // Right wall should be to the east of the drone => y > 0 + // and approximately parallel to heading => line parallel to +x. + const LineSegment2D right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f); + + // Far wall should be ahead of the drone => x > 0 + // and approximately perpendicular to heading. + const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const auto corners = + estimator.find_corner_estimates({right_wall, far_wall}, + drone_pos, + drone_heading); + + ASSERT_EQ(corners.size(), 1u); + EXPECT_NEAR(corners.front().corner_point.x(), 5.0f, kEps); + EXPECT_NEAR(corners.front().corner_point.y(), 2.0f, kEps); +} + +TEST_F(DockingPositionEstimatorTest, + FacingNorth_WallToWest_IsNotClassifiedAsRightWall) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; // Facing north in NED + + // This wall is on the left/west side => y < 0 + const LineSegment2D left_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f); + + const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const auto corners = + estimator.find_corner_estimates({left_wall, far_wall}, + drone_pos, + drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(DockingPositionEstimatorTest, + FacingNorth_FarWallBehindDrone_IsRejected) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; // Facing north in NED + + const LineSegment2D right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f); + + // Behind the drone => x < 0 + const LineSegment2D far_wall_behind = + make_line(-5.0f, -5.0f, -5.0f, 5.0f); + + const auto corners = + estimator.find_corner_estimates({right_wall, far_wall_behind}, + drone_pos, + drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(DockingPositionEstimatorTest, + FacingNorth_RightWallOutsideDistanceThreshold_IsRejected) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; // Facing north in NED + + // Too far to the east + const LineSegment2D right_wall_too_far = + make_line(0.0f, 20.0f, 10.0f, 20.0f); + + const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const auto corners = + estimator.find_corner_estimates({right_wall_too_far, far_wall}, + drone_pos, + drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(DockingPositionEstimatorTest, + FacingNorth_ApproximatelyParallelAndPerpendicularWalls_AreAccepted) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; // Facing north in NED + + // Slightly tilted but still close to heading-parallel + const LineSegment2D right_wall = make_line(0.0f, 2.0f, 10.0f, 2.4f); + + // Slightly tilted but still close to perpendicular + const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.4f, 5.0f); + + const auto corners = + estimator.find_corner_estimates({right_wall, far_wall}, + drone_pos, + drone_heading); + + EXPECT_EQ(corners.size(), 1u); +} + +TEST_F(DockingPositionEstimatorTest, + RejectsParallelWalls_WhenNoCornerCanBeFormed) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + // Both are roughly parallel to heading + const LineSegment2D line0 = make_line(0.0f, 2.0f, 10.0f, 2.0f); + const LineSegment2D line1 = make_line(0.0f, 4.0f, 10.0f, 4.0f); + + const auto corners = + estimator.find_corner_estimates({line0, line1}, + drone_pos, + drone_heading); + + EXPECT_TRUE(corners.empty()); +} + +TEST_F(DockingPositionEstimatorTest, + SelectBestCorner_ReturnsClosestCornerToDrone) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + + CornerEstimate near_corner{ + .right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, 2.0f}, + }; + + CornerEstimate far_corner{ + .right_wall = make_line(0.0f, 4.0f, 10.0f, 4.0f), + .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), + .corner_point = Eigen::Vector2f{8.0f, 4.0f}, + }; + + const auto best = + estimator.select_best_corner({far_corner, near_corner}, drone_pos); + + EXPECT_NEAR(best.corner_point.x(), 5.0f, kEps); + EXPECT_NEAR(best.corner_point.y(), 2.0f, kEps); +} + +TEST_F(DockingPositionEstimatorTest, + EstimateDockingPosition_OffsetsFromCornerTowardPoolInterior_FacingNorth) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + + // Corner at (5, 2) + // Right wall: y = 2 + // Far wall: x = 5 + // + // For drone at (0, 0): + // - normal from right wall toward drone = (0, -1) + // - normal from far wall toward drone = (-1, 0) + // + // Expected docking: + // (5, 2) + 1.0*(0, -1) + 1.5*(-1, 0) = (3.5, 1.0) + + CornerEstimate corner{ + .right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), + .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), + .corner_point = Eigen::Vector2f{5.0f, 2.0f}, + }; + + const Eigen::Vector2f docking = + estimator.estimate_docking_position(corner, drone_pos); + + EXPECT_NEAR(docking.x(), 3.5f, kEps); + EXPECT_NEAR(docking.y(), 1.0f, kEps); +} + +TEST_F(DockingPositionEstimatorTest, + FullPipeline_FindsNearestValidCornerAndEstimatesDocking_FacingNorth) { + const Eigen::Vector2f drone_pos{0.0f, 0.0f}; + const float drone_heading = 0.0f; + + const LineSegment2D right_wall_near = + make_line(0.0f, 2.0f, 10.0f, 2.0f); + const LineSegment2D far_wall_near = + make_line(5.0f, -5.0f, 5.0f, 5.0f); + + const LineSegment2D right_wall_far = + make_line(0.0f, 4.0f, 10.0f, 4.0f); + const LineSegment2D far_wall_far = + make_line(9.0f, -5.0f, 9.0f, 5.0f); + + // Noise: behind drone, should not contribute + const LineSegment2D noise = + make_line(-5.0f, -3.0f, -2.0f, -3.0f); + + const std::vector lines{ + right_wall_near, far_wall_near, + right_wall_far, far_wall_far, + noise + }; + + const auto corners = + estimator.find_corner_estimates(lines, drone_pos, drone_heading); + + ASSERT_GE(corners.size(), 2u); + + const auto best = estimator.select_best_corner(corners, drone_pos); + const auto docking = estimator.estimate_docking_position(best, drone_pos); + + EXPECT_NEAR(best.corner_point.x(), 5.0f, kEps); + EXPECT_NEAR(best.corner_point.y(), 2.0f, kEps); + + EXPECT_NEAR(docking.x(), 3.5f, kEps); + EXPECT_NEAR(docking.y(), 1.0f, kEps); +} + +} // namespace vortex::docking_position_estimator::test \ No newline at end of file From d6c5cd3e9008e1d17aa97d4ad5750d24a4424641 Mon Sep 17 00:00:00 2001 From: Sina Date: Tue, 31 Mar 2026 17:44:58 +0200 Subject: [PATCH 05/10] Fix pre-commit formatting --- .../config/docking_position_estimator.yaml | 2 +- .../docking_position_estimator.hpp | 6 +- .../docking_position_estimator_ros.hpp | 37 +-- .../src/docking_position_estimator.cpp | 4 +- .../src/docking_position_estimator_ros.cpp | 11 +- .../test/test_docking_position_estimator.cpp | 78 +++--- .../pool_exploration/test/CMakeLists.txt | 8 +- .../test/test_pool_exploration.cpp | 226 ------------------ 8 files changed, 65 insertions(+), 307 deletions(-) delete mode 100644 mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp diff --git a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml index 11e57bc..695bdd2 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml +++ b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml @@ -33,4 +33,4 @@ # Waypoint (not used now) switching_threshold: 0.5 overwrite_prior_waypoints: true - take_priority: true \ No newline at end of file + take_priority: true diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp index 1be6df7..32e9ab8 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -108,7 +108,8 @@ struct DockingPositionEstimatorConfig { }; /** - * @brief Estimator for detecting wall corners and estimating a docking position. + * @brief Estimator for detecting wall corners and estimating a docking + * position. * * The estimator classifies detected line segments as candidate pool walls, * combines compatible wall pairs into corner estimates, and computes a docking @@ -121,7 +122,8 @@ class DockingPositionEstimator { * * @param config Estimator configuration parameters. */ - explicit DockingPositionEstimator(const DockingPositionEstimatorConfig& config); + explicit DockingPositionEstimator( + const DockingPositionEstimatorConfig& config); /** * @brief Find all valid corner estimates from a set of detected line diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp index 142c9d4..7a55a26 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp @@ -29,26 +29,29 @@ namespace vortex::docking_position_estimator { /** -* @brief ROS 2 node for estimating a docking position from sonar line detections. -* -* This node provides the ROS interface for the docking position estimator. -* It subscribes to detected sonar line segments, drone pose, and sonar -* image metadata, transforms the incoming line segments into the odometry -* frame, and classifies them as candidate walls relative to the drone. -* -* From the classified walls, the node estimates corner candidates by -* intersecting right-wall and far-wall segments. The most suitable corner -* is selected, and a docking position is computed by offsetting from the -* corner along the wall normals. -* -* If a valid docking estimate is found, it is sent through the SendPose service. -*/ + * @brief ROS 2 node for estimating a docking position from sonar line + * detections. + * + * This node provides the ROS interface for the docking position estimator. + * It subscribes to detected sonar line segments, drone pose, and sonar + * image metadata, transforms the incoming line segments into the odometry + * frame, and classifies them as candidate walls relative to the drone. + * + * From the classified walls, the node estimates corner candidates by + * intersecting right-wall and far-wall segments. The most suitable corner + * is selected, and a docking position is computed by offsetting from the + * corner along the wall normals. + * + * If a valid docking estimate is found, it is sent through the SendPose + * service. + */ class DockingPositionEstimatorNode : public rclcpp::Node { public: /** * @brief Construct a new DockingPositionEstimatorNode. * - * The constructor initializes parameters, ROS interfaces, and the estimator. + * The constructor initializes parameters, ROS interfaces, and the + * estimator. * * @param options ROS node options. */ @@ -127,8 +130,8 @@ class DockingPositionEstimatorNode : public rclcpp::Node { * @brief Estimate and send a docking position from detected line segments. * * The input segments are transformed into the odometry frame, passed to the - * estimator for wall classification and corner detection, and then used to - * compute a docking position. If a valid estimate is found, it is sent + * estimator for wall classification and corner detection, and then used to + * compute a docking position. If a valid estimate is found, it is sent * through the SendPose service. * * @param msg Array of detected 2D line segments. diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp index 47ab2c0..7c7e2b7 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -136,8 +136,8 @@ WallClassification DockingPositionEstimator::classify_wall( // RIGHT WALL: projection has negative y-value in NED, wall is approximately // parallel to heading - if ( // projection.y() < config_.right_wall_max_y_m && - right_dist < 0 && //USIKKER PÅ DENNE? TO DO + if ( // projection.y() < config_.right_wall_max_y_m && + right_dist < 0 && // USIKKER PÅ DENNE? TO DO heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as RIGHT candidate"); return WallClassification::RightWall; diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp index e0f61d4..a3d525a 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -18,7 +18,8 @@ namespace vortex::docking_position_estimator { -DockingPositionEstimatorNode::DockingPositionEstimatorNode(const rclcpp::NodeOptions& options) +DockingPositionEstimatorNode::DockingPositionEstimatorNode( + const rclcpp::NodeOptions& options) : rclcpp::Node("docking_position_estimator_node", options) { setup_parameters(); setup_publishers_and_subscribers(); @@ -70,8 +71,9 @@ void DockingPositionEstimatorNode::setup_publishers_and_subscribers() { line_sub_, *tf_buffer_, odom_frame_, 10, this->get_node_logging_interface(), this->get_node_clock_interface()); - line_filter_->registerCallback(std::bind( - &DockingPositionEstimatorNode::line_callback, this, std::placeholders::_1)); + line_filter_->registerCallback( + std::bind(&DockingPositionEstimatorNode::line_callback, this, + std::placeholders::_1)); send_pose_client_ = this->create_client( "/docking_position_estimator/docking_pose"); @@ -102,7 +104,8 @@ void DockingPositionEstimatorNode::setup_estimator() { this->declare_parameter("far_wall_min_x_m"); config.right_wall_max_y_m = this->declare_parameter("right_wall_max_y_m"); - // config.choose_right_corner = this->declare_parameter("choose_right_corner"); //not used yet + // config.choose_right_corner = + // this->declare_parameter("choose_right_corner"); //not used yet config.right_wall_offset_m = this->declare_parameter("right_wall_offset_m"); config.far_wall_offset_m = diff --git a/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp index b19a762..ee37c0a 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp @@ -43,10 +43,8 @@ DockingPositionEstimatorConfig make_config() { config.perpendicular_heading_angle_threshold_rad = 70.0f * static_cast(M_PI) / 180.0f; - config.min_corner_angle_rad = - 70.0f * static_cast(M_PI) / 180.0f; - config.max_corner_angle_rad = - 110.0f * static_cast(M_PI) / 180.0f; + config.min_corner_angle_rad = 70.0f * static_cast(M_PI) / 180.0f; + config.max_corner_angle_rad = 110.0f * static_cast(M_PI) / 180.0f; config.right_wall_offset_m = 1.0f; config.far_wall_offset_m = 1.5f; @@ -58,8 +56,7 @@ DockingPositionEstimatorConfig make_config() { class DockingPositionEstimatorTest : public ::testing::Test { protected: - DockingPositionEstimatorTest() - : config(make_config()), estimator(config) {} + DockingPositionEstimatorTest() : config(make_config()), estimator(config) {} DockingPositionEstimatorConfig config; DockingPositionEstimator estimator; @@ -78,7 +75,6 @@ class DockingPositionEstimatorTest : public ::testing::Test { // right = +x // ----------------------------------------------------------------------------- - TEST_F(DockingPositionEstimatorTest, FacingNorth_RightWallToEast_AndFarWallAhead_ProducesCorner) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; @@ -92,10 +88,8 @@ TEST_F(DockingPositionEstimatorTest, // and approximately perpendicular to heading. const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); - const auto corners = - estimator.find_corner_estimates({right_wall, far_wall}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {right_wall, far_wall}, drone_pos, drone_heading); ASSERT_EQ(corners.size(), 1u); EXPECT_NEAR(corners.front().corner_point.x(), 5.0f, kEps); @@ -112,10 +106,8 @@ TEST_F(DockingPositionEstimatorTest, const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); - const auto corners = - estimator.find_corner_estimates({left_wall, far_wall}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {left_wall, far_wall}, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } @@ -128,13 +120,10 @@ TEST_F(DockingPositionEstimatorTest, const LineSegment2D right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f); // Behind the drone => x < 0 - const LineSegment2D far_wall_behind = - make_line(-5.0f, -5.0f, -5.0f, 5.0f); + const LineSegment2D far_wall_behind = make_line(-5.0f, -5.0f, -5.0f, 5.0f); - const auto corners = - estimator.find_corner_estimates({right_wall, far_wall_behind}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {right_wall, far_wall_behind}, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } @@ -150,10 +139,8 @@ TEST_F(DockingPositionEstimatorTest, const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); - const auto corners = - estimator.find_corner_estimates({right_wall_too_far, far_wall}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {right_wall_too_far, far_wall}, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } @@ -169,10 +156,8 @@ TEST_F(DockingPositionEstimatorTest, // Slightly tilted but still close to perpendicular const LineSegment2D far_wall = make_line(5.0f, -5.0f, 5.4f, 5.0f); - const auto corners = - estimator.find_corner_estimates({right_wall, far_wall}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {right_wall, far_wall}, drone_pos, drone_heading); EXPECT_EQ(corners.size(), 1u); } @@ -186,10 +171,8 @@ TEST_F(DockingPositionEstimatorTest, const LineSegment2D line0 = make_line(0.0f, 2.0f, 10.0f, 2.0f); const LineSegment2D line1 = make_line(0.0f, 4.0f, 10.0f, 4.0f); - const auto corners = - estimator.find_corner_estimates({line0, line1}, - drone_pos, - drone_heading); + const auto corners = estimator.find_corner_estimates( + {line0, line1}, drone_pos, drone_heading); EXPECT_TRUE(corners.empty()); } @@ -217,8 +200,9 @@ TEST_F(DockingPositionEstimatorTest, EXPECT_NEAR(best.corner_point.y(), 2.0f, kEps); } -TEST_F(DockingPositionEstimatorTest, - EstimateDockingPosition_OffsetsFromCornerTowardPoolInterior_FacingNorth) { +TEST_F( + DockingPositionEstimatorTest, + EstimateDockingPosition_OffsetsFromCornerTowardPoolInterior_FacingNorth) { const Eigen::Vector2f drone_pos{0.0f, 0.0f}; // Corner at (5, 2) @@ -250,25 +234,17 @@ TEST_F(DockingPositionEstimatorTest, const Eigen::Vector2f drone_pos{0.0f, 0.0f}; const float drone_heading = 0.0f; - const LineSegment2D right_wall_near = - make_line(0.0f, 2.0f, 10.0f, 2.0f); - const LineSegment2D far_wall_near = - make_line(5.0f, -5.0f, 5.0f, 5.0f); + const LineSegment2D right_wall_near = make_line(0.0f, 2.0f, 10.0f, 2.0f); + const LineSegment2D far_wall_near = make_line(5.0f, -5.0f, 5.0f, 5.0f); - const LineSegment2D right_wall_far = - make_line(0.0f, 4.0f, 10.0f, 4.0f); - const LineSegment2D far_wall_far = - make_line(9.0f, -5.0f, 9.0f, 5.0f); + const LineSegment2D right_wall_far = make_line(0.0f, 4.0f, 10.0f, 4.0f); + const LineSegment2D far_wall_far = make_line(9.0f, -5.0f, 9.0f, 5.0f); // Noise: behind drone, should not contribute - const LineSegment2D noise = - make_line(-5.0f, -3.0f, -2.0f, -3.0f); + const LineSegment2D noise = make_line(-5.0f, -3.0f, -2.0f, -3.0f); - const std::vector lines{ - right_wall_near, far_wall_near, - right_wall_far, far_wall_far, - noise - }; + const std::vector lines{right_wall_near, far_wall_near, + right_wall_far, far_wall_far, noise}; const auto corners = estimator.find_corner_estimates(lines, drone_pos, drone_heading); @@ -285,4 +261,4 @@ TEST_F(DockingPositionEstimatorTest, EXPECT_NEAR(docking.y(), 1.0f, kEps); } -} // namespace vortex::docking_position_estimator::test \ No newline at end of file +} // namespace vortex::docking_position_estimator::test diff --git a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt index 854aaa0..47cdd4c 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt +++ b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt @@ -1,20 +1,20 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_pool_exploration - test_pool_exploration.cpp + test_pool_map.cpp ) -target_include_directories(test_pool_exploration PUBLIC +target_include_directories(test_pool_map PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include ) -ament_target_dependencies(test_pool_exploration +ament_target_dependencies(test_pool_map rclcpp nav_msgs vortex_msgs Eigen3 ) -target_link_libraries(test_pool_exploration +target_link_libraries(test_pool_map ${PROJECT_NAME}_lib ) diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp deleted file mode 100644 index 9e4b501..0000000 --- a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp +++ /dev/null @@ -1,226 +0,0 @@ -#include - -#include -#include -#include - -#include -#include - -namespace vortex::pool_exploration::test { - -using vortex::utils::types::LineSegment2D; -using vortex::utils::types::Point2D; - -namespace { - -Point2D make_point(float x, float y) { - return Point2D{.x = x, .y = y}; -} - -LineSegment2D make_line(float x0, float y0, float x1, float y1) { - return LineSegment2D{ - .p0 = make_point(x0, y0), - .p1 = make_point(x1, y1), - }; -} - -PoolExplorationPlannerConfig make_config() { - PoolExplorationPlannerConfig config{}; - - // Avstander for gyldige vegger - config.min_wall_distance_m = 0.2f; - config.max_wall_distance_m = 10.0f; - - // Klassifisering - // Right wall: y må være negative nok i NED - config.right_wall_max_y_m = -0.1f; - - // Far/front wall: x må være positive nok - config.far_wall_min_x_m = 0.1f; - - // Vinkler - config.parallel_heading_angle_threshold_rad = - 20.0f * static_cast(M_PI) / 180.0f; - config.perpendicular_heading_angle_threshold_rad = - 70.0f * static_cast(M_PI) / 180.0f; - - // Hjørnevinkel - config.min_corner_angle_rad = 70.0f * static_cast(M_PI) / 180.0f; - config.max_corner_angle_rad = 110.0f * static_cast(M_PI) / 180.0f; - - // Docking-offset - config.right_wall_offset_m = 1.0f; - config.far_wall_offset_m = 1.5f; - - return config; -} - -} // namespace - -class PoolExplorationPlannerTest : public ::testing::Test { - protected: - PoolExplorationPlannerTest() : config(make_config()), planner(config) {} - - PoolExplorationPlannerConfig config; - PoolExplorationPlanner planner; -}; - -TEST_F(PoolExplorationPlannerTest, - FindCornerEstimates_FindsCornerFromPerpendicularWalls) { - // Drone i origo, heading langs +x - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - const float drone_heading = 0.0f; - - // "Right wall" i NED: horizontal linje under drone (negative y) - // "Front wall": vertical linje foran drone (positive x) - const LineSegment2D right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f); - const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); - - const std::vector lines{right_wall, front_wall}; - - const auto corners = - planner.find_corner_estimates(lines, drone_pos, drone_heading); - - ASSERT_EQ(corners.size(), 1u); - - EXPECT_NEAR(corners[0].corner_point.x(), 5.0f, 1e-4f); - EXPECT_NEAR(corners[0].corner_point.y(), -2.0f, 1e-4f); -} - -TEST_F(PoolExplorationPlannerTest, FindCornerEstimates_RejectsParallelWalls) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - const float drone_heading = 0.0f; - - // Begge linjer omtrent parallelle med heading -> skal ikke gi hjørne - const LineSegment2D line0 = make_line(0.0f, -2.0f, 10.0f, -2.0f); - const LineSegment2D line1 = make_line(0.0f, -4.0f, 10.0f, -4.0f); - - const std::vector lines{line0, line1}; - - const auto corners = - planner.find_corner_estimates(lines, drone_pos, drone_heading); - - EXPECT_TRUE(corners.empty()); -} - -TEST_F(PoolExplorationPlannerTest, - FindCornerEstimates_RejectsWallsOutsideDistanceThreshold) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - const float drone_heading = 0.0f; - - // Right wall er for langt unna - const LineSegment2D right_wall_too_far = - make_line(0.0f, -20.0f, 10.0f, -20.0f); - const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f); - - const std::vector lines{right_wall_too_far, front_wall}; - - const auto corners = - planner.find_corner_estimates(lines, drone_pos, drone_heading); - - EXPECT_TRUE(corners.empty()); -} - -TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ReturnsClosestCorner) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - - CornerEstimate c0{.right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), - .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), - .corner_point = Eigen::Vector2f{5.0f, -2.0f}}; - - CornerEstimate c1{.right_wall = make_line(0.0f, -4.0f, 10.0f, -4.0f), - .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), - .corner_point = Eigen::Vector2f{8.0f, -4.0f}}; - - const auto best = planner.select_best_corner({c1, c0}, drone_pos); - - EXPECT_NEAR(best.corner_point.x(), 5.0f, 1e-4f); - EXPECT_NEAR(best.corner_point.y(), -2.0f, 1e-4f); -} - -TEST_F(PoolExplorationPlannerTest, SelectBestCorner_ThrowsOnEmptyInput) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - - EXPECT_THROW(planner.select_best_corner({}, drone_pos), std::runtime_error); -} - -TEST_F(PoolExplorationPlannerTest, - EstimateDockingPosition_OffsetsFromCornerTowardDroneSideOfWalls) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - - // Hjørne i (5, -2) - // Right wall: y = -2 - // Front wall: x = 5 - // - // Med drone i (0,0): - // - normal fra right wall not drone peker opp: (0, +1) - // - normal fra front wall not drone peker venstre: (-1, 0) - // - // Forventet docking: - // (5, -2) + 1.0*(0,1) + 1.5*(-1,0) = (3.5, -1.0) - - CornerEstimate corner{.right_wall = make_line(0.0f, -2.0f, 10.0f, -2.0f), - .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), - .corner_point = Eigen::Vector2f{5.0f, -2.0f}}; - - const Eigen::Vector2f docking = - planner.estimate_docking_position(corner, drone_pos); - - EXPECT_NEAR(docking.x(), 3.5f, 1e-4f); - EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); -} - -TEST_F(PoolExplorationPlannerTest, - FullPipeline_FindsCornerSelectsBestAndEstimatesDocking) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - const float drone_heading = 0.0f; - - // Ett riktig hjørne nærme - const LineSegment2D right_wall_near = make_line(0.0f, -2.0f, 10.0f, -2.0f); - const LineSegment2D front_wall_near = make_line(5.0f, -5.0f, 5.0f, 5.0f); - - // Ett riktig hjørne lenger unna - const LineSegment2D right_wall_far = make_line(0.0f, -4.0f, 10.0f, -4.0f); - const LineSegment2D front_wall_far = make_line(9.0f, -5.0f, 9.0f, 5.0f); - - // Litt støy some ikke skal bli brukt - const LineSegment2D noise = make_line(-5.0f, 3.0f, -2.0f, 3.0f); - - const std::vector lines{right_wall_near, front_wall_near, - right_wall_far, front_wall_far, - noise}; - - const auto corners = - planner.find_corner_estimates(lines, drone_pos, drone_heading); - - ASSERT_GE(corners.size(), 2u); - - const auto best = planner.select_best_corner(corners, drone_pos); - const auto docking = planner.estimate_docking_position(best, drone_pos); - - EXPECT_NEAR(best.corner_point.x(), 5.0f, 1e-4f); - EXPECT_NEAR(best.corner_point.y(), -2.0f, 1e-4f); - - EXPECT_NEAR(docking.x(), 3.5f, 1e-4f); - EXPECT_NEAR(docking.y(), -1.0f, 1e-4f); -} - -TEST_F(PoolExplorationPlannerTest, - FindCornerEstimates_AcceptsApproximatelyPerpendicularWalls) { - const Eigen::Vector2f drone_pos{0.0f, 0.0f}; - const float drone_heading = 0.0f; - - // Litt skjeve vegger, men fortsatt innenfor tersklene - const LineSegment2D right_wall = make_line(0.0f, -2.0f, 10.0f, -1.5f); - const LineSegment2D front_wall = make_line(5.0f, -5.0f, 5.5f, 5.0f); - - const std::vector lines{right_wall, front_wall}; - - const auto corners = - planner.find_corner_estimates(lines, drone_pos, drone_heading); - - EXPECT_EQ(corners.size(), 1u); -} - -} // namespace vortex::pool_exploration::test From c9d005305008ffdfe617dbf10c373531e7702f0f Mon Sep 17 00:00:00 2001 From: Sina Date: Tue, 31 Mar 2026 18:00:51 +0200 Subject: [PATCH 06/10] fixed pool exploration tests --- .../subsea_docking/pool_exploration/test/CMakeLists.txt | 8 ++++---- .../test/{test_pool_map.cpp => test_pool_exploration.cpp} | 0 2 files changed, 4 insertions(+), 4 deletions(-) rename mission/tacc/subsea_docking/pool_exploration/test/{test_pool_map.cpp => test_pool_exploration.cpp} (100%) diff --git a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt index 47cdd4c..854aaa0 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt +++ b/mission/tacc/subsea_docking/pool_exploration/test/CMakeLists.txt @@ -1,20 +1,20 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_pool_exploration - test_pool_map.cpp + test_pool_exploration.cpp ) -target_include_directories(test_pool_map PUBLIC +target_include_directories(test_pool_exploration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include ) -ament_target_dependencies(test_pool_map +ament_target_dependencies(test_pool_exploration rclcpp nav_msgs vortex_msgs Eigen3 ) -target_link_libraries(test_pool_map +target_link_libraries(test_pool_exploration ${PROJECT_NAME}_lib ) diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp similarity index 100% rename from mission/tacc/subsea_docking/pool_exploration/test/test_pool_map.cpp rename to mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp From f7b01cfd6d86749d4701439f09d6c23ab1c42003 Mon Sep 17 00:00:00 2001 From: Sina Date: Tue, 31 Mar 2026 18:15:04 +0200 Subject: [PATCH 07/10] fixed typo --- .../pool_exploration/test/test_pool_exploration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp index a2759fb..15472c5 100644 --- a/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp @@ -61,7 +61,7 @@ TEST(PoolExplorationTest, GridInitialization) EXPECT_EQ(cell, -1); } } -/* + TEST(PoolExplorationTest, SetGridCellInsideBounds) { PoolExplorationMap map(10.0, 10.0, 1.0, "map"); From 9642075aca00907915c7a7f304d6734d74d3245c Mon Sep 17 00:00:00 2001 From: Sina Date: Wed, 8 Apr 2026 19:39:10 +0200 Subject: [PATCH 08/10] added left wall classification and left corner estimate option --- .../config/docking_position_estimator.yaml | 2 +- .../docking_position_estimator.hpp | 81 ++++++++++--------- .../src/docking_position_estimator.cpp | 73 ++++++++++++----- .../src/docking_position_estimator_ros.cpp | 2 + 4 files changed, 102 insertions(+), 56 deletions(-) diff --git a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml index 695bdd2..4fd9dc8 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml +++ b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml @@ -28,7 +28,7 @@ right_wall_max_y_m: 0.4 far_wall_min_x_m: 0.5 - # choose_right_corner: 1 #not used yet + use_left_wall: 0 # Waypoint (not used now) switching_threshold: 0.5 diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp index 32e9ab8..da81aea 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -3,6 +3,8 @@ #include #include +#include + #include #include #include @@ -34,7 +36,7 @@ enum class WallClassification { /** @brief Wall segment classified as the right wall. */ RightWall, - /** @brief Wall segment classified as the left wall. (for future use)*/ + /** @brief Wall segment classified as the left wall. */ LeftWall, /** @brief Wall segment classified as the far wall. */ @@ -100,6 +102,8 @@ struct DockingPositionEstimatorConfig { /** @brief Offset from the far wall when estimating docking position [m]. */ float far_wall_offset_m; + int use_left_wall; + /** @brief * Selects whether to prefer the right-side corner in the mission logic. */ @@ -126,24 +130,27 @@ class DockingPositionEstimator { const DockingPositionEstimatorConfig& config); /** - * @brief Find all valid corner estimates from a set of detected line - * segments. - * - * Each line segment is first classified as a candidate wall. Candidate - * right walls and far walls are then paired, and their intersection is - * checked. A pair is accepted as a corner if the angle between the two - * walls lies within the configured corner-angle interval. - * - * @param lines Input line segments expressed in the reference frame. - * @param drone_pos Current drone position in the same frame as the lines. - * @param drone_heading Current drone heading [rad]. - * @return Vector of valid corner estimates. - */ - // EVT LAGE FOR VENSTRE VEGG, TO DO + * @brief Find all valid corner estimates from a set of detected line segments. + * + * Each line segment is first classified as a candidate wall. Candidate + * side walls (right or left, depending on configuration) are paired with + * far walls, and their intersection is computed. + * + * Right walls are used when left-wall mode is disabled. + * Left walls are used when left-wall mode is enabled. + * + * A pair is accepted as a corner if: + * - the line intersection exists, + * - the angle between the two walls lies within the configured corner-angle interval. + * + * @param lines Input line segments expressed in the reference frame. + * @param drone_pos Current drone position in the same frame as the lines. + * @param drone_heading Current drone heading [rad]. + * @return Vector of valid corner estimates. + */ std::vector find_corner_estimates( const std::vector& lines, - const Eigen::Vector2f& - drone_pos, // sjekk at får inn pos og heading riktig? + const Eigen::Vector2f& drone_pos, // sjekk at får inn pos og heading riktig? float drone_heading) const; /** @@ -178,25 +185,27 @@ class DockingPositionEstimator { private: /** - * @brief Classify a line segment as a right wall, far wall, or rejected. - * - * A line is classified using: - * - the distance from the drone to the orthogonal projection on the line, - * - the position of that projection relative to the expected mission - * geometry, - * - and the unsigned angle between the wall direction and drone heading. - * - * Right-wall candidates must satisfy the configured right-side projection - * test and be approximately parallel to the drone heading. - * - * Far-wall candidates must satisfy the configured forward projection test - * and be approximately perpendicular to the drone heading. - * - * @param line Input line segment. - * @param drone_pos Current drone position. - * @param drone_heading Current drone heading [rad]. - * @return Wall classification result. - */ + * @brief Classify a line segment as a right wall, left wall, far wall, or rejected. + * + * A line is classified using: + * - the distance from the drone to the orthogonal projection on the line, + * - the position of that projection relative to the expected mission geometry, + * - and the unsigned angle between the wall direction and drone heading. + * + * Right-wall candidates must satisfy the configured right-side projection test + * and be approximately parallel to the drone heading. + * + * Left-wall candidates must satisfy the configured left-side projection test + * and be approximately parallel to the drone heading. + * + * Far-wall candidates must satisfy the configured forward projection test + * and be approximately perpendicular to the drone heading. + * + * @param line Input line segment. + * @param drone_pos Current drone position. + * @param drone_heading Current drone heading [rad]. + * @return Wall classification result. + */ WallClassification classify_wall( const vortex::utils::types::LineSegment2D& line, const Eigen::Vector2f& drone_pos, diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp index 7c7e2b7..55a4170 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -2,7 +2,6 @@ // #include #include -// #include #include #include @@ -18,6 +17,7 @@ std::vector DockingPositionEstimator::find_corner_estimates( float drone_heading) const { std::vector right_wall_candidates; std::vector far_wall_candidates; + std::vector left_wall_candidates; std::vector corner_estimates; for (const auto& line : lines) { @@ -26,33 +26,57 @@ std::vector DockingPositionEstimator::find_corner_estimates( if (classification == WallClassification::RightWall) { right_wall_candidates.push_back(line); - } else if (classification == WallClassification::FarWall) { + } + else if (classification == WallClassification::FarWall) { far_wall_candidates.push_back(line); } + else if (classification == WallClassification::LeftWall) { + left_wall_candidates.push_back(line); + } } spdlog::info("==== SUMMARY ===="); spdlog::info("Right wall candidates: {}", right_wall_candidates.size()); spdlog::info("Far wall candidates: {}", far_wall_candidates.size()); + spdlog::info("Left wall candidates: {}", left_wall_candidates.size()); - for (const auto& right_wall : right_wall_candidates) { - for (const auto& far_wall : far_wall_candidates) { - Eigen::Vector2f corner_intersection; - if (!compute_line_intersection(right_wall, far_wall, - corner_intersection)) { - continue; - } + auto process_corner = [&](const auto& side_wall, + const auto& far_wall) { + + Eigen::Vector2f corner_intersection; + + if (!compute_line_intersection(side_wall, far_wall, corner_intersection)) { + return; + } + + float wall_angle = angle_between_lines(side_wall, far_wall); + + if (wall_angle < config_.min_corner_angle_rad || + wall_angle > config_.max_corner_angle_rad) { + return; + } + + corner_estimates.push_back({ + side_wall, + far_wall, + corner_intersection + }); + }; - const float wall_angle = angle_between_lines(right_wall, far_wall); + for (const auto& far_wall : far_wall_candidates) { - if (wall_angle >= config_.min_corner_angle_rad && - wall_angle <= config_.max_corner_angle_rad) { - corner_estimates.push_back( - {right_wall, far_wall, corner_intersection}); + if (!config_.use_left_wall) { + for (const auto& right_wall : right_wall_candidates) { + process_corner(right_wall, far_wall); } } - } + if (config_.use_left_wall) { + for (const auto& left_wall : left_wall_candidates) { + process_corner(left_wall, far_wall); + } + } + } return corner_estimates; } @@ -76,6 +100,7 @@ CornerEstimate DockingPositionEstimator::select_best_corner( return best_corner; } +// TO DO for left wall as well Eigen::Vector2f DockingPositionEstimator::estimate_docking_position( const CornerEstimate& estimated_corner, const Eigen::Vector2f& drone_pos) const { @@ -130,14 +155,17 @@ WallClassification DockingPositionEstimator::classify_wall( heading_wall_angle * 180.0 / M_PI); Eigen::Vector2f forward(std::cos(drone_heading), std::sin(drone_heading)); - Eigen::Vector2f right(std::sin(drone_heading), -std::cos(drone_heading)); + Eigen::Vector2f right(-std::sin(drone_heading), std::cos(drone_heading)); + Eigen::Vector2f left(std::sin(drone_heading), -std::cos(drone_heading)); + float forward_dist = rel.dot(forward); float right_dist = rel.dot(right); + float left_dist = rel.dot(left); // RIGHT WALL: projection has negative y-value in NED, wall is approximately // parallel to heading if ( // projection.y() < config_.right_wall_max_y_m && - right_dist < 0 && // USIKKER PÅ DENNE? TO DO + right_dist > 0 && heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as RIGHT candidate"); return WallClassification::RightWall; @@ -147,11 +175,18 @@ WallClassification DockingPositionEstimator::classify_wall( // perpendicular to heading if ( // projection.x() > config_.far_wall_min_x_m && forward_dist > 0 && - heading_wall_angle > - config_.perpendicular_heading_angle_threshold_rad) { + heading_wall_angle > config_.perpendicular_heading_angle_threshold_rad) { spdlog::info(" -> Classified as FAR candidate"); return WallClassification::FarWall; } + + // LEFT WALL + if (left_dist > 0 && + heading_wall_angle > config_.parallel_heading_angle_threshold_rad) { + spdlog::info(" -> Classified as LEFT candidate"); + return WallClassification::LeftWall; + } + spdlog::info(" -> REJECTED by geometry"); return WallClassification::Rejected; } diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp index a3d525a..f357bc4 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -110,6 +110,8 @@ void DockingPositionEstimatorNode::setup_estimator() { this->declare_parameter("right_wall_offset_m"); config.far_wall_offset_m = this->declare_parameter("far_wall_offset_m"); + config.use_left_wall = + this->declare_parameter("use_left_wall"); estimator_ = std::make_unique(config); } From ec23e480210ba324d33c090016c9eb8b22ac19e8 Mon Sep 17 00:00:00 2001 From: Sina Date: Thu, 16 Apr 2026 17:25:42 +0200 Subject: [PATCH 09/10] config switch between right and left wall --- .../config/docking_position_estimator.yaml | 4 +- .../docking_position_estimator.hpp | 43 ++++++++++--------- .../docking_position_estimator_ros.hpp | 6 +-- .../src/docking_position_estimator.cpp | 15 +++---- .../src/docking_position_estimator_ros.cpp | 19 ++++---- .../test/test_docking_position_estimator.cpp | 8 ++-- 6 files changed, 48 insertions(+), 47 deletions(-) diff --git a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml index 4fd9dc8..6a0318e 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml +++ b/mission/tacc/subsea_docking/docking_position_estimator/config/docking_position_estimator.yaml @@ -22,13 +22,13 @@ perpendicular_heading_angle_threshold_rad: 1.35 min_corner_angle_rad: 1.50 max_corner_angle_rad: 1.70 - right_wall_offset_m: 2.5 #offset along right wall + side_wall_offset_m: 2.5 #offset along right wall og left wall far_wall_offset_m: 4.0 #offset along far wall right_wall_max_y_m: 0.4 far_wall_min_x_m: 0.5 - use_left_wall: 0 + use_left_wall: false # Waypoint (not used now) switching_threshold: 0.5 diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp index da81aea..b5ede31 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -12,15 +12,15 @@ namespace vortex::docking_position_estimator { /** - * @brief Represents a candidate docking corner formed by a right wall and a far + * @brief Represents a candidate docking corner formed by a side wall and a far * wall. * * A corner estimate consists of two detected wall segments and their * intersection point in the reference frame. */ struct CornerEstimate { - /** @brief Line segment classified as the right wall. */ - vortex::utils::types::LineSegment2D right_wall; + /** @brief Line segment classified as the side wall. */ + vortex::utils::types::LineSegment2D side_wall; /** @brief Line segment classified as the far wall. */ vortex::utils::types::LineSegment2D far_wall; @@ -79,14 +79,27 @@ struct DockingPositionEstimatorConfig { * perpendicular [rad]. */ float perpendicular_heading_angle_threshold_rad; - /** @brief Minimum allowed angle between right wall and far wall for a valid + /** @brief Minimum allowed angle between side wall and far wall for a valid * corner [rad]. */ float min_corner_angle_rad; - /** @brief Maximum allowed angle between right wall and far wall for a valid + /** @brief Maximum allowed angle between side wall and far wall for a valid * corner [rad]. */ float max_corner_angle_rad; + /** @brief Offset from the right or left wall when estimating docking position [m]. + */ + float side_wall_offset_m; + + /** @brief Offset from the far wall when estimating docking position [m]. */ + float far_wall_offset_m; + + /** @brief Selects whether to use the left wall instead of the right wall + * for corner detection. false = right wall, true = left wall. */ + bool use_left_wall; + + + //REMOVE THIS? TO DO /** @brief Maximum y-value used when checking whether a projection belongs * to the right wall. */ float right_wall_max_y_m; @@ -95,20 +108,6 @@ struct DockingPositionEstimatorConfig { * to the far wall. */ float far_wall_min_x_m; - /** @brief Offset from the right wall when estimating docking position [m]. - */ - float right_wall_offset_m; - - /** @brief Offset from the far wall when estimating docking position [m]. */ - float far_wall_offset_m; - - int use_left_wall; - - /** @brief - * Selects whether to prefer the right-side corner in the mission logic. - */ - - // int choose_right_corner; // not used yet }; /** @@ -171,7 +170,8 @@ class DockingPositionEstimator { * @brief Estimate a docking position from a validated corner estimate. * * The docking point is computed by offsetting the detected corner along the - * inward-facing normals of the right wall and far wall. + * inward-facing normals of the side wall (right or left, depending on + * configuration) and the far wall. * * @param estimated_corner Corner estimate containing the two wall segments * and their intersection. @@ -201,6 +201,9 @@ class DockingPositionEstimator { * Far-wall candidates must satisfy the configured forward projection test * and be approximately perpendicular to the drone heading. * + * Classification of right and left walls is gated by the use_left_wall + * configuration flag — only the relevant side wall type is evaluated. + * * @param line Input line segment. * @param drone_pos Current drone position. * @param drone_heading Current drone heading [rad]. diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp index 7a55a26..0c582d1 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp @@ -38,9 +38,9 @@ namespace vortex::docking_position_estimator { * frame, and classifies them as candidate walls relative to the drone. * * From the classified walls, the node estimates corner candidates by - * intersecting right-wall and far-wall segments. The most suitable corner - * is selected, and a docking position is computed by offsetting from the - * corner along the wall normals. + * intersecting right-wall or left-wall and far-wall segments. The most + * suitable corner is selected, and a docking position is computed by + * offsetting from the corner along the wall normals. * * If a valid docking estimate is found, it is sent through the SendPose * service. diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp index 55a4170..786cfee 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -83,8 +83,6 @@ std::vector DockingPositionEstimator::find_corner_estimates( CornerEstimate DockingPositionEstimator::select_best_corner( const std::vector& possible_corners, const Eigen::Vector2f& drone_pos) const { - // if (possible_corners.empty()) {throw std::runtime_error("No candidate - // corners available"); } // UNNTAKSHÅNDTERING I ROS I STEDET float min_distance = std::numeric_limits::max(); CornerEstimate best_corner = possible_corners.front(); @@ -100,18 +98,17 @@ CornerEstimate DockingPositionEstimator::select_best_corner( return best_corner; } -// TO DO for left wall as well Eigen::Vector2f DockingPositionEstimator::estimate_docking_position( const CornerEstimate& estimated_corner, const Eigen::Vector2f& drone_pos) const { - Eigen::Vector2f right_normal = - compute_normal_towards_point(estimated_corner.right_wall, drone_pos); + Eigen::Vector2f side_normal = + compute_normal_towards_point(estimated_corner.side_wall, drone_pos); Eigen::Vector2f far_normal = compute_normal_towards_point(estimated_corner.far_wall, drone_pos); Eigen::Vector2f docking_estimate = estimated_corner.corner_point + - right_normal * config_.right_wall_offset_m + + side_normal * config_.side_wall_offset_m + far_normal * config_.far_wall_offset_m; return docking_estimate; @@ -165,6 +162,7 @@ WallClassification DockingPositionEstimator::classify_wall( // RIGHT WALL: projection has negative y-value in NED, wall is approximately // parallel to heading if ( // projection.y() < config_.right_wall_max_y_m && + !config_.use_left_wall && right_dist > 0 && heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as RIGHT candidate"); @@ -181,8 +179,9 @@ WallClassification DockingPositionEstimator::classify_wall( } // LEFT WALL - if (left_dist > 0 && - heading_wall_angle > config_.parallel_heading_angle_threshold_rad) { + if (config_.use_left_wall && + left_dist > 0 && + heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as LEFT candidate"); return WallClassification::LeftWall; } diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp index f357bc4..7b5434c 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -100,20 +100,20 @@ void DockingPositionEstimatorNode::setup_estimator() { this->declare_parameter("min_corner_angle_rad"); config.max_corner_angle_rad = this->declare_parameter("max_corner_angle_rad"); - config.far_wall_min_x_m = - this->declare_parameter("far_wall_min_x_m"); - config.right_wall_max_y_m = - this->declare_parameter("right_wall_max_y_m"); - // config.choose_right_corner = - // this->declare_parameter("choose_right_corner"); //not used yet - config.right_wall_offset_m = - this->declare_parameter("right_wall_offset_m"); + config.side_wall_offset_m = + this->declare_parameter("side_wall_offset_m"); config.far_wall_offset_m = this->declare_parameter("far_wall_offset_m"); config.use_left_wall = - this->declare_parameter("use_left_wall"); + this->declare_parameter("use_left_wall"); estimator_ = std::make_unique(config); + + // FJERNE? TO DO + config.far_wall_min_x_m = + this->declare_parameter("far_wall_min_x_m"); + config.right_wall_max_y_m = + this->declare_parameter("right_wall_max_y_m"); } void DockingPositionEstimatorNode::pose_callback( @@ -136,7 +136,6 @@ void DockingPositionEstimatorNode::sonar_info_callback( void DockingPositionEstimatorNode::line_callback( const vortex_msgs::msg::LineSegment2DArray::ConstSharedPtr& msg) { estimate_and_send_docking_waypoint(*msg); - // drawSegmentsInMapFrame(*msg); } void DockingPositionEstimatorNode::estimate_and_send_docking_waypoint( diff --git a/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp index ee37c0a..3e3e0b6 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp @@ -46,7 +46,7 @@ DockingPositionEstimatorConfig make_config() { config.min_corner_angle_rad = 70.0f * static_cast(M_PI) / 180.0f; config.max_corner_angle_rad = 110.0f * static_cast(M_PI) / 180.0f; - config.right_wall_offset_m = 1.0f; + config.side_wall_offset_m = 1.0f; config.far_wall_offset_m = 1.5f; return config; @@ -182,13 +182,13 @@ TEST_F(DockingPositionEstimatorTest, const Eigen::Vector2f drone_pos{0.0f, 0.0f}; CornerEstimate near_corner{ - .right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), + .side_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), .corner_point = Eigen::Vector2f{5.0f, 2.0f}, }; CornerEstimate far_corner{ - .right_wall = make_line(0.0f, 4.0f, 10.0f, 4.0f), + .side_wall = make_line(0.0f, 4.0f, 10.0f, 4.0f), .far_wall = make_line(8.0f, -5.0f, 8.0f, 5.0f), .corner_point = Eigen::Vector2f{8.0f, 4.0f}, }; @@ -217,7 +217,7 @@ TEST_F( // (5, 2) + 1.0*(0, -1) + 1.5*(-1, 0) = (3.5, 1.0) CornerEstimate corner{ - .right_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), + .side_wall = make_line(0.0f, 2.0f, 10.0f, 2.0f), .far_wall = make_line(5.0f, -5.0f, 5.0f, 5.0f), .corner_point = Eigen::Vector2f{5.0f, 2.0f}, }; From 3956cba8d8b90810c02ad46aae63c0923da27983 Mon Sep 17 00:00:00 2001 From: Sina Date: Thu, 16 Apr 2026 18:05:41 +0200 Subject: [PATCH 10/10] fix formatting --- .../docking_position_estimator.hpp | 102 +++++++++--------- .../docking_position_estimator_ros.hpp | 4 +- .../src/docking_position_estimator.cpp | 32 ++---- .../src/docking_position_estimator_ros.cpp | 3 +- 4 files changed, 67 insertions(+), 74 deletions(-) diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp index b5ede31..7cfe2f0 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -2,8 +2,8 @@ #define DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ #include -#include #include +#include #include #include @@ -87,7 +87,8 @@ struct DockingPositionEstimatorConfig { * corner [rad]. */ float max_corner_angle_rad; - /** @brief Offset from the right or left wall when estimating docking position [m]. + /** @brief Offset from the right or left wall when estimating docking + * position [m]. */ float side_wall_offset_m; @@ -95,11 +96,10 @@ struct DockingPositionEstimatorConfig { float far_wall_offset_m; /** @brief Selects whether to use the left wall instead of the right wall - * for corner detection. false = right wall, true = left wall. */ + * for corner detection. false = right wall, true = left wall. */ bool use_left_wall; - - //REMOVE THIS? TO DO + // REMOVE THIS? TO DO /** @brief Maximum y-value used when checking whether a projection belongs * to the right wall. */ float right_wall_max_y_m; @@ -107,7 +107,6 @@ struct DockingPositionEstimatorConfig { /** @brief Minimum x-value used when checking whether a projection belongs * to the far wall. */ float far_wall_min_x_m; - }; /** @@ -129,27 +128,30 @@ class DockingPositionEstimator { const DockingPositionEstimatorConfig& config); /** - * @brief Find all valid corner estimates from a set of detected line segments. - * - * Each line segment is first classified as a candidate wall. Candidate - * side walls (right or left, depending on configuration) are paired with - * far walls, and their intersection is computed. - * - * Right walls are used when left-wall mode is disabled. - * Left walls are used when left-wall mode is enabled. - * - * A pair is accepted as a corner if: - * - the line intersection exists, - * - the angle between the two walls lies within the configured corner-angle interval. - * - * @param lines Input line segments expressed in the reference frame. - * @param drone_pos Current drone position in the same frame as the lines. - * @param drone_heading Current drone heading [rad]. - * @return Vector of valid corner estimates. - */ + * @brief Find all valid corner estimates from a set of detected line + * segments. + * + * Each line segment is first classified as a candidate wall. Candidate + * side walls (right or left, depending on configuration) are paired with + * far walls, and their intersection is computed. + * + * Right walls are used when left-wall mode is disabled. + * Left walls are used when left-wall mode is enabled. + * + * A pair is accepted as a corner if: + * - the line intersection exists, + * - the angle between the two walls lies within the configured corner-angle + * interval. + * + * @param lines Input line segments expressed in the reference frame. + * @param drone_pos Current drone position in the same frame as the lines. + * @param drone_heading Current drone heading [rad]. + * @return Vector of valid corner estimates. + */ std::vector find_corner_estimates( const std::vector& lines, - const Eigen::Vector2f& drone_pos, // sjekk at får inn pos og heading riktig? + const Eigen::Vector2f& + drone_pos, // sjekk at får inn pos og heading riktig? float drone_heading) const; /** @@ -185,30 +187,32 @@ class DockingPositionEstimator { private: /** - * @brief Classify a line segment as a right wall, left wall, far wall, or rejected. - * - * A line is classified using: - * - the distance from the drone to the orthogonal projection on the line, - * - the position of that projection relative to the expected mission geometry, - * - and the unsigned angle between the wall direction and drone heading. - * - * Right-wall candidates must satisfy the configured right-side projection test - * and be approximately parallel to the drone heading. - * - * Left-wall candidates must satisfy the configured left-side projection test - * and be approximately parallel to the drone heading. - * - * Far-wall candidates must satisfy the configured forward projection test - * and be approximately perpendicular to the drone heading. - * - * Classification of right and left walls is gated by the use_left_wall - * configuration flag — only the relevant side wall type is evaluated. - * - * @param line Input line segment. - * @param drone_pos Current drone position. - * @param drone_heading Current drone heading [rad]. - * @return Wall classification result. - */ + * @brief Classify a line segment as a right wall, left wall, far wall, or + * rejected. + * + * A line is classified using: + * - the distance from the drone to the orthogonal projection on the line, + * - the position of that projection relative to the expected mission + * geometry, + * - and the unsigned angle between the wall direction and drone heading. + * + * Right-wall candidates must satisfy the configured right-side projection + * test and be approximately parallel to the drone heading. + * + * Left-wall candidates must satisfy the configured left-side projection + * test and be approximately parallel to the drone heading. + * + * Far-wall candidates must satisfy the configured forward projection test + * and be approximately perpendicular to the drone heading. + * + * Classification of right and left walls is gated by the use_left_wall + * configuration flag — only the relevant side wall type is evaluated. + * + * @param line Input line segment. + * @param drone_pos Current drone position. + * @param drone_heading Current drone heading [rad]. + * @return Wall classification result. + */ WallClassification classify_wall( const vortex::utils::types::LineSegment2D& line, const Eigen::Vector2f& drone_pos, diff --git a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp index 0c582d1..a4a2c41 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp @@ -38,8 +38,8 @@ namespace vortex::docking_position_estimator { * frame, and classifies them as candidate walls relative to the drone. * * From the classified walls, the node estimates corner candidates by - * intersecting right-wall or left-wall and far-wall segments. The most - * suitable corner is selected, and a docking position is computed by + * intersecting right-wall or left-wall and far-wall segments. The most + * suitable corner is selected, and a docking position is computed by * offsetting from the corner along the wall normals. * * If a valid docking estimate is found, it is sent through the SendPose diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp index 786cfee..e850dd8 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -26,11 +26,9 @@ std::vector DockingPositionEstimator::find_corner_estimates( if (classification == WallClassification::RightWall) { right_wall_candidates.push_back(line); - } - else if (classification == WallClassification::FarWall) { + } else if (classification == WallClassification::FarWall) { far_wall_candidates.push_back(line); - } - else if (classification == WallClassification::LeftWall) { + } else if (classification == WallClassification::LeftWall) { left_wall_candidates.push_back(line); } } @@ -40,12 +38,11 @@ std::vector DockingPositionEstimator::find_corner_estimates( spdlog::info("Far wall candidates: {}", far_wall_candidates.size()); spdlog::info("Left wall candidates: {}", left_wall_candidates.size()); - auto process_corner = [&](const auto& side_wall, - const auto& far_wall) { - + auto process_corner = [&](const auto& side_wall, const auto& far_wall) { Eigen::Vector2f corner_intersection; - if (!compute_line_intersection(side_wall, far_wall, corner_intersection)) { + if (!compute_line_intersection(side_wall, far_wall, + corner_intersection)) { return; } @@ -56,15 +53,10 @@ std::vector DockingPositionEstimator::find_corner_estimates( return; } - corner_estimates.push_back({ - side_wall, - far_wall, - corner_intersection - }); + corner_estimates.push_back({side_wall, far_wall, corner_intersection}); }; for (const auto& far_wall : far_wall_candidates) { - if (!config_.use_left_wall) { for (const auto& right_wall : right_wall_candidates) { process_corner(right_wall, far_wall); @@ -83,7 +75,6 @@ std::vector DockingPositionEstimator::find_corner_estimates( CornerEstimate DockingPositionEstimator::select_best_corner( const std::vector& possible_corners, const Eigen::Vector2f& drone_pos) const { - float min_distance = std::numeric_limits::max(); CornerEstimate best_corner = possible_corners.front(); @@ -161,9 +152,8 @@ WallClassification DockingPositionEstimator::classify_wall( // RIGHT WALL: projection has negative y-value in NED, wall is approximately // parallel to heading - if ( // projection.y() < config_.right_wall_max_y_m && - !config_.use_left_wall && - right_dist > 0 && + if ( // projection.y() < config_.right_wall_max_y_m && + !config_.use_left_wall && right_dist > 0 && heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as RIGHT candidate"); return WallClassification::RightWall; @@ -173,14 +163,14 @@ WallClassification DockingPositionEstimator::classify_wall( // perpendicular to heading if ( // projection.x() > config_.far_wall_min_x_m && forward_dist > 0 && - heading_wall_angle > config_.perpendicular_heading_angle_threshold_rad) { + heading_wall_angle > + config_.perpendicular_heading_angle_threshold_rad) { spdlog::info(" -> Classified as FAR candidate"); return WallClassification::FarWall; } // LEFT WALL - if (config_.use_left_wall && - left_dist > 0 && + if (config_.use_left_wall && left_dist > 0 && heading_wall_angle < config_.parallel_heading_angle_threshold_rad) { spdlog::info(" -> Classified as LEFT candidate"); return WallClassification::LeftWall; diff --git a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp index 7b5434c..a98c3f0 100644 --- a/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -104,8 +104,7 @@ void DockingPositionEstimatorNode::setup_estimator() { this->declare_parameter("side_wall_offset_m"); config.far_wall_offset_m = this->declare_parameter("far_wall_offset_m"); - config.use_left_wall = - this->declare_parameter("use_left_wall"); + config.use_left_wall = this->declare_parameter("use_left_wall"); estimator_ = std::make_unique(config);