From 4391804edb7d5d20648888726fc9409052a47595 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Wed, 25 Jun 2025 17:23:32 +0100
Subject: [PATCH 01/12] refactoring

---
 .../alars_auv_search_planner/path_planners.py |  46 ++++---
 .../alars_auv_search_planner/prob_grid_map.py | 122 ++++++++----------
 .../search_planner_controller.py              |   5 +-
 .../launch/search_planning_launch.py          |  11 +-
 4 files changed, 93 insertions(+), 91 deletions(-)

diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
index 3d90eb0e..edac0480 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
@@ -67,8 +67,9 @@ def __init__(self, name = 'init_actions', params = None):
             self.sam_init_pos = params["sam.init_pos"]
             self.sam_pos_var= params["sam.initial_state.pos_variance"]
             self.flight_height = params["flight_height"]
-            self.drone_map_frame_id = params['frames.id.quadrotor_map'] 
+            self.map_frame_id = params['frames.id.map'] 
             self.drone_odom_frame_id = params['frames.id.quadrotor_odom'] 
+            self.sam_odom_frame_id = params['frames.id.sam_odom'] 
         else:
             self.get_logger().error("No valid parameters received in SearchPlanner node")
 
@@ -76,10 +77,6 @@ def __init__(self, name = 'init_actions', params = None):
             msg_type = PoseStamped,
             topic = '/sam_auv_v1/teleport',
             qos_profile= 10)
-        self.teleport_sam_publisher = self.create_publisher(
-            msg_type = PoseStamped,
-            topic = '/Quadrotor/teleport',
-            qos_profile= 10)
         
         self.sam_odom_callback = self.create_subscription(
             msg_type = Odometry,
@@ -105,7 +102,7 @@ def teleport_sam(self):
         in SAM's odom frame.
         """
         msg = PoseStamped()
-        msg.header.frame_id = 'sam_auv_v1/odom_gt'
+        msg.header.frame_id = self.drone_odom_frame_id # CHECK
         msg.header.stamp = self.get_clock().now().to_msg()
         msg.pose.position.x =  self.sam_init_pos[0]
         msg.pose.position.y =  self.sam_init_pos[1]
@@ -130,13 +127,16 @@ def get_GPSxy_ping(self) -> PointStamped :
         It's not a real GPS measurement.
         """
         if self.sam_pos is not None:
-            cov = [[self.sam_pos_var, 0], [0, self.sam_pos_var]]
+            cov = [[self.sam_pos_var, 0], [0, self.sam_pos_var]] #TODO: change to sam_pos
             X = np.random.multivariate_normal(self.sam_init_pos[0:2], cov)
             GPS_ping = PointStamped()
             GPS_ping.header.stamp = self.get_clock().now().to_msg()
-            GPS_ping.header.frame_id = self.drone_map_frame_id 
+            GPS_ping.header.frame_id = self.map_frame_id 
             GPS_ping.point.x = X[0]
             GPS_ping.point.y = X[1]
+            # TODO: remove later
+            GPS_ping.point.x = 1270.0
+            GPS_ping.point.y = 1165.0
             return GPS_ping
         else: return None
         
@@ -187,6 +187,7 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
         self.battery_state = None
         self.drone_position = PointStamped()
         self.drone_vel = None
+        self.sam_position = PointStamped()
         self.sam_vel = None
         self.distance_thresh = 0.1
 
@@ -206,8 +207,9 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
             self.battery_threshold = params['battery.threshold']
             self.equivalent_drone_vel = params['battery.equivalent_drone_vel'] 
 
-            self.drone_map_frame_id = params['frames.id.quadrotor_map'] 
+            self.map_frame_id = params['frames.id.map'] 
             self.drone_odom_frame_id = params['frames.id.quadrotor_odom'] 
+            self.sam_odom_frame_id = params['frames.id.sam_odom'] 
         else:
             self.get_logger().error("No valid parameters received in Path Model")
 
@@ -238,6 +240,10 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
             msg_type = Path,
             topic = '/Quadrotor/path',
             qos_profile= 10)
+        self.sam_pos_publisher = self.create_publisher( # visualization purposes only
+            msg_type = PointStamped,
+            topic = '/sam_auv_v1/position',
+            qos_profile= 10)
         
 
     @abstractmethod     
@@ -264,7 +270,7 @@ def transform_point(self, point:PointStamped) -> PointStamped:
         # transform desired position (relative position) to odom frame
         t = self.tf_buffer.lookup_transform(
             target_frame = self.drone_odom_frame_id,  
-            source_frame = point.header.frame_id, #point.header.frame_id, #self.drone_map_frame_id,                 
+            source_frame = point.header.frame_id, #point.header.frame_id, #self.map_frame_id,                 
             time=rclpy.time.Time() )
         return tf2_geometry_msgs.do_transform_point(point, t)
     
@@ -303,7 +309,6 @@ def battery_ok(self, path) -> bool:
 
     def publish_path(self) -> None:
         """ Publish path for visualization in rviz """
-        self.get_logger().info(f'Path = {self.path}')
         path_msg = Path()
         path_msg.header.stamp = self.get_clock().now().to_msg()
         path_msg.header.frame_id = self.drone_odom_frame_id
@@ -364,16 +369,21 @@ def return_to_base(self):
         return odom_position.point.x, odom_position.point.y
 
         
-    def drone_odom_callback(self, msg):
-        """ Retrieve drone position (map_gt)"""
-        self.drone_position.point =  msg.pose.pose.position #np.array([msg.pose.pose.position.x, msg.pose.pose.position.y])
+    def drone_odom_callback(self, msg: Odometry):
+        """ Retrieve drone position (currently odometry gives in map_gt)"""
+        self.drone_position.point =  msg.pose.pose.position 
         self.drone_position.header.stamp = self.get_clock().now().to_msg()
-        self.drone_position.header.frame_id = self.drone_map_frame_id #msg.header.frame_id -> check if i can use this one
+        self.drone_position.header.frame_id = msg.header.frame_id 
         self.drone_vel = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y])
 
-    def sam_odom_callback(self, msg):
-        """ Retrieve SAM's velocity from odometry"""
+    def sam_odom_callback(self, msg: Odometry):
+        """ Retrieve SAM's velocity from odometry and publish it's position for visualization purposes"""
         self.sam_vel = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y])
+        self.sam_position.point = msg.pose.pose.position
+        self.sam_position.header = msg.header
+        self.sam_pos_publisher.publish(self.sam_position)
+
+
 
     def drone_battery_callback(self, msg):
         """ Retrieve current battery percentage"""
@@ -867,7 +877,7 @@ def generate_path(self) -> bool:
 
             # if mode = real, the next path is generated as soon service receives request. If mode = sim, we use distance feeback to
             # determine when to publish next path
