diff --git a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
index 621bd576..6f2a0102 100644
--- a/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
+++ b/messages/smarc_mission_msgs/srv/InitAUVSearch.srv
@@ -1,7 +1,10 @@
-#request
-geometry_msgs/PointStamped gps
+#request 
+geographic_msgs/GeoPoint gps
+
+float64 radius 
+
+float64 initial_altitude
 
-geometry_msgs/PointStamped quadrotor_ipos
 ---
 #response
 bool success
\ No newline at end of file
diff --git a/path_planning/alars_auv_search_planner/README.md b/path_planning/alars_auv_search_planner/README.md
index 94b52e68..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 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 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.
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..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
@@ -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):
     """
@@ -65,10 +66,11 @@ 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.drone_map_frame_id = params['frames.id.quadrotor_map'] 
+            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'] 
         else:
             self.get_logger().error("No valid parameters received in SearchPlanner node")
 
@@ -76,10 +78,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 +103,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,11 +128,11 @@ 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]
             return GPS_ping
@@ -176,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')
 
@@ -187,6 +185,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
 
@@ -197,17 +196,18 @@ 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']
             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")
 
@@ -232,12 +232,16 @@ 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 = params['topics.move_drone'], 
             qos_profile= 10)
         self.path_publisher = self.create_publisher( # visualization purposes only
             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     
@@ -260,11 +264,12 @@ 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,  
-            source_frame = point.header.frame_id, #point.header.frame_id, #self.drone_map_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)
     
@@ -303,7 +308,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
@@ -362,18 +366,35 @@ 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):
-        """ 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 +888,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..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
@@ -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:
 
@@ -54,11 +56,12 @@ 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.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,8 +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.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.GPS_ping_odom, _ = self.transform_point(self.GPS_ping)
         self.createMap(self.GPS_ping_odom) 
 
         # gaussian prior, scale probabilities to fit int -> viewing purposes only
@@ -132,18 +134,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 +236,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 +295,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 +361,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..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
@@ -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
@@ -71,23 +71,34 @@ 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) 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.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
+
+        # (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):
@@ -100,9 +111,11 @@ 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']
+            path_msg.header.stamp = self.get_clock().now().to_msg()
             response.path = path_msg 
             return response
         else: return None
@@ -137,8 +150,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"""
@@ -226,14 +242,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,
@@ -252,7 +267,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, 
 
@@ -268,8 +284,11 @@ 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,
+
+            '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 a8330a51..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
@@ -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 = 4.5 #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.req.initial_altitude = 1.0
 
         self.future = self.init_gridmap_client.call_async(self.req)
         rclpy.spin_until_future_complete(self, self.future)
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..03e7c848
--- /dev/null
+++ b/path_planning/alars_auv_search_planner/config/README.md
@@ -0,0 +1,88 @@
+# Search planning parameters
+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 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. User-defined, only useful in `sim`.                                                             |
+
+## 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 center.     |
+| `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). 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 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). |
+
+## 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..ceb2a052
--- /dev/null
+++ b/path_planning/alars_auv_search_planner/config/params.yaml
@@ -0,0 +1,67 @@
+/**/SearchPlanner_Controller:
+  ros__parameters:
+    mode: "sim"
+    path_planner: "spiral"
+
+    sam:
+      init_pos: [1260.0, 1150.0]
+      init_pos_variance: 0.1
+      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"
+    
+    topics:
+      move_drone: "/Quadrotor/move_to_setpoint"
\ 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 79f3d6d4..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
@@ -1,96 +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='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]')
 
-    # --- 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
-    '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'
-    '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(2), 
+    # ---- 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.quadrotor_map': 'map_gt_gt',
-    'frames.id.quadrotor_odom': 'Quadrotor/odom_gt',
+    ])
 
-    }
+    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,
     ])
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",
         ],
     },
 )