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/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..6a0318e --- /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 + 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: false + + # Waypoint (not used now) + switching_threshold: 0.5 + overwrite_prior_waypoints: true + 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 new file mode 100644 index 0000000..7cfe2f0 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator.hpp @@ -0,0 +1,311 @@ +#ifndef DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ +#define DOCKING_POSITION_ESTIMATOR__DOCKING_POSITION_ESTIMATOR_HPP_ + +#include +#include +#include + +#include +#include +#include + +namespace vortex::docking_position_estimator { + +/** + * @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 side wall. */ + vortex::utils::types::LineSegment2D side_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 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 side wall and far wall for a valid + * corner [rad]. */ + float min_corner_angle_rad; + + /** @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; + + /** @brief Minimum x-value used when checking whether a projection belongs + * to the far wall. */ + float far_wall_min_x_m; +}; + +/** + * @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 + * 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? + 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 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. + * @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, 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, + 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..a4a2c41 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/include/docking_position_estimator/docking_position_estimator_ros.hpp @@ -0,0 +1,236 @@ +#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 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. + */ +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..e850dd8 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator.cpp @@ -0,0 +1,306 @@ +#include // til testing +// #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 left_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); + } 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()); + + 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}); + }; + + 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); + } + } + + if (config_.use_left_wall) { + for (const auto& left_wall : left_wall_candidates) { + process_corner(left_wall, far_wall); + } + } + } + return 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(); + + 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 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 + + side_normal * config_.side_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)); + 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 && + !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; + } + + // 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; + } + + // LEFT WALL + 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; + } + + 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..a98c3f0 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/src/docking_position_estimator_ros.cpp @@ -0,0 +1,289 @@ +#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.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"); + + 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( + 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); +} + +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..3e3e0b6 --- /dev/null +++ b/mission/tacc/subsea_docking/docking_position_estimator/test/test_docking_position_estimator.cpp @@ -0,0 +1,264 @@ +#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.side_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{ + .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{ + .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}, + }; + + 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{ + .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}, + }; + + 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 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..9814ddd --- /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..8ddf81c --- /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 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 new file mode 100644 index 0000000..9e15e0e --- /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 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..ebdd703 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration.hpp @@ -0,0 +1,305 @@ +#ifndef POOL_EXPLORATION__POOL_EXPLORATION_HPP_ +#define POOL_EXPLORATION__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. + */ + explicit 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 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 Planner configuration. */ + PoolExplorationPlannerConfig config_; +}; + +} // namespace vortex::pool_exploration + +#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 new file mode 100644 index 0000000..c6d45c1 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_exploration_ros.hpp @@ -0,0 +1,267 @@ +#ifndef POOL_EXPLORATION__POOL_EXPLORATION_ROS_HPP_ +#define POOL_EXPLORATION__POOL_EXPLORATION_ROS_HPP_ + +#include +#include +#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 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_; + + /** + * @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 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__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..821d2fb --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/include/pool_exploration/pool_grid.hpp @@ -0,0 +1,62 @@ +#ifndef POOL_EXPLORATION__POOL_GRID_HPP_ +#define POOL_EXPLORATION__POOL_GRID_HPP_ + +#include +#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 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 algorithm (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__POOL_GRID_HPP_ 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..c6f36de --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration.cpp @@ -0,0 +1,374 @@ +#include // til testing +#include +#include + +#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 relative 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 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? + + 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..1a89e7a --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_exploration_ros.cpp @@ -0,0 +1,401 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +// #include +#include +#include // ERSTATTE MED VORTEX? ^^^ +#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 +} + +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< + geometry_msgs::msg::PoseWithCovarianceStamped>( + 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< + 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)); + + send_pose_client_ = this->create_client( + "/pool_exploration/docking_pose"); + + 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 (!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 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"); + // 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(); + // tf_broadcaster_->sendTransform(map_to_odom_tf); + + // 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(); }); +} + +// 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..eb4756d --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/src/pool_grid.cpp @@ -0,0 +1,142 @@ +#include // til testing +#include +#include +#include + +#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..854aaa0 --- /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 +) 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..15472c5 --- /dev/null +++ b/mission/tacc/subsea_docking/pool_exploration/test/test_pool_exploration.cpp @@ -0,0 +1,214 @@ +#include +#include +#include +#include +#include + +using vortex::pool_exploration::PoolExplorationPlanner; +using vortex::pool_exploration::PoolExplorationPlannerConfig; +/* +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 some 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 cellar 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 not x-akse + + float diff = map.lineAngleDifference({p0,p1}, heading); + + // Linje 45 grader not 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); +} + +*/ 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 + +