Skip to content

Alars search #178

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 13 commits into from
Jul 2, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions messages/smarc_mission_msgs/srv/InitAUVSearch.srv
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion path_planning/alars_auv_search_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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):
"""
Expand Down Expand Up @@ -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,
Expand All @@ -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]
Expand All @@ -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
Expand Down Expand Up @@ -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')

Expand All @@ -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

Expand All @@ -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")

Expand All @@ -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
Expand All @@ -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)

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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"""
Expand Down Expand Up @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:

Expand All @@ -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")
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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 """
Expand Down Expand Up @@ -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"""
Expand Down Expand Up @@ -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

Loading