-            if self.params['mode'] == 'real': self.path_needed = True 
+            self.path_needed = True if self.params['mode'] == 'real' else False
         
 
         elif not self.path_needed and not self.path_completed:
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
index b74cf638..5795a854 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
@@ -42,6 +42,8 @@ def __init__(self, name = "SearchPlanner_gridmap", params:dict = None, GPS_ping:
 
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.drone_position = PointStamped()
+        self.sam_vel = Vector3Stamped()
 
         if params:
 
@@ -57,8 +59,9 @@ def __init__(self, name = "SearchPlanner_gridmap", params:dict = None, GPS_ping:
             self.camera_fov = (pi/180)*params["camera_fov"]
             self.flight_height = params["flight_height"]
 
-            self.drone_map_frame_id = params['frames.id.quadrotor_map'] 
+            self.map_frame_id = params['frames.id.map'] 
             self.drone_odom_frame_id = params['frames.id.quadrotor_odom'] 
+            self.sam_odom_frame_id = params['frames.id.sam_odom'] 
 
         else:
             self.get_logger().error("No parameters received")
@@ -86,7 +89,7 @@ def initiate_grid_map(self) -> None:
         """
 
         # create grid map without probabilities (just its dimensions) as class attributes and ros msg.
-        self.GPS_ping_odom = self.transform_point(self.GPS_ping)
+        self.GPS_ping_odom, _ = self.transform_point(self.GPS_ping)
         self.get_logger().info(f'Received GPS ping in odom (x,y) = {round(float(self.GPS_ping_odom[0]),2), round(float(self.GPS_ping_odom[1]),2)}')
         self.createMap(self.GPS_ping_odom) 
 
@@ -132,18 +135,8 @@ def kernel(self):
         The kernel is a 3*3 matrix with bigger values on the elements that are aligned with SAM's velocity direction.
         Eg: if SAM's velocity = (0,1), then the top-central element of the kernel will be significantly high
         """
-        # Transform sam velocity to odom frame #TODO: check frame of SAM's Odometry
-        t = self.tf_buffer.lookup_transform(
-            target_frame=self.drone_odom_frame_id, 
-            source_frame=self.drone_map_frame_id,                 
-            time=rclpy.time.Time())
-        vec = Vector3Stamped()
-        vec.header.frame_id = self.drone_map_frame_id
-        vec.header.stamp = self.get_clock().now().to_msg()
-        vec.vector.x = self.sam_vel[0]
-        vec.vector.y = self.sam_vel[1]
-        result = tf2_geometry_msgs.do_transform_vector3(vec, t)
-        sam_vel_odom = [result.vector.x, result.vector.y]
+
+        sam_vel_odom = self.transform_vector(self.sam_vel)
 
         # Compute weights by mapping SAM's velocity to closest base vector and retrieving most appropriate kernel
         (q, Q) = self.compute_kernel_coeff(sam_vel=np.linalg.norm(sam_vel_odom), 
@@ -244,32 +237,33 @@ def find_cells2update(self):
         the "projected radius" (which depends on height of flight and camer FOV).
         
         """
-        if self.drone_pos_odom_gt is not None:
-            x_cells, y_cells = [], []
-            x, y = self.drone_pos_odom_gt.pose.position.x, self.drone_pos_odom_gt.pose.position.y
-            detection_radius = tan(self.camera_fov/2)*self.drone_pos_odom_gt.pose.position.z
-            xc_cell, yc_cell = self.find_cell(x_coord = x, y_coord = y) # drone's current cell
-
-            # create a mask around current cell to avoid iterate over full workspace
-            encirclements = int(detection_radius/self.resol)+1
-            base_mask = np.ix_(np.arange(yc_cell-encirclements,yc_cell+encirclements+1,1), np.arange(xc_cell-encirclements,xc_cell+encirclements+1,1))
-
-            # remove cells that lie outside the workspace
-            base_mask_y_filtered = np.extract((0 <= base_mask[0]) & (base_mask[0] <= self.Ncells_y -1), base_mask[0])
-            base_mask_x_filtered = np.extract((0 <= base_mask[1]) & (base_mask[1] <= self.Ncells_x -1), base_mask[1])
-            base_mask_filtered = (base_mask_y_filtered.reshape(base_mask_y_filtered.shape[0],1), base_mask_x_filtered )
-            
-            # apply distance and time criteria
-            sub_X, sub_Y, sub_Time = self.X[base_mask_filtered], self.Y[base_mask_filtered], self.map_Time[base_mask_filtered]
-            condition_mask = (
-                ((np.power(sub_X - x, 2) + np.power(sub_Y - y, 2)) < detection_radius ** 2)
-                & ((self.get_clock().now().nanoseconds) - sub_Time > self.time_margin)
-            )
-            condition_rows, condition_cols = np.where(condition_mask)
-            rows = condition_rows + yc_cell-encirclements
-            columns = condition_cols + xc_cell-encirclements
-
-            return rows, columns
+        
+        x_cells, y_cells = [], []
+        drone_pos_odom, height = self.transform_point(self.drone_position)
+        x, y = drone_pos_odom[0], drone_pos_odom[1]
+        detection_radius = tan(self.camera_fov/2)*height
+        xc_cell, yc_cell = self.find_cell(x_coord = x, y_coord = y) # drone's current cell
+
+        # create a mask around current cell to avoid iterate over full workspace
+        encirclements = int(detection_radius/self.resol)+1
+        base_mask = np.ix_(np.arange(yc_cell-encirclements,yc_cell+encirclements+1,1), np.arange(xc_cell-encirclements,xc_cell+encirclements+1,1))
+
+        # remove cells that lie outside the workspace
+        base_mask_y_filtered = np.extract((0 <= base_mask[0]) & (base_mask[0] <= self.Ncells_y -1), base_mask[0])
+        base_mask_x_filtered = np.extract((0 <= base_mask[1]) & (base_mask[1] <= self.Ncells_x -1), base_mask[1])
+        base_mask_filtered = (base_mask_y_filtered.reshape(base_mask_y_filtered.shape[0],1), base_mask_x_filtered )
+        
+        # apply distance and time criteria
+        sub_X, sub_Y, sub_Time = self.X[base_mask_filtered], self.Y[base_mask_filtered], self.map_Time[base_mask_filtered]
+        condition_mask = (
+            ((np.power(sub_X - x, 2) + np.power(sub_Y - y, 2)) < detection_radius ** 2)
+            & ((self.get_clock().now().nanoseconds) - sub_Time > self.time_margin)
+        )
+        condition_rows, condition_cols = np.where(condition_mask)
+        rows = condition_rows + yc_cell-encirclements
+        columns = condition_cols + xc_cell-encirclements
+
+        return rows, columns
 
     def find_cell(self, x_coord, y_coord):
         """ Maps (x,y) coordinates to cell coordinates """
@@ -302,15 +296,18 @@ def transform_point(self, point:PointStamped) -> np.array:
             target_frame = self.drone_odom_frame_id,  
             source_frame = point.header.frame_id,                 
             time=rclpy.time.Time() )
-        # goal = PointStamped()
-        # goal.header.stamp = t.header.stamp
-        # goal.header.frame_id = self.drone_map_frame_id
-        # goal.point.x = point[0]
-        # goal.point.y = point[1]
-        # goal.point.z = self.flight_height 
         new_point = tf2_geometry_msgs.do_transform_point(point, t)
-        self.get_logger().info(f"GPS ping in quadrotor's odom = {(round(float(new_point.point.x),2), round(float(new_point.point.y),2))}")
-        return np.array([new_point.point.x, new_point.point.y]) 
+        return np.array([new_point.point.x, new_point.point.y]), new_point.point.z
+    
+    def transform_vector(self, vector:Vector3Stamped) -> np.array:
+        """Convert points in map frame to odom frame"""
+        t = self.tf_buffer.lookup_transform(
+            target_frame = self.drone_odom_frame_id,  
+            source_frame = vector.header.frame_id,                 
+            time=rclpy.time.Time() )
+        new_vector = tf2_geometry_msgs.do_transform_vector3(vector, t)
+        sam_vel_odom = [new_vector.vector.x, new_vector.vector.y]
+        return np.array(sam_vel_odom) 
     
     def initiatePrior(self, GPS_ping_odom:np.array):
         """ Initiate Gaussian prior in the odom_gt frame"""
@@ -365,25 +362,16 @@ def map_probabilities(self, data):
         slope = (new_max - new_min)/(max - min)
         return np.int8(slope*(data - max) + new_max)
 
-
-    def drone_odom_callback(self, msg):
-        """ Retrieve drone position (in map_gt) and tranform to init_drone_frame and to Quadrotor/odom_gt (for occupancy grid)"""
-
-        # transform to /Quadrotor/odom_gt
-        t = self.tf_buffer.lookup_transform(
-            target_frame=self.drone_odom_frame_id, 
-            source_frame=self.drone_map_frame_id,                 
-            time=rclpy.time.Time(),
-            timeout = Duration(seconds = 1))
-        pose_map = PoseStamped()
-        pose_map.header.stamp = t.header.stamp
-        pose_map.header.frame_id = msg.header.frame_id # should be map_gt
-        pose_map.pose = msg.pose.pose
-        self.drone_pos_odom_gt = tf2_geometry_msgs.do_transform_pose_stamped(pose_map, t)
-
  
-    def sam_odom_callback(self, msg):
+    def sam_odom_callback(self, msg:Odometry):
         """ Retrieve SAM position (map_gt)"""
-        self.sam_vel = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y])
-        #self.get_logger().info(f'SAM velocity: {self.sam_vel}')
+        self.sam_vel.vector.x = msg.twist.twist.linear.x 
+        self.sam_vel.vector.y = msg.twist.twist.linear.y
+        self.sam_vel.vector.z = msg.twist.twist.linear.z 
+        self.sam_vel.header = msg.header #np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y])
+
+    def drone_odom_callback(self, msg: Odometry):
+        """ Retrieve drone position (map_gt)"""
+        self.drone_position.point = msg.pose.pose.position
+        self.drone_position.header = msg.header
 
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
index 60ca6ce8..3de719ba 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
@@ -103,6 +103,8 @@ def get_path_srv_callback(self, request, response):
                 pose.position.z = self.model_params["flight_height"]
                 pose_list.append(pose)
             path_msg.poses = pose_list
+            path_msg.header.frame_id = self.model_params['frames.id.quadrotor_odom']
+            path_msg.header.stamp = self.get_clock().now().to_msg()
             response.path = path_msg 
             return response
         else: return None
@@ -268,8 +270,9 @@ def get_params(self):
             "battery.threshold": self.get_parameter("battery.threshold").value,
             "battery.equivalent_drone_vel": self.get_parameter("battery.equivalent_drone_vel").value,
 
-            'frames.id.quadrotor_map': self.get_parameter('frames.id.quadrotor_map').value,
+            'frames.id.map': self.get_parameter('frames.id.map').value,
             'frames.id.quadrotor_odom': self.get_parameter('frames.id.quadrotor_odom').value,
+            'frames.id.sam_odom': self.get_parameter('frames.id.sam_odom').value
 
 
         }
diff --git a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
index 79f3d6d4..b227cd14 100644
--- a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
+++ b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
@@ -13,9 +13,9 @@ def generate_launch_description():
     params = {
 
     # --- choose path planner and vehicles' initial positions
-    'mode': 'real', # If 'sim', it's assumed the user wants to test the pkg standalone. If 'real', appropriate service requests have to be made
+    'mode': 'sim', # If 'sim', it's assumed the user wants to test the pkg standalone. If 'real', appropriate service requests have to be made
     'path_planner': 'apf', # 'spiral', 'greedy', 'astar', 'apf' -> path planner type, read documentation/readme
-    'sam.init_pos': [float(1270.0), float(1146.0)], # position to which sam will teleport (in map). User-defined, only useful in  'sim'
+    'sam.init_pos': [float(1260.0), float(1150.0)], # position to which sam will teleport (in map). User-defined, only useful in  'sim'
     'drone.init_pos': [float(5), float(5)], # position to which drone will move (in odom) at the beginning
 
     # --- common parameters to all path planners
@@ -25,7 +25,7 @@ def generate_launch_description():
     'intermediate_dt': float(2.0), # dt between path points in case drones gets too unstable.
     # lat [s] × velocity sets the waypoint distance threshold. Recommended values for distance cap = 1: 
     # Spiral = 0.6, Greedy = 3, A* = 2.
-    'look_ahead_time': float(2), 
+    'look_ahead_time': float(1), 
 
     # --- Spiral planner parameters
     'spiral.vel_factor': 1.0, # the spiral center will move x times faster than AUV. Turn it to 0 to make a static spiral
@@ -76,8 +76,9 @@ def generate_launch_description():
     # 'drone_initial_state.orientation': [-3.285610546299722e-06, -2.3353104552370496e-06, -0.3826446831226349, -0.923895537853241], #x,y,z,w
 
     # --- Frame's names
-    'frames.id.quadrotor_map': 'map_gt_gt',
-    'frames.id.quadrotor_odom': 'Quadrotor/odom_gt',
+    'frames.id.map': 'map_gt',
+    'frames.id.quadrotor_odom': 'Quadrotor/odom',
+    'frames.id.sam_odom': 'sam_auv_v1/odom',
 
     }
 

From 4bd439d16f5deab354201736ad1500b018ecad79 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Wed, 25 Jun 2025 19:59:44 +0100
Subject: [PATCH 02/12] introduced yaml file and launch arguments

---
 .../alars_auv_search_planner/config/README.md |  88 +++++++++++++
 .../config/params.yaml                        |  64 +++++++++
 .../launch/search_planning_launch.py          | 122 ++++++------------
 3 files changed, 190 insertions(+), 84 deletions(-)
 create mode 100644 path_planning/alars_auv_search_planner/config/README.md
 create mode 100644 path_planning/alars_auv_search_planner/config/params.yaml

diff --git a/path_planning/alars_auv_search_planner/config/README.md b/path_planning/alars_auv_search_planner/config/README.md
new file mode 100644
index 00000000..4c023485
--- /dev/null
+++ b/path_planning/alars_auv_search_planner/config/README.md
@@ -0,0 +1,88 @@
+# Search planning parameters
+A brief explanation of parameter:
+
+## Launch arguments (params that are changed regularly)
+
+| **Parameter**    | **Description**                                                                                                                |
+| ---------------- | ------------------------------------------------------------------------------------------------------------------------------ |
+| `mode`           | If `sim`, it's assumed the user wants to test the package standalone. If `real`, appropriate service requests have to be made. |
+| `path_planner`   | `spiral`, `greedy`, `astar`, or `apf` → Path planner type. See documentation for details.                                      |
+| `sam.init_pos`   | Position to which SAM will teleport (in map). User-defined, only useful in `sim`.                                              |
+| `drone.init_pos` | Position to which the drone will move (in `odom`) at the beginning.                                                            |
+
+## Drone params
+| **Parameter**               | **Description**                                                                                                        |
+| --------------------------- | ---------------------------------------------------------------------------------------------------------------------- |
+| `initialization.time_delay` | Time (in seconds) between node execution and grid map initialization.                                                  |
+| `drone.flight_height`       | \[m] Constant flight height of the drone.                                                                              |
+| `drone.camera_fov`          | \[degrees] Horizontal camera field of view.                                                                            |
+| `drone.intermediate_dt`     | Time step between path points if the drone becomes unstable.                                                           |
+| `drone.look_ahead_time`     | Look-ahead time \[s] × velocity sets the waypoint distance threshold. Spiral = 0.6, Greedy = 3, A\* = 2 (recommended). |
+
+## Spiral params
+| **Parameter**       | **Description**                                                                                |
+| ------------------- | ---------------------------------------------------------------------------------------------- |
+| `spiral.vel_factor` | The spiral center will move `x` times faster than the AUV. Set to `0` for a static spiral.     |
+| `spiral.dtheta`     | Angle increment between spiral points in radians. Smaller values = denser spiral. (e.g., π/6). |
+
+## Greedy params
+| **Parameter**           | **Description**                                                                                 |
+| ----------------------- | ----------------------------------------------------------------------------------------------- |
+| `greedy.horizon_radius` | Radius within which the planner chooses the highest-probability cell. `-1` uses full workspace. |
+
+## A* params
+| **Parameter**                   | **Description**                                                                 |
+| ------------------------------- | ------------------------------------------------------------------------------- |
+| `astar.obstacles.max_length`    | Max obstacle size relative to the smallest workspace dimension.                 |
+| `astar.obstacles.quantile_per`  | Percentile of lowest-probability cells considered for obstacle generation.      |
+| `astar.obstacles.obstacles_per` | Fraction of above-selected cells that will become obstacles (randomly sampled). |
+| `astar.horizon_radius`          | Radius within which the planner selects goals. `-1` uses full workspace.        |
+
+## ARF (Artificial Potential Field) params
+| **Parameter**              | **Description**                                                                              |
+| -------------------------- | -------------------------------------------------------------------------------------------- |
+| `arf.k_attractive`         | Attractive potential coefficient. Higher = stronger pull toward goal.                        |
+| `arf.k_repulsive`          | Repulsive potential coefficient. Higher = stronger repulsion from nearby cells.              |
+| `arf.goal_distance_factor` | Factor controlling how "massive" the goal feels. Higher = slower acceleration toward it.     |
+| `arf.d_min`                | Minimum distance at which cells start exerting repulsive force.                              |
+| `arf.d_max`                | Maximum range within which cells can exert repulsive force.                                  |
+| `arf.horizon_radius`       | Radius within which the planner selects the highest-probability goal. `-1` = full workspace. |
+
+## SAM params
+| **Parameter**           | **Description**                                                            |
+| ----------------------- | -------------------------------------------------------------------------- |
+| `sam.init_pos_variance` | Variance used when simulating the GPS ping (adds noise to position).       |
+| `sam.vel_variance`      | Variance used when simulating velocity noise. Currently unused.            |
+| `sam.max_floating_vel`  | Max SAM velocity due to wind/water. Used to adapt spiral motion correctly. |
+
+## Grid map params
+| **Parameter**                         | **Description**                                                      |
+| ------------------------------------- | -------------------------------------------------------------------- |
+| `grid_map.workspace.width`            | Width of the grid map (in meters).                                   |
+| `grid_map.workspace.height`           | Height of the grid map (in meters).                                  |
+| `grid_map.workspace.resol`            | Grid cell resolution (in meters per cell).                           |
+| `grid_map.workspace.variance`         | Gaussian variance used in the initial grid distribution.             |
+| `grid_map.update.rate`                | Bayes Filter update frequency (Hz).                                  |
+| `grid_map.update.true_detection_rate` | Probability of a true positive in the Bayes Filter.                  |
+| `grid_map.update.time_margin`         | Prevents re-updating of recently updated cells (within `x` seconds). |
+
+## Battery params
+| **Parameter**                  | **Description**                                                                     |
+| ------------------------------ | ----------------------------------------------------------------------------------- |
+| `battery.discharge_rate`       | Drone battery discharge rate (% per minute).                                        |
+| `battery.threshold`            | If estimated battery drops below this after planning, the drone returns to base.    |
+| `battery.equivalent_drone_vel` | Fallback velocity \[m/s] used in path duration estimate if the drone is stationary. |
+
+## TF params
+| **Parameter**              | **Description**                   |
+| -------------------------- | --------------------------------- |
+| `frames.id.map`            | Frame ID of the global map.       |
+| `frames.id.quadrotor_odom` | Frame ID of the drone's odometry. |
+| `frames.id.sam_odom`       | Frame ID of SAM’s odometry.       |
+
+
+
+
+
+
+
diff --git a/path_planning/alars_auv_search_planner/config/params.yaml b/path_planning/alars_auv_search_planner/config/params.yaml
new file mode 100644
index 00000000..8dbd24a1
--- /dev/null
+++ b/path_planning/alars_auv_search_planner/config/params.yaml
@@ -0,0 +1,64 @@
+/**/SearchPlanner_Controller:
+  ros__parameters:
+    mode: "sim"
+    path_planner: "spiral"
+
+    sam:
+      init_pos: [1260.0, 1150.0]
+      init_pos_variance: 10
+      vel_variance: 4.0
+      max_floating_vel: 2.0
+
+    drone:
+      init_pos: [5.0, 5.0]
+      flight_height: 6.0
+      camera_fov: 82.0
+      intermediate_dt: 2.0
+      look_ahead_time: 1.0
+
+    initialization:
+      time_delay: 1.0
+
+    spiral:
+      vel_factor: 1.0
+      dtheta: 0.5235987756  # in radians (pi/6 as of now)
+
+    greedy:
+      horizon_radius: -1.0
+
+    astar:
+      obstacles:
+        max_length: 0.8
+        quantile_per: 70
+        obstacles_per: 5
+      horizon_radius: -1.0
+
+    arf:
+      k_attractive: 100
+      k_repulsive: 70
+      goal_distance_factor: 70
+      d_min: 2
+      d_max: 30
+      horizon_radius: 20
+
+    grid_map:
+      workspace:
+        width: 120.0
+        height: 120.0
+        resol: 2.0
+        variance: 10.0
+      update:
+        rate: 0.5
+        true_detection_rate: 0.9999999999  # 1.0 - 1e-10
+        time_margin: 10.0
+
+    battery:
+      discharge_rate: 1.0
+      threshold: 20
+      equivalent_drone_vel: 1.0
+
+    frames:
+      id:
+        map: "map_gt"
+        quadrotor_odom: "Quadrotor/odom"
+        sam_odom: "sam_auv_v1/odom"
\ No newline at end of file
diff --git a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
index b227cd14..2408c9b3 100644
--- a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
+++ b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
@@ -1,97 +1,51 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-from math import pi
+from launch_ros.substitutions import FindPackageShare
 
 def generate_launch_description():
-    robot_name = LaunchConfiguration('robot_name')
 
-    # All configurable parameters (sorted by relevance)
-    params = {
+    # ---- frequently changed params as launch arguments ...
+    mode_arg = DeclareLaunchArgument('mode', default_value='sim')
+    path_planner_arg = DeclareLaunchArgument('path_planner', default_value='apf')
+    sam_init_pos_arg = DeclareLaunchArgument('sam_init_pos', default_value='[1260.0, 1150.0]')
+    drone_init_pos_arg = DeclareLaunchArgument('drone_init_pos', default_value='[5.0, 5.0]')
 
-    # --- choose path planner and vehicles' initial positions
-    'mode': 'sim', # If 'sim', it's assumed the user wants to test the pkg standalone. If 'real', appropriate service requests have to be made
-    'path_planner': 'apf', # 'spiral', 'greedy', 'astar', 'apf' -> path planner type, read documentation/readme
-    'sam.init_pos': [float(1260.0), float(1150.0)], # position to which sam will teleport (in map). User-defined, only useful in  'sim'
-    'drone.init_pos': [float(5), float(5)], # position to which drone will move (in odom) at the beginning
+    # ... and as node params
+    mode = LaunchConfiguration('mode')
+    path_planner = LaunchConfiguration('path_planner')
+    sam_init_pos = LaunchConfiguration('sam_init_pos')
+    drone_init_pos = LaunchConfiguration('drone_init_pos')
 
-    # --- common parameters to all path planners
-    'initialization.time_delay': float(1), # time between node execution and grid map initialization (in seconds)
-    'flight_height': 6.0,
-    'camera_fov': 82.0,
-    'intermediate_dt': float(2.0), # dt between path points in case drones gets too unstable.
-    # lat [s] × velocity sets the waypoint distance threshold. Recommended values for distance cap = 1: 
-    # Spiral = 0.6, Greedy = 3, A* = 2.
-    'look_ahead_time': float(1), 
+    # ---- rarely changed params from yaml (yaml has every parameter but launch arguments will override)
+    config_file = PathJoinSubstitution([
+        FindPackageShare('alars_auv_search_planner'),
+        'config',
+        'params.yaml'
 
-    # --- Spiral planner parameters
-    'spiral.vel_factor': 1.0, # the spiral center will move x times faster than AUV. Turn it to 0 to make a static spiral
-    'spiral.dtheta': float(pi/6), #increments in angle between spiral points (smaller value -> more points per spiral)
-
-    # --- Greedy parameters (greedy)
-    'greedy.horizon_radius': float(-1), # Heuristic planners pick the highest-probability cell within radius x; -1 uses full workspace
-
-    # --- AStar planner parameters
-    'astar.obstacles.max_length' : 0.8, #threshold on obstacle dimension wrt the minimum dimension of the workspace (rectangle)
-    'astar.obstacles.quantile_per': 70, # quantile of cells from which we'll try to define obstacles (x % of cells with lowest probability)
-    'astar.obstacles.obstacles_per': 5, # from cells chosen above, we'll chose a fraction randomly to define obstacles
-    'astar.horizon_radius': float(-1), # Heuristic planners pick the highest-probability cell within radius x; -1 uses full workspace
-
-    # --- Artificial Potential Field (ARF) parameters
-    #'arf.look_ahead_time': 5, # different from regular lat: it will say how far the waypoint is from current position, not a distance threshold!
-    'arf.k_attractive': 100, # k constant for attractive potential field (bigger -> goal will attract more)
-    'arf.k_repulsive': 70, # k constant for repulsive potential field (bigger -> neighbouring cells will repulse more)
-    'arf.goal_distance_factor': 70, # factor to determine how far is the goal (bigger factor -> smaller acceleration by resultant of forces -> closer goal). Think of it as mass
-    'arf.d_min': 2, # (minimum) distance threshold to define which cells will exert repulsive force
-    'arf.d_max': 30, # (maximum) distance threshold to define which cells will exert repulsive force
-    'arf.horizon_radius': float(20), # arf planner picks the highest-probability cell within radius x; -1 uses full workspace
-
-    # --- + SAM configs
-    'sam.initial_state.pos_variance': 10, # variance considered when sampling the pseudo GPS ping (sam position + noise)
-    'sam.vel_variance': 4.0, # variance considered when getting SAM velocity with noise (simulating real conditions, noise is Gaussian). Not used for now
-    'sam.max_floating_vel': 2.0, # maximum SAM velocity on water due to wind + waves. It's used in spiral to define a correct spiral
-
-    # --- Grid Map workspace and update parameters
-    'grid_map.workspace.width': float(120.0),
-    'grid_map.workspace.height': float(120.0),
-    'grid_map.workspace.resol': float(2), 
-    'grid_map.workspace.variance': 10.0, # gaussian variance considered in the prior distribution (initial grid map)
-    'grid_map.update.rate': 0.5, # frequency of Bayes Filter runs (prediction + update)
-    'grid_map.update.true_detection_rate': 1.0 - 1e-10, # parameter used in Bayes filter -> probability of having a true positive
-    'grid_map.update.time_margin': 10.0, # parameter to avoid updating repeated cells -> don't update cells that were updated x or less seconds ago
-
-    # --- Battery parameters
-    'battery.discharge_rate': 1.0, # % per min
-    'battery.threshold': 20, # after planning a path, if the final estimated battery % is below this threshold, the drone will return to base instead
-
-    # [m/s] -> (constant) velocity used in the time duration estimation of each path. This value is only used if velocity = 0 at the the time of the 
-    # calculation, otherwise the current drone velocity will be used. Set it to a small value
-    'battery.equivalent_drone_vel': 1.0, 
-
-    # # --- Drone initial state (in map_gt), don't change unless there was a change in frames
-    # 'drone_initial_state.position': [1297.0, 1156.0, 1.1628761291503906], # (x, y, z)
-    # 'drone_initial_state.orientation': [-3.285610546299722e-06, -2.3353104552370496e-06, -0.3826446831226349, -0.923895537853241], #x,y,z,w
-
-    # --- Frame's names
-    'frames.id.map': 'map_gt',
-    'frames.id.quadrotor_odom': 'Quadrotor/odom',
-    'frames.id.sam_odom': 'sam_auv_v1/odom',
+    ])
 
-    }
+    planner_node = Node(
+        package='alars_auv_search_planner',
+        executable='search_planner_controller',
+        namespace='Quadrotor',
+        output='screen',
+        parameters=[
+            config_file,
+            {
+                'mode': mode,
+                'path_planner': path_planner,
+                'sam.init_pos': sam_init_pos,
+                'drone.init_pos': drone_init_pos,
+            }
+        ]
+    )
 
     return LaunchDescription([
-        DeclareLaunchArgument(
-            'robot_name',
-            default_value='Quadrotor'
-        ),
-        Node(
-            package='alars_auv_search_planner',
-            executable='search_planner_controller',
-            namespace=robot_name,
-            output='screen',
-            parameters=[params]
-        )
+        mode_arg,
+        path_planner_arg,
+        sam_init_pos_arg,
+        drone_init_pos_arg,
+        planner_node,
     ])

From 0e258a58bfdc50295c1a8e4fb1bb0a0bf41c6c1f Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Wed, 25 Jun 2025 20:00:09 +0100
Subject: [PATCH 03/12] refactoring to account change in pkg params

---
 .../alars_auv_search_planner/path_planners.py | 17 +++++++--------
 .../alars_auv_search_planner/prob_grid_map.py |  4 ++--
 .../search_planner_controller.py              | 21 +++++++++++--------
 3 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
index edac0480..83b28dc0 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
@@ -65,8 +65,8 @@ def __init__(self, name = 'init_actions', params = None):
         if params:
             self.drone_init_xy = params["drone.init_pos"]
             self.sam_init_pos = params["sam.init_pos"]
-            self.sam_pos_var= params["sam.initial_state.pos_variance"]
-            self.flight_height = params["flight_height"]
+            self.sam_pos_var= params["sam.init_pos_variance"]
+            self.flight_height = params["drone.flight_height"]
             self.map_frame_id = params['frames.id.map'] 
             self.drone_odom_frame_id = params['frames.id.quadrotor_odom'] 
             self.sam_odom_frame_id = params['frames.id.sam_odom'] 
@@ -134,9 +134,6 @@ def get_GPSxy_ping(self) -> PointStamped :
             GPS_ping.header.frame_id = self.map_frame_id 
             GPS_ping.point.x = X[0]
             GPS_ping.point.y = X[1]
-            # TODO: remove later
-            GPS_ping.point.x = 1270.0
-            GPS_ping.point.y = 1165.0
             return GPS_ping
         else: return None
         
@@ -198,10 +195,10 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
 
         if params:        
             self.planner_type = params["path_planner"]
-            self.camera_fov = params["camera_fov"]
-            self.flight_height = params["flight_height"]
-            self.lat = params["look_ahead_time"]
-            self.intermediate_dt = params["intermediate_dt"]
+            self.camera_fov = params["drone.camera_fov"]
+            self.flight_height = params["drone.flight_height"]
+            self.lat = params["drone.look_ahead_time"]
+            self.intermediate_dt = params["drone.intermediate_dt"]
 
             self.battery_discharge_rate = params['battery.discharge_rate']
             self.battery_threshold = params['battery.threshold']
@@ -234,7 +231,7 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
         
         self.point_publisher = self.create_publisher(
             msg_type = PoseStamped,
-            topic = '/Quadrotor/go_to_setpoint',
+            topic = '/Quadrotor/move_to_setpoint',
             qos_profile= 10)
         self.path_publisher = self.create_publisher( # visualization purposes only
             msg_type = Path,
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
index 5795a854..c5744b1d 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
@@ -56,8 +56,8 @@ def __init__(self, name = "SearchPlanner_gridmap", params:dict = None, GPS_ping:
             self.beta = params["grid_map.update.true_detection_rate"]
             self.time_margin = params["grid_map.update.time_margin"]*1e9 #nanoseconds
 
-            self.camera_fov = (pi/180)*params["camera_fov"]
-            self.flight_height = params["flight_height"]
+            self.camera_fov = (pi/180)*params["drone.camera_fov"]
+            self.flight_height = params["drone.flight_height"]
 
             self.map_frame_id = params['frames.id.map'] 
             self.drone_odom_frame_id = params['frames.id.quadrotor_odom'] 
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
index 3de719ba..e3d18ac5 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
@@ -58,6 +58,9 @@ def __init__(self):
         # calculate necessary timer calls (counts) to perform a task. Planner will only start when the update in the grid map is
         # ready to be applied (otherwise the drone will start moving but the cells' probabilities won't be updated)
         self.path_update_dt = 0.2 # it's not that relevant, therefore there isn't need to be in the launch file
+        print(self.model_params['mode'])
+        print(self.model_params['drone.init_pos'])
+        print(self.model_params['initialization.time_delay'])
         self.grid_update_dt = self.model_params['grid_map.update.rate']
         self.countsToInitializeMap =  int(self.model_params['initialization.time_delay']/self.grid_update_dt) + 1
         self.countsToUpdateMap = self.countsToInitializeMap + int(self.model_params["grid_map.update.time_margin"]/self.grid_update_dt)
@@ -100,7 +103,7 @@ def get_path_srv_callback(self, request, response):
                 pose = Pose()
                 pose.position.x = position[0]
                 pose.position.y = position[1]
-                pose.position.z = self.model_params["flight_height"]
+                pose.position.z = self.model_params["drone.flight_height"]
                 pose_list.append(pose)
             path_msg.poses = pose_list
             path_msg.header.frame_id = self.model_params['frames.id.quadrotor_odom']
@@ -228,14 +231,13 @@ def get_params(self):
         self.model_params = {
             "mode": self.get_parameter("mode").value,
             "path_planner": self.get_parameter("path_planner").value,
-            "drone.init_pos": self.get_parameter("drone.init_pos").value,
-            "sam.init_pos": self.get_parameter("sam.init_pos").value,
-
             'initialization.time_delay': self.get_parameter("initialization.time_delay").value,
-            "flight_height": self.get_parameter("flight_height").value,
-            "camera_fov": self.get_parameter("camera_fov").value,
-            "look_ahead_time": self.get_parameter("look_ahead_time").value,
-            "intermediate_dt": self.get_parameter("intermediate_dt").value,
+
+            "drone.init_pos": self.get_parameter("drone.init_pos").value,
+            "drone.flight_height": self.get_parameter("drone.flight_height").value,
+            "drone.camera_fov": self.get_parameter("drone.camera_fov").value,
+            "drone.look_ahead_time": self.get_parameter("drone.look_ahead_time").value,
+            "drone.intermediate_dt": self.get_parameter("drone.intermediate_dt").value,
 
             "spiral.vel_factor": self.get_parameter("spiral.vel_factor").value,
             "spiral.dtheta": self.get_parameter("spiral.dtheta").value,
@@ -254,7 +256,8 @@ def get_params(self):
             'arf.d_max': self.get_parameter("arf.d_max").value,
             'arf.horizon_radius': self.get_parameter("arf.horizon_radius").value,
             
-            "sam.initial_state.pos_variance": self.get_parameter("sam.initial_state.pos_variance").value,
+            "sam.init_pos": self.get_parameter("sam.init_pos").value,
+            "sam.init_pos_variance": self.get_parameter("sam.init_pos_variance").value,
             'sam.vel_variance': self.get_parameter('sam.vel_variance').value,
             'sam.max_floating_vel': self.get_parameter('sam.max_floating_vel').value, 
 

From 1900bdf77b3310dd5563e3226488a808878b2d4a Mon Sep 17 00:00:00 2001
From: Francisco Miranda <148491717+Chico1904@users.noreply.github.com>
Date: Sat, 28 Jun 2025 16:35:46 +0100
Subject: [PATCH 04/12] Update parameters README.md

---
 .../alars_auv_search_planner/config/README.md      | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/path_planning/alars_auv_search_planner/config/README.md b/path_planning/alars_auv_search_planner/config/README.md
index 4c023485..03e7c848 100644
--- a/path_planning/alars_auv_search_planner/config/README.md
+++ b/path_planning/alars_auv_search_planner/config/README.md
@@ -1,14 +1,14 @@
 # Search planning parameters
-A brief explanation of parameter:
+A brief explanation of each parameter:
 
 ## Launch arguments (params that are changed regularly)
 
 | **Parameter**    | **Description**                                                                                                                |
 | ---------------- | ------------------------------------------------------------------------------------------------------------------------------ |
 | `mode`           | If `sim`, it's assumed the user wants to test the package standalone. If `real`, appropriate service requests have to be made. |
-| `path_planner`   | `spiral`, `greedy`, `astar`, or `apf` → Path planner type. See documentation for details.                                      |
+| `path_planner`   | `spiral`, `greedy`, `astar`, or `apf` → Path planner type. See code documentation for details.                                      |
 | `sam.init_pos`   | Position to which SAM will teleport (in map). User-defined, only useful in `sim`.                                              |
-| `drone.init_pos` | Position to which the drone will move (in `odom`) at the beginning.                                                            |
+| `drone.init_pos` | Position to which the drone will move (in `odom`) at the beginning. User-defined, only useful in `sim`.                                                             |
 
 ## Drone params
 | **Parameter**               | **Description**                                                                                                        |
@@ -22,7 +22,7 @@ A brief explanation of parameter:
 ## Spiral params
 | **Parameter**       | **Description**                                                                                |
 | ------------------- | ---------------------------------------------------------------------------------------------- |
-| `spiral.vel_factor` | The spiral center will move `x` times faster than the AUV. Set to `0` for a static spiral.     |
+| `spiral.vel_factor` | The spiral center will move `x` times faster than the AUV. Set to `0` for a static spiral center.     |
 | `spiral.dtheta`     | Angle increment between spiral points in radians. Smaller values = denser spiral. (e.g., π/6). |
 
 ## Greedy params
@@ -58,11 +58,11 @@ A brief explanation of parameter:
 ## Grid map params
 | **Parameter**                         | **Description**                                                      |
 | ------------------------------------- | -------------------------------------------------------------------- |
-| `grid_map.workspace.width`            | Width of the grid map (in meters).                                   |
-| `grid_map.workspace.height`           | Height of the grid map (in meters).                                  |
+| `grid_map.workspace.width`            | Width of the grid map (in meters). Only useful in `sim`.                                      |
+| `grid_map.workspace.height`           | Height of the grid map (in meters). Only useful in `sim`.                                  |
 | `grid_map.workspace.resol`            | Grid cell resolution (in meters per cell).                           |
 | `grid_map.workspace.variance`         | Gaussian variance used in the initial grid distribution.             |
-| `grid_map.update.rate`                | Bayes Filter update frequency (Hz).                                  |
+| `grid_map.update.rate`                | Bayes Filter update period (s).                                  |
 | `grid_map.update.true_detection_rate` | Probability of a true positive in the Bayes Filter.                  |
 | `grid_map.update.time_margin`         | Prevents re-updating of recently updated cells (within `x` seconds). |
 

From 7cb3d70d005da17440cdea318b9905a8df744d1e Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Mon, 30 Jun 2025 16:52:58 +0100
Subject: [PATCH 05/12] srv now receives latlon. init srv reinitiates planner

---
 .../smarc_mission_msgs/srv/InitAUVSearch.srv  |  7 +--
 .../alars_auv_search_planner/path_planners.py | 20 +++++--
 .../search_planner_controller.py              | 54 +++++++++++--------
 .../test_initmap_srv.py                       | 20 +++----
 4 files changed, 61 insertions(+), 40 deletions(-)

diff --git a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
index 621bd576..ac912573 100644
--- a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
+++ b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
@@ -1,7 +1,8 @@
-#request
-geometry_msgs/PointStamped gps
+#request 
+geographic_msgs/GeoPoint gps
+
+float64 radius 
 
-geometry_msgs/PointStamped quadrotor_ipos
 ---
 #response
 bool success
\ No newline at end of file
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
index 83b28dc0..643a3ca2 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
@@ -35,6 +35,7 @@
 import numpy as np
 from math import cos, sin, pi, sqrt, tan, factorial, dist
 from .Astar import AStar
+from .prob_grid_map import ProbabilisticGridMap
 
 class InitializeActions(Node):
     """
@@ -173,7 +174,7 @@ class SearchPlanner(Node, ABC):
     Notes:
 
     """
-    def __init__(self, name="pathplanner_parent", params = None, grid_map = None):
+    def __init__(self, name="pathplanner_parent", params = None, grid_map:ProbabilisticGridMap = None):
         super().__init__(node_name = name)
         self.get_logger().info('Parent search planner initialized')
 
@@ -263,10 +264,11 @@ def generate_waypoint(self, x, y, z):
         current_pos_odom = self.transform_point(self.drone_position)
         return current_pos_odom.point.x, current_pos_odom.point.y
         
-    def transform_point(self, point:PointStamped) -> PointStamped:
+    def transform_point(self, point:PointStamped, frame:str = None) -> PointStamped:
+        if frame is None: frame =  self.drone_odom_frame_id
         # transform desired position (relative position) to odom frame
         t = self.tf_buffer.lookup_transform(
-            target_frame = self.drone_odom_frame_id,  
+            target_frame = frame,  
             source_frame = point.header.frame_id, #point.header.frame_id, #self.map_frame_id,                 
             time=rclpy.time.Time() )
         return tf2_geometry_msgs.do_transform_point(point, t)
@@ -364,6 +366,18 @@ def return_to_base(self):
         self.point_publisher.publish(pose_msg)
         odom_position = self.transform_point(self.drone_position)
         return odom_position.point.x, odom_position.point.y
+    
+    def reinitialize_planner(self):
+        """ 
+        Reinitializes grid map and planner state (called when client makes requests a new service and 
+        the search planning has to be reinitiated without destroying the node)
+        """
+        if self.params['path_planner'] == 'spiral': self.phase = 'line'
+        self.path_needed = True
+        self.path_completed = False
+        self.wait_finished = True
+        self.grid_map.initiate_grid_map()
+        
 
         
     def drone_odom_callback(self, msg: Odometry):
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
index e3d18ac5..30e42460 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
@@ -3,12 +3,12 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
-from .prob_grid_map import ProbabilisticGridMap 
-from .path_planners import InitializeActions, SpiralPathModel, GreedyPathModel, AStarPathModel, APFPathModel
 from math import dist
 from smarc_mission_msgs.srv import DronePath, InitAUVSearch
 from geometry_msgs.msg import PoseArray, Pose, PointStamped
-
+from .prob_grid_map import ProbabilisticGridMap 
+from .path_planners import InitializeActions, SpiralPathModel, GreedyPathModel, AStarPathModel, APFPathModel
+from smarc_utilities.georef_utils import convert_latlon_to_utm
 
 ##############################################################################
 # TODO
@@ -58,9 +58,6 @@ def __init__(self):
         # calculate necessary timer calls (counts) to perform a task. Planner will only start when the update in the grid map is
         # ready to be applied (otherwise the drone will start moving but the cells' probabilities won't be updated)
         self.path_update_dt = 0.2 # it's not that relevant, therefore there isn't need to be in the launch file
-        print(self.model_params['mode'])
-        print(self.model_params['drone.init_pos'])
-        print(self.model_params['initialization.time_delay'])
         self.grid_update_dt = self.model_params['grid_map.update.rate']
         self.countsToInitializeMap =  int(self.model_params['initialization.time_delay']/self.grid_update_dt) + 1
         self.countsToUpdateMap = self.countsToInitializeMap + int(self.model_params["grid_map.update.time_margin"]/self.grid_update_dt)
@@ -74,23 +71,33 @@ def __init__(self):
 
         # initialize planner and grid map but without GPS ping and drone position; move drone to flight height (solely vertically)
         self.grid_map = ProbabilisticGridMap(params = self.model_params)
-
-        if self.model_params['path_planner'] == 'spiral':
-            self.planner = SpiralPathModel(params = self.model_params, grid_map = self.grid_map)
-        elif self.model_params['path_planner'] == 'greedy':
-            self.planner = GreedyPathModel(params = self.model_params, grid_map = self.grid_map)
-        elif self.model_params['path_planner'] == 'astar':
-            self.planner = AStarPathModel(params = self.model_params, grid_map = self.grid_map)
-        elif self.model_params['path_planner'] == 'apf':
-            self.planner = APFPathModel(params = self.model_params, grid_map = self.grid_map)
-        else:
-            self.get_logger().error('Incorrect path planner label! Check launch file')
+        planner_dict = {'spiral': SpiralPathModel,
+                        'greedy': GreedyPathModel,
+                        'astar': AStarPathModel,
+                        'apf': APFPathModel}
+        try:
+            self.planner = planner_dict[self.model_params['path_planner']](params = self.model_params, grid_map = self.grid_map)
+        except:
+            self.get_logger().error('Incorrect path planner label! Check launch and README files')
 
     def init_search_srv_callback(self, request, response):
-        """ Stores the GPS_ping and desired quadrotor initial position, which will trigger path generation in respective timer."""
-        self.GPS_ping = request.gps
-        self.drone_init_pos = self.planner.transform_point(request.quadrotor_ipos)
+        """ 
+        Stores the GPS_ping (after transforming it to map) and desired quadrotor initial position, 
+        which will trigger path generation in respective timer.
+        """
+        # if activated by client, quadrotor doesn't perform initial movement (assigning purposes only)
+        self.drone_init_pos = PointStamped()
+        self.drone_init_pos.header.frame_id = self.model_params['frames.id.quadrotor_odom']      
+
+        # get search radius (range) convert GPS ping to correct coordinates
+        self.grid_map.w = self.grid_map.h = 2*request.radius
+        self.GPS_ping_utm = convert_latlon_to_utm(request.gps)
+        self.GPS_ping = self.planner.transform_point(self.GPS_ping_utm, self.model_params['frames.id.map'])
         response.success = True
+
+        # (re)initialize planner (including grid map)
+        self.grid_map.GPS_ping = self.GPS_ping
+        self.planner.reinitialize_planner()
         return response
 
     def get_path_srv_callback(self, request, response):
@@ -142,8 +149,11 @@ def attemp_retrieval(self):
                 self.get_logger().info(f'Received GPS ping (x,y) = {round(float(self.GPS_ping.point.x),2), round(float(self.GPS_ping.point.y),2)}')
                 if None in (self.drone_init_pos.point.x, self.drone_init_pos.point.y,  self.drone_init_pos.point.z):
                     self.init_done = True
-                else:
-                    self.relocate_timer = self.create_timer(0.5, self.check_drone_position)
+                else: # create timer to relocate drone (valid only in mode = sim)
+                    if self.model_params['mode'] == 'sim':
+                        self.relocate_timer = self.create_timer(0.5, self.check_drone_position)
+                    else:
+                        self.init_done = True
 
     def check_drone_position(self):
         """ Continously publish initial waypoint and checking distance"""
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
index a8330a51..d42e7976 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
@@ -2,7 +2,8 @@
 import rclpy
 from rclpy.node import Node
 from smarc_mission_msgs.srv import DronePath, InitAUVSearch
-from geometry_msgs.msg import PointStamped, PoseArray
+from geometry_msgs.msg import PointStamped
+from geographic_msgs.msg import GeoPoint
 
 
 class TestSrv(Node):
@@ -20,19 +21,14 @@ def send_init_gridmap_request(self):
         # Define GPS ping and initial drone position
         self.req = InitAUVSearch.Request()
 
-        gps = PointStamped()
-        gps.header.stamp = self.get_clock().now().to_msg()
-        gps.header.frame_id = 'map_gt_gt'
-        gps.point.x = 1268.0
-        gps.point.y = 1150.0
+        gps = GeoPoint()
+        gps.latitude = 58.85058132601718 #58.85028132601718
+        gps.longitude = 17.67416659875381 #17.67486659875381
+        gps.altitude = 1.1628758907318115 #11.1628758907318115
+        
         self.req.gps = gps
 
-        quadrotor_ipos = PointStamped()
-        quadrotor_ipos.header.stamp = self.get_clock().now().to_msg()
-        quadrotor_ipos.header.frame_id = 'Quadrotor/odom_gt'
-        quadrotor_ipos.point.x = 2.0
-        quadrotor_ipos.point.y = 2.0
-        self.req.quadrotor_ipos = quadrotor_ipos
+        self.req.radius = 100.0
 
         self.future = self.init_gridmap_client.call_async(self.req)
         rclpy.spin_until_future_complete(self, self.future)

From 6d87b885b6a66c43d741739a89bcd86c8f432ea0 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Mon, 30 Jun 2025 16:53:25 +0100
Subject: [PATCH 06/12] update readme (srv section)

---
 path_planning/alars_auv_search_planner/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/path_planning/alars_auv_search_planner/README.md b/path_planning/alars_auv_search_planner/README.md
index 94b52e68..be99751e 100644
--- a/path_planning/alars_auv_search_planner/README.md
+++ b/path_planning/alars_auv_search_planner/README.md
@@ -33,7 +33,7 @@ If the user wants to integrate this in a broader task, two clients have to be cr
 ## **(ROS) Srv**
 | Service name | Service Type | Description |
 | --- | ---| ---|
-| 'init_auv_search' | InitAUVSearch | It initiates the **PGM**. It should be requested when the drone is +or- within the desired workspace and search planning is about to initiate. It needs the GPS ping and the initial drone position in any frame connected to Quadrotor's odom|
+| 'init_auv_search' | InitAUVSearch | It initiates the **PGM**. It should be requested when the search planning is about to initiate. It needs the GPS ping and the initial drone position in any frame connected to Quadrotor's odom. It can be called several times in a single simulation since the planner and the grid map are reinitialized when this service is requested|
 | 'get_quadrotor_path' | DronePath |  It computes a path based on the **PGM** and returns it as a PoseArray |
 ---
 Check *test_initmap_srv.py* and *test_getpath_srv.py* to see how the client can be set up.

From 5452e0c7ec38433c1102dc047543e0a53081329c Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Mon, 30 Jun 2025 16:54:12 +0100
Subject: [PATCH 07/12] refactoring+testing params

---
 path_planning/alars_auv_search_planner/config/params.yaml   | 2 +-
 .../launch/search_planning_launch.py                        | 6 +++---
 path_planning/alars_auv_search_planner/setup.py             | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/path_planning/alars_auv_search_planner/config/params.yaml b/path_planning/alars_auv_search_planner/config/params.yaml
index 8dbd24a1..1d1077c4 100644
--- a/path_planning/alars_auv_search_planner/config/params.yaml
+++ b/path_planning/alars_auv_search_planner/config/params.yaml
@@ -5,7 +5,7 @@
 
     sam:
       init_pos: [1260.0, 1150.0]
-      init_pos_variance: 10
+      init_pos_variance: 0.1
       vel_variance: 4.0
       max_floating_vel: 2.0
 
diff --git a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
index 2408c9b3..1fca334e 100644
--- a/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
+++ b/path_planning/alars_auv_search_planner/launch/search_planning_launch.py
@@ -7,9 +7,9 @@
 def generate_launch_description():
 
     # ---- frequently changed params as launch arguments ...
-    mode_arg = DeclareLaunchArgument('mode', default_value='sim')
-    path_planner_arg = DeclareLaunchArgument('path_planner', default_value='apf')
-    sam_init_pos_arg = DeclareLaunchArgument('sam_init_pos', default_value='[1260.0, 1150.0]')
+    mode_arg = DeclareLaunchArgument('mode', default_value='real')
+    path_planner_arg = DeclareLaunchArgument('path_planner', default_value='spiral')
+    sam_init_pos_arg = DeclareLaunchArgument('sam_init_pos', default_value='[1297.0, 1153.0]')
     drone_init_pos_arg = DeclareLaunchArgument('drone_init_pos', default_value='[5.0, 5.0]')
 
     # ... and as node params
diff --git a/path_planning/alars_auv_search_planner/setup.py b/path_planning/alars_auv_search_planner/setup.py
index 2f6d99f4..69049ed3 100644
--- a/path_planning/alars_auv_search_planner/setup.py
+++ b/path_planning/alars_auv_search_planner/setup.py
@@ -27,7 +27,7 @@
         'console_scripts': [
             "search_planner_controller = alars_auv_search_planner.search_planner_controller:main",
             "test_getpath_srv = alars_auv_search_planner.test_getpath_srv:main",
-            "test_initmap_srv = alars_auv_search_planner.test_initmap_srv:main"
+            "test_initmap_srv = alars_auv_search_planner.test_initmap_srv:main",
         ],
     },
 )

From fa790e87db37e0452fe561f09dec5eec34b41ff0 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Mon, 30 Jun 2025 22:29:42 +0100
Subject: [PATCH 08/12] drone setpoint topic as node parameter

---
 .../alars_auv_search_planner/path_planners.py                | 2 +-
 path_planning/alars_auv_search_planner/config/params.yaml    | 5 ++++-
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
index 643a3ca2..8007b02d 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/path_planners.py
@@ -232,7 +232,7 @@ def __init__(self, name="pathplanner_parent", params = None, grid_map:Probabilis
         
         self.point_publisher = self.create_publisher(
             msg_type = PoseStamped,
-            topic = '/Quadrotor/move_to_setpoint',
+            topic = params['topics.move_drone'], 
             qos_profile= 10)
         self.path_publisher = self.create_publisher( # visualization purposes only
             msg_type = Path,
diff --git a/path_planning/alars_auv_search_planner/config/params.yaml b/path_planning/alars_auv_search_planner/config/params.yaml
index 1d1077c4..ceb2a052 100644
--- a/path_planning/alars_auv_search_planner/config/params.yaml
+++ b/path_planning/alars_auv_search_planner/config/params.yaml
@@ -61,4 +61,7 @@
       id:
         map: "map_gt"
         quadrotor_odom: "Quadrotor/odom"
-        sam_odom: "sam_auv_v1/odom"
\ No newline at end of file
+        sam_odom: "sam_auv_v1/odom"
+    
+    topics:
+      move_drone: "/Quadrotor/move_to_setpoint"
\ No newline at end of file

From 96ab8207c10da20068f3921b0fec578bfcbbf046 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Mon, 30 Jun 2025 22:30:20 +0100
Subject: [PATCH 09/12] added altitude in client request

---
 messages/smarc_mission_msgs/srv/InitAUVSearch.srv     |  2 ++
 .../search_planner_controller.py                      | 11 +++++++----
 .../alars_auv_search_planner/test_initmap_srv.py      |  2 +-
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
index ac912573..01d21a11 100644
--- a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
+++ b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
@@ -3,6 +3,8 @@ geographic_msgs/GeoPoint gps
 
 float64 radius 
 
+float64 altitude
+
 ---
 #response
 bool success
\ No newline at end of file
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
index 30e42460..147a6a71 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
@@ -89,10 +89,11 @@ def init_search_srv_callback(self, request, response):
         self.drone_init_pos = PointStamped()
         self.drone_init_pos.header.frame_id = self.model_params['frames.id.quadrotor_odom']      
 
-        # get search radius (range) convert GPS ping to correct coordinates
+        # get search radius (range) and altitude #TODO; convert GPS ping to correct coordinates
         self.grid_map.w = self.grid_map.h = 2*request.radius
-        self.GPS_ping_utm = convert_latlon_to_utm(request.gps)
-        self.GPS_ping = self.planner.transform_point(self.GPS_ping_utm, self.model_params['frames.id.map'])
+        self.planner.flight_height = self.planner.grid_map.flight_height = request.altitude
+        GPS_ping_utm = convert_latlon_to_utm(request.gps)
+        self.GPS_ping = self.planner.transform_point(GPS_ping_utm, self.model_params['frames.id.map'])
         response.success = True
 
         # (re)initialize planner (including grid map)
@@ -285,7 +286,9 @@ def get_params(self):
 
             'frames.id.map': self.get_parameter('frames.id.map').value,
             'frames.id.quadrotor_odom': self.get_parameter('frames.id.quadrotor_odom').value,
-            'frames.id.sam_odom': self.get_parameter('frames.id.sam_odom').value
+            'frames.id.sam_odom': self.get_parameter('frames.id.sam_odom').value,
+
+            'topics.move_drone': self.get_parameter('topics.move_drone').value
 
 
         }
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
index d42e7976..7bb77f94 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
@@ -27,8 +27,8 @@ def send_init_gridmap_request(self):
         gps.altitude = 1.1628758907318115 #11.1628758907318115
         
         self.req.gps = gps
-
         self.req.radius = 100.0
+        self.req.altitude = 7.5
 
         self.future = self.init_gridmap_client.call_async(self.req)
         rclpy.spin_until_future_complete(self, self.future)

From 5fbeeed33f05c91aacbd44c8fbd8e7ab5bdc3738 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Tue, 1 Jul 2025 15:31:04 +0100
Subject: [PATCH 10/12] drone's initial altitude wrt water level as param

---
 messages/smarc_mission_msgs/srv/InitAUVSearch.srv             | 2 +-
 .../alars_auv_search_planner/search_planner_controller.py     | 2 +-
 .../alars_auv_search_planner/test_initmap_srv.py              | 4 ++--
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
index 01d21a11..6f2a0102 100644
--- a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
+++ b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
@@ -3,7 +3,7 @@ geographic_msgs/GeoPoint gps
 
 float64 radius 
 
-float64 altitude
+float64 initial_altitude
 
 ---
 #response
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
index 147a6a71..3b5a0d52 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/search_planner_controller.py
@@ -91,7 +91,7 @@ def init_search_srv_callback(self, request, response):
 
         # get search radius (range) and altitude #TODO; convert GPS ping to correct coordinates
         self.grid_map.w = self.grid_map.h = 2*request.radius
-        self.planner.flight_height = self.planner.grid_map.flight_height = request.altitude
+        self.planner.flight_height = self.planner.grid_map.flight_height = request.initial_altitude + request.gps.altitude
         GPS_ping_utm = convert_latlon_to_utm(request.gps)
         self.GPS_ping = self.planner.transform_point(GPS_ping_utm, self.model_params['frames.id.map'])
         response.success = True
diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
index 7bb77f94..c709e28a 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/test_initmap_srv.py
@@ -24,11 +24,11 @@ def send_init_gridmap_request(self):
         gps = GeoPoint()
         gps.latitude = 58.85058132601718 #58.85028132601718
         gps.longitude = 17.67416659875381 #17.67486659875381
-        gps.altitude = 1.1628758907318115 #11.1628758907318115
+        gps.altitude = 4.5 #11.1628758907318115
         
         self.req.gps = gps
         self.req.radius = 100.0
-        self.req.altitude = 7.5
+        self.req.initial_altitude = 1.0
 
         self.future = self.init_gridmap_client.call_async(self.req)
         rclpy.spin_until_future_complete(self, self.future)

From 6018c8ca83d297419a2dce44c416a707bd2ca3e4 Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Tue, 1 Jul 2025 15:31:21 +0100
Subject: [PATCH 11/12] update REAMDE (srv section)

---
 path_planning/alars_auv_search_planner/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/path_planning/alars_auv_search_planner/README.md b/path_planning/alars_auv_search_planner/README.md
index be99751e..6301ea8b 100644
--- a/path_planning/alars_auv_search_planner/README.md
+++ b/path_planning/alars_auv_search_planner/README.md
@@ -33,7 +33,7 @@ If the user wants to integrate this in a broader task, two clients have to be cr
 ## **(ROS) Srv**
 | Service name | Service Type | Description |
 | --- | ---| ---|
-| 'init_auv_search' | InitAUVSearch | It initiates the **PGM**. It should be requested when the search planning is about to initiate. It needs the GPS ping and the initial drone position in any frame connected to Quadrotor's odom. It can be called several times in a single simulation since the planner and the grid map are reinitialized when this service is requested|
+| 'init_auv_search' | InitAUVSearch | It initiates the **PGM**. It should be requested when the search planning is about to initiate. It needs the search radius around the GPS ping, the GPS ping (GeoPoint msg) and the drone's initial altitude wrt to the water level. It can be called several times in a single simulation since the planner and the grid map are reinitialized when this service is requested|
 | 'get_quadrotor_path' | DronePath |  It computes a path based on the **PGM** and returns it as a PoseArray |
 ---
 Check *test_initmap_srv.py* and *test_getpath_srv.py* to see how the client can be set up.

From 72999494b325b2632145177f80f10ca2c803e58f Mon Sep 17 00:00:00 2001
From: Francisco <franmira2003@gmail.com>
Date: Tue, 1 Jul 2025 15:32:06 +0100
Subject: [PATCH 12/12] removed prints

---
 .../alars_auv_search_planner/prob_grid_map.py                    | 1 -
 1 file changed, 1 deletion(-)

diff --git a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
index c5744b1d..651ac3ef 100644
--- a/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
+++ b/path_planning/alars_auv_search_planner/alars_auv_search_planner/prob_grid_map.py
@@ -90,7 +90,6 @@ def initiate_grid_map(self) -> None:
 
         # create grid map without probabilities (just its dimensions) as class attributes and ros msg.
         self.GPS_ping_odom, _ = self.transform_point(self.GPS_ping)
-        self.get_logger().info(f'Received GPS ping in odom (x,y) = {round(float(self.GPS_ping_odom[0]),2), round(float(self.GPS_ping_odom[1]),2)}')
         self.createMap(self.GPS_ping_odom) 
 
         # gaussian prior, scale probabilities to fit int -> viewing purposes only