colav task

asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml

@@ -0,0 +1,4 @@
+-33.722464875303054 150.6733186790048
+-33.720864277234156 150.673353826611
+-33.72115033638088 150.67502276015122
+-33.72271743627459 150.67461276680072

asv_setup/config/sitaw/map_manager_params.yaml

@@ -1,7 +1,7 @@
 map_manager_node:
   ros__parameters:
     use_predef_landmask: true
-    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml"
+    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml"
     # landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml"
     map_resolution: 0.2
     map_width: 1000
@@ -11,6 +11,11 @@ map_manager_node:
     # map_origin_lat: 63.414660884931976 
     # map_origin_lon: 10.398554661537544
     # use_predef_map_origin: true
-    map_origin_lat: -33.72242824301795
-    map_origin_lon: 150.6740063854522
+    # Map origin for sim_navigation
+    # map_origin_lat: -33.72242824301795
+    # map_origin_lon: 150.6740063854522
+    # use_predef_map_origin: true
+    # Map origin for sim_maneuvering
+    map_origin_lat: -33.72213985487166
+    map_origin_lon: 150.67413507552098
     use_predef_map_origin: true

mission/njord_tasks/collision_avoidance_task/CMakeLists.txt

@@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
 
 include_directories(include)
 
@@ -38,6 +40,7 @@ target_link_libraries(collision_avoidance_task_node
   tf2::tf2
   tf2_ros::tf2_ros
   tf2_geometry_msgs::tf2_geometry_msgs
+  pcl_common
 )
 
 ament_target_dependencies(collision_avoidance_task_node 
@@ -50,6 +53,7 @@ ament_target_dependencies(collision_avoidance_task_node
   tf2
   tf2_ros
   tf2_geometry_msgs
+  pcl_conversions
 )
 
 install(

mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp

@@ -2,9 +2,20 @@
 #define COLLISION_AVOIDANCE_TASK_ROS_HPP
 
 #include <njord_task_base/njord_task_base_ros.hpp>
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>  // Required for tf2::doTransform
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 
 namespace collision_avoidance_task {
 
+struct LandmarkOdomID {
+  nav_msgs::msg::Odometry odom;
+  int32_t id;
+};
 class CollisionAvoidanceTaskNode : public NjordTaskBaseNode {
 public:
   explicit CollisionAvoidanceTaskNode(
@@ -17,7 +28,50 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode {
    */
   void main_task();
 
+  /**
+   * @brief Predict the positions of the first two buoys
+   * 
+   * @return Eigen::Array<double, 2, 2> predicted_positions
+   */
+  Eigen::Array<double, 2, 2> predict_first_buoy_pair();
+
+  Eigen::Array<double, 2, 2>
+  predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0,
+                           const geometry_msgs::msg::Point &buoy_1);
+
+  void track_vessel(const Eigen::Vector2d& direction_vector_downwards,
+      const Eigen::Vector2d& direction_vector_forwards,
+      const geometry_msgs::msg::Point& waypoint_second_buoy_pair);
+
+  nav_msgs::msg::Odometry get_vessel_odom();
+
+  LandmarkOdomID filter_landmarks(
+      const vortex_msgs::msg::LandmarkArray &landmarks,
+      const Eigen::Vector2d& direction_vector_downwards,
+      const Eigen::Vector2d& direction_vector_forwards,
+       const geometry_msgs::msg::Point& waypoint_second_buoy_pair);
+
+  void vessel_collision_heading();
+
+  double calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2);
+
+  bool has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards);
+
+  Eigen::Array<double, 2, 2>
+  predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0,
+                           const geometry_msgs::msg::Point &buoy_1);
+
+  Eigen::Array<double, 2, 2>
+  predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red,
+                                 const geometry_msgs::msg::Point &buoy_green);
+
+
+
 private:
+  mutable std::mutex vessel_odom_mutex_;
+  bool new_vessel_odom_msg_ = false;
+  nav_msgs::msg::Odometry vessel_odom_;
+  std::condition_variable vessel_odom_cv_;
 };
 
 } // namespace collision_avoidance_task

mission/njord_tasks/collision_avoidance_task/package.xml

@@ -21,6 +21,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>pcl_conversions</depend>
 
 
   <export>

mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml

@@ -1,20 +1,23 @@
 collision_avoidance_task_node:
   ros__parameters:
-    map_origin_lat: 63.4411585867919
-    map_origin_lon: 10.419400373255625
+    map_origin_lat: -33.72213988382845
+    map_origin_lon: 150.67413500672993
     map_origin_set: true
     gps_start_lat: 63.44097054434422
     gps_start_lon: 10.419997767413607
     gps_end_lat: 63.44125901804796
     gps_end_lon: 10.41857835889424
     gps_start_x: 0.0
     gps_start_y: 0.0
-    gps_end_x: 0.0
-    gps_end_y: 0.0
-    gps_frame_coords_set: false
-
-
+    gps_end_x: 0.05243377001522731
+    gps_end_y: 37.196639612524166
+    gps_frame_coords_set: true
     map_origin_topic: "/map/origin"
     odom_topic: "/seapath/odom/ned"
     landmark_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.
+    distance_between_buoy_pairs: 5.0 # Distance between buoy pairs in meters
+    vessel_assignment_confidence: 5 # Number of consequtive identical assignments from landmark to vessel before we consider the assignment as correct