Merge pull request #178 from KKalem/humble

Alars search

Author: KKalem

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

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.

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,21 +66,18 @@ 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")
 
         self.teleport_sam_publisher = self.create_publisher(
             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:

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