diff --git a/filtering/line_filtering/config/line_filtering.yaml b/filtering/line_filtering/config/line_filtering.yaml index 68ee017fb..6f08c55cc 100644 --- a/filtering/line_filtering/config/line_filtering.yaml +++ b/filtering/line_filtering/config/line_filtering.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: line_sub_topic: "/line_detection/line_segments" - sonar_info_sub_topic: "/orca/sonar_info" + sonar_info_sub_topic: "/moby/fls/sonar_info" line_pub_topic: "/filtered_lines" marker_pub_topic: "/filtered_lines_markers" - target_frame: "odom" - sonar_frame: "Orca/FLS" + target_frame: "moby/odom" + sonar_frame: "moby/sonar_link" timer_rate_ms: 200 diff --git a/mission/tacc/docking_task/pool_exploration/CMakeLists.txt b/mission/tacc/docking_task/pool_exploration/CMakeLists.txt new file mode 100644 index 000000000..ea47a8786 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/README.md b/mission/tacc/docking_task/pool_exploration/README.md new file mode 100644 index 000000000..ab2c4baa1 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/config/pool_exploration.yaml b/mission/tacc/docking_task/pool_exploration/config/pool_exploration.yaml new file mode 100644 index 000000000..e784a02cc --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/include/pool_exploration/pool_exploration.hpp b/mission/tacc/docking_task/pool_exploration/include/pool_exploration/pool_exploration.hpp new file mode 100644 index 000000000..af353dd3b --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp b/mission/tacc/docking_task/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp new file mode 100644 index 000000000..53cb4a50e --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/include/pool_exploration/pool_grid.hpp b/mission/tacc/docking_task/pool_exploration/include/pool_exploration/pool_grid.hpp new file mode 100644 index 000000000..614db42ee --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/launch/pool_exploration.launch.py b/mission/tacc/docking_task/pool_exploration/launch/pool_exploration.launch.py new file mode 100644 index 000000000..4ace14358 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/package.xml b/mission/tacc/docking_task/pool_exploration/package.xml new file mode 100644 index 000000000..3856ba9e9 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/src/pool_exploration.cpp b/mission/tacc/docking_task/pool_exploration/src/pool_exploration.cpp new file mode 100644 index 000000000..493ce9b9f --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/src/pool_exploration_ros.cpp b/mission/tacc/docking_task/pool_exploration/src/pool_exploration_ros.cpp new file mode 100644 index 000000000..a5af8e083 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/src/pool_grid.cpp b/mission/tacc/docking_task/pool_exploration/src/pool_grid.cpp new file mode 100644 index 000000000..402516485 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/test/CMakeLists.txt b/mission/tacc/docking_task/pool_exploration/test/CMakeLists.txt new file mode 100644 index 000000000..20eb0f1de --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/test/test_pool_exploration.cpp b/mission/tacc/docking_task/pool_exploration/test/test_pool_exploration.cpp new file mode 100644 index 000000000..61e97ecc2 --- /dev/null +++ b/mission/tacc/docking_task/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/docking_task/pool_exploration/test/test_pool_map.cpp b/mission/tacc/docking_task/pool_exploration/test/test_pool_map.cpp new file mode 100644 index 000000000..34e0f7719 --- /dev/null +++ b/mission/tacc/docking_task/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