utility functions for task base

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;
     }

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<int32_t, int32_t> 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++) {

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;

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<double, 2, 2> representing the predicted positions
+   * of the first two buoys
+   */
+  Eigen::Array<double, 2, 2> predict_first_buoy_pair();
+
+    /**
+   * @brief Predict the positions of the first two buoys pairs
+   *
+   * @return An Eigen::Array<double, 2, 4> representing the predicted positions
+   * of the first two buoy pairs
+   */
+  Eigen::Array<double, 2, 4> 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

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"
-
+    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

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<double>("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<LandmarkPoseID> 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<double, 2, 4> 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<LandmarkPoseID> 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<double, 2, 2> 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<double, 2, 2> 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<double, 2, 4> 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<double, 2, 4> 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

mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp

@@ -17,20 +17,54 @@
 #include <thread>
 #include <utility>
 
+  /**
+   * @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<double, double> 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<nav_msgs::msg::Odometry> 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<nav_msgs::msg::Odometry> 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<vortex_msgs::msg::LandmarkArray> 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<double, 2, Eigen::Dynamic> &predicted_positions,
       const Eigen::Array<double, 2, Eigen::Dynamic> &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<LandmarkPoseID> get_buoy_landmarks(const Eigen::Array<double, 2, Eigen::Dynamic>& 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<geometry_msgs::msg::PoseArray>::SharedPtr
       gps_map_coord_visualization_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
@@ -75,6 +135,9 @@ class NjordTaskBaseNode : public rclcpp::Node {
   std::shared_ptr<vortex_msgs::msg::LandmarkArray> 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