dynamic buoy prediction

vortexntnu · Jul 26, 2024 · d9c848c · d9c848c
1 parent 168772e
commit d9c848c
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 6 deletions.
diff --git a/mission/docking_task/include/docking_task/docking_task_ros.hpp b/mission/docking_task/include/docking_task/docking_task_ros.hpp
@@ -100,9 +100,10 @@ class DockingTaskNode : public rclcpp::Node {
    * buoy pair.
    *
    * @param predicted_positions The predicted positions of the buoys
+   * @return The ids of the last pair of buoys in the formation
    */
-  void
-  navigate_formation(const Eigen::Array<double, 2, 6> &predicted_positions);
+  std::pair<uint32_t, uint32_t>
+  navigate_formation(Eigen::Array<double, 2, 6> &predicted_positions);
 
   /**
    * @brief Calculate the coordinates of a gps point in the map frame

diff --git a/mission/docking_task/src/docking_task_ros.cpp b/mission/docking_task/src/docking_task_ros.cpp
@@ -124,7 +124,7 @@ void DockingTaskNode::main_task() {
 
   auto formation = predict_buoy_formation(landmark1, landmark2);
 
-  navigate_formation(formation);
+  auto [buoy_left_id, buoy_right_id] = navigate_formation(formation);
 }
 
 std::pair<LandmarkWithID, LandmarkWithID> DockingTaskNode::initial_waypoint() {
@@ -526,8 +526,7 @@ DockingTaskNode::auction_algorithm(const Eigen::MatrixXd &reward_matrix) {
   return assignment;
 }
 
-void DockingTaskNode::navigate_formation(
-    const Eigen::Array<double, 2, 6> &predicted_positions) {
+std::pair<uint32_t, uint32_t> DockingTaskNode::navigate_formation(Eigen::Array<double, 2, 6> &predicted_positions) {
   RCLCPP_INFO(this->get_logger(), "Navigating though formation");
   std::vector<uint32_t> prev_assignment;
   std::vector<uint32_t> landmark_ids;
@@ -627,6 +626,25 @@ void DockingTaskNode::navigate_formation(
             landmark_poses_odom_frame.poses.at(assignment(3)).position.x;
         buoy_right_y =
             landmark_poses_odom_frame.poses.at(assignment(3)).position.y;
+        Eigen::Vector2d direction_vector
+            ((buoy_left_x - landmark_poses_odom_frame.poses.at(assignment(4)).position.x +
+             buoy_right_x - landmark_poses_odom_frame.poses.at(assignment(5)).position.x) /
+            2, (buoy_left_y - landmark_poses_odom_frame.poses.at(assignment(4)).position.y +
+             buoy_right_y - landmark_poses_odom_frame.poses.at(assignment(5)).position.y) /2);
+        direction_vector.normalize();
+        // Update the predicted positions
+        predicted_positions << buoy_left_x + direction_vector.x() * 5,
+            buoy_right_x + direction_vector.x() * 5,
+            buoy_left_x,
+            buoy_right_x,
+            buoy_left_x - direction_vector.x() * 5,
+            buoy_right_x - direction_vector.x() * 5,
+            buoy_left_y + direction_vector.y() * 5,
+            buoy_right_y + direction_vector.y() * 5,
+            buoy_left_y,
+            buoy_right_y,
+            buoy_left_y - direction_vector.y() * 5,
+            buoy_right_y - direction_vector.y() * 5;
       } else {
         buoy_left_x =
             landmark_poses_odom_frame.poses.at(assignment(0)).position.x;
@@ -676,7 +694,7 @@ void DockingTaskNode::navigate_formation(
       } else {
         // Return from function when the asv is close to the second waypoint
         reach_waypoint(0.2);
-        return;
+        return {landmark_ids.at(assignment(0)), landmark_ids.at(assignment(1))};
       }
     }
   }