diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index dd6742a..b4c349e 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -26,7 +26,7 @@ void CollisionAvoidanceTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { setup_map_odom_tf_and_subs(); - set_gps_frame_coords(); + set_gps_odom_points(); setup_lock.unlock(); break; } diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index 9a54321..6677670 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -36,7 +36,7 @@ void DockingTaskNode::main_task() { continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_frame_coords(); + set_gps_odom_points(); setup_map_odom_tf_and_subs(); setup_lock.unlock(); break; @@ -406,7 +406,7 @@ std::pair DockingTaskNode::navigate_formation( } Eigen::VectorXi assignment = - assign_landmarks(predicted_positions, measured_positions); + auction_algorithm(predicted_positions, measured_positions); if (result == 0) { for (Eigen::Index i = 0; i < assignment.size(); i++) { diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index 0c5c0c4..40965cf 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -24,7 +24,7 @@ void ManeuveringTaskNode::main_task() { continue; } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { - set_gps_frame_coords(); + set_gps_odom_points(); setup_map_odom_tf_and_subs(); setup_lock.unlock(); break; diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 8cbe29f..a8c1608 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -17,8 +17,27 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ void main_task(); + + /** + * @brief Predict the positions of the first two buoys + * + * @return An Eigen::Array representing the predicted positions + * of the first two buoys + */ + Eigen::Array predict_first_buoy_pair(); + + /** + * @brief Predict the positions of the first two buoys pairs + * + * @return An Eigen::Array representing the predicted positions + * of the first two buoy pairs + */ + Eigen::Array predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); + + private: - geometry_msgs::msg::Point previous_waypoint_odom_frame_; + + }; } // namespace navigation_task diff --git a/mission/njord_tasks/navigation_task/navigation_task.pdf b/mission/njord_tasks/navigation_task/navigation_task.pdf new file mode 100644 index 0000000..9c0357a Binary files /dev/null and b/mission/njord_tasks/navigation_task/navigation_task.pdf differ diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index 70b3dad..b6f3732 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -12,9 +12,10 @@ navigation_task_node: gps_end_x: 0.0 gps_end_y: 0.0 gps_frame_coords_set: false - - map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_pose_topic: "landmarks_out" - \ No newline at end of file + assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct + + # Task specific parameters + distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 17c0445..a28fb31 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -5,6 +5,8 @@ namespace navigation_task { NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) : NjordTaskBaseNode("navigation_task_node", options) { + declare_parameter("distance_to_first_buoy_pair", 2.0); + std::thread(&NavigationTaskNode::main_task, this).detach(); } @@ -25,12 +27,95 @@ void NavigationTaskNode::main_task() { } if (!(this->get_parameter("gps_frame_coords_set").as_bool())) { setup_map_odom_tf_and_subs(); - set_gps_frame_coords(); + set_gps_odom_points(); setup_lock.unlock(); break; } setup_lock.unlock(); } + // First pair of buoys + Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + std::vector buoy_landmarks_0_to_1 = get_buoy_landmarks(predicted_first_buoy_pair); + if(buoy_landmarks_0_to_1.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); + } + geometry_msgs::msg::Point waypoint_first_pair; + waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / 2; + waypoint_first_pair.y = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / 2; + waypoint_first_pair.z = 0.0; + send_waypoint(waypoint_first_pair); + reach_waypoint(1.0); + + // Second pair of buoys + Eigen::Array predicted_first_and_second_pair = predict_first_and_second_buoy_pair(buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); + std::vector buoy_landmarks_0_to_3 = get_buoy_landmarks(predicted_first_and_second_pair); + if(buoy_landmarks_0_to_3.size() != 4) { + RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); + } + geometry_msgs::msg::Point waypoint_second_pair; + waypoint_second_pair.x = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / 2; + waypoint_second_pair.y = (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / 2; + waypoint_second_pair.z = 0.0; + send_waypoint(waypoint_second_pair); + reach_waypoint(1.0); + + // West buoy +} + +Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { + // Predict the positions of the first two buoys + geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; + geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; + buoy_0_base_link_frame.header.frame_id = "base_link"; + buoy_1_base_link_frame.header.frame_id = "base_link"; + + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); + + buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_0_base_link_frame.pose.position.y = -2.5; + buoy_0_base_link_frame.pose.position.z = 0.0; + buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; + buoy_1_base_link_frame.pose.position.y = 2.5; + buoy_1_base_link_frame.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped buoy_0_odom_frame; + geometry_msgs::msg::PoseStamped buoy_1_odom_frame; + + try { + auto transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero, + tf2::durationFromSec(1.0)); + tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform); + tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + } + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; + predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; + predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; + predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; + + return predicted_positions; +} + +Eigen::Array NavigationTaskNode::predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1){ + Eigen::Vector2d direction_vector; + direction_vector << previous_waypoint_odom_frame_.x - this->get_parameter("gps_start_x").as_double(), + previous_waypoint_odom_frame_.y - this->get_parameter("gps_start_y").as_double(); + direction_vector.normalize(); + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x; + predicted_positions(1, 0) = buoy_0.y; + predicted_positions(0, 1) = buoy_1.x; + predicted_positions(1, 1) = buoy_1.y; + predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + + return predicted_positions; } } // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 4600cba..ee32bef 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -17,20 +17,54 @@ #include #include + /** + * @brief A struct representing a landmark with id and pose in the odom frame + */ + struct LandmarkPoseID { + geometry_msgs::msg::Pose pose_odom_frame; + int32_t id; +}; + class NjordTaskBaseNode : public rclcpp::Node { public: NjordTaskBaseNode(const std::string &node_name, const rclcpp::NodeOptions &options); protected: - void setup_map_odom_tf_and_subs(); - void set_gps_frame_coords(); + std::pair lla2flat(double lat, double lon) const; void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - std::shared_ptr get_odom(); void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + + /** + * @brief Spins until the map to odom tf is available. + * Stores the tf in the member variable map_odom_tf_ + * Then initializes the odom and landmark subscribers + */ + void setup_map_odom_tf_and_subs(); + + /** + * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member variables + * Requires that the map_origin_ member variable is correctly set + */ + void set_gps_odom_points(); + + /** + * @brief Get the odometry message + * If no odometry message is available, the function will wait for a new message + * + * @return A shared pointer to the odometry message + */ + std::shared_ptr get_odom(); + + /** + * @brief Get the landmarks in the odom frame + * If no landmarks are available, the function will wait for a new message + * + * @return A shared pointer to the landmarks in the odom frame + */ std::shared_ptr get_landmarks_odom_frame(); /** @@ -46,10 +80,36 @@ class NjordTaskBaseNode : public rclcpp::Node { * means that the corresponding predicted position(buoy) is not assigned to * any measured position(landmark). */ - Eigen::VectorXi assign_landmarks( + Eigen::VectorXi auction_algorithm( const Eigen::Array &predicted_positions, const Eigen::Array &measured_positions); + + /** + * @brief Use predicted positions of buoys to get the landmarks with id and pose + * corresponding to the buoys. + * @param predicted_positions The predicted positions of the buoys + * @return A vector of landmarks with id and pose where the index in the vector + * corresponds to the index of the predicted position in the predicted_positions array. + */ + std::vector get_buoy_landmarks(const Eigen::Array& predicted_positions); + + /** + * @brief Send a waypoint to the waypoint service + * @param waypoint The waypoint to send in odom frame + * Also sets the member variable previous_waypoint_odom_frame_ to the waypoint + */ + void send_waypoint(const geometry_msgs::msg::Point &waypoint); + + /** + * @brief Utility function that returns when within distance_threshold of the waypoint. + * The reference waypoint is the member variable previous_waypoint_odom_frame_ set by the send_waypoint function. + * + * @param distance_threshold The distance threshold for reaching the waypoint + */ + void reach_waypoint(const double distance_threshold); + + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; rclcpp::Publisher::SharedPtr @@ -75,6 +135,9 @@ class NjordTaskBaseNode : public rclcpp::Node { std::shared_ptr landmarks_msg_; bool new_odom_msg_ = false; bool new_landmark_msg_ = false; + + + geometry_msgs::msg::Point previous_waypoint_odom_frame_; }; #endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index 597f25e..0ba3b42 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -15,6 +15,7 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, declare_parameter("gps_end_x", 0.0); declare_parameter("gps_end_y", 0.0); declare_parameter("gps_frame_coords_set", false); + declare_parameter("assignment_confidence", 10); declare_parameter("map_origin_topic", "/map/origin"); declare_parameter("odom_topic", "/seapath/odom/ned"); @@ -52,6 +53,8 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, waypoint_client_ = this->create_client("/waypoint"); + + } void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { @@ -88,37 +91,49 @@ void NjordTaskBaseNode::setup_map_odom_tf_and_subs() { std::placeholders::_1)); } -void NjordTaskBaseNode::set_gps_frame_coords() { +void NjordTaskBaseNode::set_gps_odom_points() { auto [gps_start_x, gps_start_y] = lla2flat(this->get_parameter("gps_start_lat").as_double(), this->get_parameter("gps_start_lon").as_double()); auto [gps_end_x, gps_end_y] = lla2flat(this->get_parameter("gps_end_lat").as_double(), this->get_parameter("gps_end_lon").as_double()); - this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_x)); - this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_y)); - this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_x)); - this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_y)); - this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO(this->get_logger(), - "GPS map frame coordinates set to: %f, %f, %f, %f", gps_start_x, - gps_start_y, gps_end_x, gps_end_y); - geometry_msgs::msg::PoseArray gps_points; - gps_points.header.frame_id = "map"; + geometry_msgs::msg::PoseStamped gps_start_map_frame; + gps_start_map_frame.header.frame_id = "map"; + gps_start_map_frame.pose.position.x = gps_start_x; + gps_start_map_frame.pose.position.y = gps_start_y; + geometry_msgs::msg::PoseStamped gps_end_map_frame; + gps_end_map_frame.header.frame_id = "map"; + gps_end_map_frame.pose.position.x = gps_end_x; + gps_end_map_frame.pose.position.y = gps_end_y; + geometry_msgs::msg::PoseStamped gps_start_odom_frame; + geometry_msgs::msg::PoseStamped gps_end_odom_frame; + try { + tf2::doTransform(gps_start_map_frame, gps_start_odom_frame, map_odom_tf_); + tf2::doTransform(gps_end_map_frame, gps_end_odom_frame, map_odom_tf_); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); + return; + } - // Convert GPS points to geometry poses - geometry_msgs::msg::Pose start_pose; - start_pose.position.x = gps_start_x; - start_pose.position.y = gps_start_y; - gps_points.poses.push_back(start_pose); + this->set_parameter(rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); + this->set_parameter(rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); + this->set_parameter(rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); + this->set_parameter(rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); + this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); + RCLCPP_INFO(this->get_logger(), + "GPS oodm frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.pose.position.x, gps_start_odom_frame.pose.position.y, + gps_end_odom_frame.pose.position.x, gps_end_odom_frame.pose.position.y); - geometry_msgs::msg::Pose end_pose; - end_pose.position.x = gps_end_x; - end_pose.position.y = gps_end_y; - gps_points.poses.push_back(end_pose); + geometry_msgs::msg::PoseArray gps_points_odom_frame; + gps_points_odom_frame.header.frame_id = "odom"; + + gps_points_odom_frame.poses.push_back(gps_start_odom_frame.pose); + gps_points_odom_frame.poses.push_back(gps_end_odom_frame.pose); - gps_map_coord_visualization_pub_->publish(gps_points); + gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); } std::pair NjordTaskBaseNode::lla2flat(double lat, @@ -170,7 +185,7 @@ void NjordTaskBaseNode::map_origin_callback( // Set the map to odom transform setup_map_odom_tf_and_subs(); // Set GPS frame coordinates - set_gps_frame_coords(); + set_gps_odom_points(); map_origin_sub_.reset(); setup_lock.unlock(); @@ -216,7 +231,7 @@ NjordTaskBaseNode::get_landmarks_odom_frame() { return landmarks_msg_; } -Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( +Eigen::VectorXi NjordTaskBaseNode::auction_algorithm( const Eigen::Array &predicted_positions, const Eigen::Array &measured_positions) { int num_predicted = predicted_positions.cols(); @@ -227,6 +242,7 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( RCLCPP_ERROR(this->get_logger(), "Number of predicted positions is greater than number of " "measured positions in auction algorithm"); + return Eigen::VectorXi::Constant(num_predicted, -1); } double epsilon = 1e-6; // Small positive number to prevent division by zero @@ -283,4 +299,140 @@ Eigen::VectorXi NjordTaskBaseNode::assign_landmarks( prices[max_item] += max_value - second_max_value + epsilon; } return assignment; +} + +std::vector NjordTaskBaseNode::get_buoy_landmarks(const Eigen::Array& predicted_positions){ + std::vector landmark_ids; + std::vector expected_assignment; + Eigen::Array measured_buoy_positions(2, predicted_positions.cols()); + int confidence_threshold = this->get_parameter("assignment_confidence").as_int(); + int result = 0; + while (result < confidence_threshold) { + landmark_ids.clear(); + auto landmark_msg = get_landmarks_odom_frame(); + + // Extract measured positions and corresponding landmark ids + Eigen::Array measured_positions(2, landmark_msg->landmarks.size()); + for (size_t i = 0; i < landmark_msg->landmarks.size(); i++) { + landmark_ids.push_back(landmark_msg->landmarks[i].id); + measured_positions(0, i) = landmark_msg->landmarks[i].odom.pose.pose.position.x; + measured_positions(1, i) = landmark_msg->landmarks[i].odom.pose.pose.position.y; + } + + // Check if there are enough landmarks detected to perform auction algorithm + if (predicted_positions.cols() > measured_positions.cols()) { + RCLCPP_ERROR(this->get_logger(), + "Not enough landmarks detected to perform auction algorithm"); + result = 0; + continue; + } + // Perform auction algorithm + Eigen::VectorXi assignment = auction_algorithm(predicted_positions, measured_positions); + + // Extract measured positions of assigned buoys + for (Eigen::Index i = 0; i < assignment.size(); i++) { + measured_buoy_positions(0, i) = measured_positions(0, assignment(i)); + measured_buoy_positions(1, i) = measured_positions(1, assignment(i)); + } + + // Check if any buoys are unassigned + // Should never happen as long as the number of landmarks detected is greater than or equal to the number of buoys + bool unassigned_buoy = false; + for (Eigen::Index i = 0; i < assignment.size(); i++) { + if (assignment(i) == -1) { + unassigned_buoy = true; + break; + } + } + + // If any buoys are unassigned, restart assignment process + if(unassigned_buoy){ + result = 0; + continue; + } + + // If this is the first iteration, save the assignment and continue + if (result == 0) { + expected_assignment.clear(); + for (Eigen::Index i = 0; i < assignment.size(); i++) { + expected_assignment.push_back(landmark_ids.at(assignment(i))); + } + result++; + continue; + } + + // Check if the assignment is consistent with the previous assignment + bool consistent_assignment = true; + for (Eigen::Index i = 0; i < assignment.size(); i++) { + if (landmark_ids.at(assignment(i)) != expected_assignment.at(i)) { + consistent_assignment = false; + break; + } + } + + // If the assignment is consistent, increment the result counter + // Otherwise, reset the result counter + if (consistent_assignment) { + result++; + continue; + } else { + result = 0; + continue; + } + + } + // Loop has completed, return the id and pose of the landmarks assigned to buoys + std::vector buoys; + for (size_t i = 0; i < expected_assignment.size(); i++) { + LandmarkPoseID landmark; + landmark.id = expected_assignment.at(i); + landmark.pose_odom_frame.position.x = measured_buoy_positions(0, i); + landmark.pose_odom_frame.position.y = measured_buoy_positions(1, i); + buoys.push_back(landmark); + } + return buoys; +} + +void NjordTaskBaseNode::send_waypoint(const geometry_msgs::msg::Point &waypoint) { + auto request = std::make_shared(); + request->waypoint.push_back(waypoint); + auto result_future = waypoint_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", + waypoint.x, waypoint.y); + // Check if the service was successful + + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); + } + + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint.x; + waypoint_vis.pose.position.y = waypoint.y; + waypoint_visualization_pub_->publish(waypoint_vis); + + previous_waypoint_odom_frame_ = waypoint; +} + +void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { + RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); + + auto get_current_position = [&]() { + auto odom_msg = get_odom(); + return std::make_pair(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y); + }; + + auto [x, y] = get_current_position(); + + while (std::hypot(x - previous_waypoint_odom_frame_.x, y - previous_waypoint_odom_frame_.y) > distance_threshold) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + std::tie(x, y) = get_current_position(); + } + + RCLCPP_INFO(this->get_logger(), "Reached waypoint"); } \ No newline at end of file