Merge branch 'test/29.04' of https://github.com/vortexntnu/vortex-asv …

…into test/29.04
vortexntnu · Aug 7, 2024 · 628fa19 · 628fa19
2 parents adf847a + b9f431c
commit 628fa19
Show file tree

Hide file tree

Showing 18 changed files with 1,088 additions and 335 deletions.
diff --git a/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml
@@ -0,0 +1,5 @@
+-33.722576743831986 150.67377683501326
+-33.72213785615945 150.6737273159938
+-33.722119332734295 150.6744244880215
+-33.72265441348879 150.6743192836287
+
diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml
@@ -1,7 +1,8 @@
 map_manager_node:
   ros__parameters:
-    use_predef_landmask: false
-    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml"
+    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_office.yaml"
     map_resolution: 0.2
     map_width: 1000
     map_height: 1000
@@ -10,6 +11,6 @@ map_manager_node:
     # map_origin_lat: 63.414660884931976 
     # map_origin_lon: 10.398554661537544
     # use_predef_map_origin: true
-    map_origin_lat: 0.0
-    map_origin_lon: 0.0
-    use_predef_map_origin: false
+    map_origin_lat: -33.72242824301795
+    map_origin_lon: 150.6740063854522
+    use_predef_map_origin: true
diff --git a/mission/njord_tasks/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_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(docking_task_node
   tf2::tf2
   tf2_ros::tf2_ros
   tf2_geometry_msgs::tf2_geometry_msgs
+  pcl_common
 )
 
 ament_target_dependencies(docking_task_node 
@@ -50,6 +53,7 @@ ament_target_dependencies(docking_task_node
   tf2
   tf2_ros
   tf2_geometry_msgs
+  pcl_conversions
 )
 
 install(

diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp
@@ -4,10 +4,10 @@
 #include <Eigen/Dense>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <njord_task_base/njord_task_base_ros.hpp>
-#include <tf2/LinearMath/Matrix3x3.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
 
 namespace docking_task {
 
@@ -41,11 +41,11 @@ class DockingTaskNode : public NjordTaskBaseNode {
 
   Eigen::Array<double, 2, 2> predict_first_buoy_pair();
 
-  Eigen::Array<double, 2, 4>
+  Eigen::Array<double, 2, 2>
   predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0,
                            const geometry_msgs::msg::Point &buoy_1);
 
-  Eigen::Array<double, 2, 4>
+  Eigen::Array<double, 2, 2>
   predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0,
                           const geometry_msgs::msg::Point &buoy_1,
                           const geometry_msgs::msg::Point &buoy_2,

diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml
@@ -21,6 +21,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>pcl_conversions</depend>
 
 
   <export>

diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml
@@ -1,10 +1,12 @@
 docking_task_node:
   ros__parameters:
-    map_origin_lat: 63.4411585867919
-    map_origin_lon: 10.419400373255625
+    # map_origin_lat: 63.4411585867919
+    # map_origin_lon: 10.419400373255625
+    map_origin_lat: -33.72242824301795
+    map_origin_lon: 150.6740063854522
     map_origin_set: true
-    gps_start_lat: 63.44097054434422
-    gps_start_lon: 10.419997767413607
+    gps_start_lat: -33.72255346763595
+    gps_start_lon: 150.67394210459224
     gps_end_lat: 63.44125901804796
     gps_end_lon: 10.41857835889424
     gps_start_x: 0.0