Skip to content
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.

Sign up for GitHub

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

Red/green left/right buoy pair detection #197

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Red/green left/right buoy pair detection #197

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6232f1a
It now detects if red is left or right and finds the buoy pairs accor…
pliam1105 Feb 14, 2025
3b774e4
Minor stuff
pliam1105 Feb 14, 2025
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
179 changes: 116 additions & 63 deletions all_seaing_autonomy/all_seaing_autonomy/roboboat/waypoint_finder.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
import yaml
import rclpy
from rclpy.node import Node
from all_seaing_interfaces.msg import ObstacleMap
from all_seaing_interfaces.msg import ObstacleMap, Obstacle
from geometry_msgs.msg import Point, Pose, Vector3, Quaternion
from all_seaing_interfaces.msg import BuoyPair, BuoyPairArray, Waypoint, WaypointArray
from nav_msgs.msg import Odometry
Expand All @@ -14,7 +14,14 @@
from rclpy.action import ActionClient, ActionServer
from all_seaing_interfaces.action import FollowPath


class InternalBuoyPair():
def __init__(self):
self.left = Obstacle()
self.right = Obstacle()

def __init__(self, left_buoy, right_buoy):
self.left = left_buoy
self.right = right_buoy

class WaypointFinder(Node):
def __init__(self):
Expand Down Expand Up @@ -64,6 +71,8 @@ def __init__(self):
# self.last_sent_waypoint = None
self.sent_waypoints = set()

self.red_left = True

def norm_squared(self, vec, ref=(0, 0)):
return vec[0] ** 2 + vec[1] ** 2

Expand All @@ -86,7 +95,7 @@ def midpoint(self, vec1, vec2):
return ((vec1[0] + vec2[0]) / 2, (vec1[1] + vec2[1]) / 2)

def midpoint_pair(self, pair):
return self.midpoint(self.ob_coords(pair[0]), self.ob_coords(pair[1]))
return self.midpoint(self.ob_coords(pair.left), self.ob_coords(pair.right))

def split_buoys(self, obstacles):
"""
Expand Down Expand Up @@ -124,26 +133,48 @@ def buoy_pairs_to_markers(self, buoy_pairs):
id=(4 * i),
)
)
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.left)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(r=1.0, a=1.0),
id=(4 * i) + 1,
if(self.red_left):
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.left)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(r=1.0, a=1.0),
id=(4 * i) + 1,
)
)
)
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.right)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(g=1.0, a=1.0),
id=(4 * i) + 2,
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.right)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(g=1.0, a=1.0),
id=(4 * i) + 2,
)
)
else:
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.left)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(g=1.0, a=1.0),
id=(4 * i) + 1,
)
)
marker_array.markers.append(
Marker(
type=Marker.SPHERE,
pose=self.pair_to_pose(self.ob_coords(buoy_pair.right)),
header=Header(frame_id="odom"),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(r=1.0, a=1.0),
id=(4 * i) + 2,
)
)
)
marker_array.markers.append(
Marker(
type=Marker.CYLINDER,
Expand Down Expand Up @@ -179,19 +210,31 @@ def setup_buoys(self):
green_buoys, red_buoys = obstacles_in_front(green_init), obstacles_in_front(
red_init
)
self.get_logger().info(
self.get_logger().debug(
f"initial red buoys: {red_buoys}, green buoys: {green_buoys}"
)
if len(red_buoys) == 0 or len(green_buoys) == 0:
self.get_logger().info("No starting buoy pairs!")
self.get_logger().debug("No starting buoy pairs!")
return False

# from the red buoys that are in front of the robot, take the one that is closest to it, and do the same for the green buoys
# this pair is the front pair of the starting box of the robot
self.starting_buoys = (
self.get_closest_to((0, 0), red_buoys, local=True),
self.get_closest_to((0, 0), green_buoys, local=True),
)
closest_red = self.get_closest_to((0, 0), red_buoys, local=True)
closest_green = self.get_closest_to((0, 0), green_buoys, local=True)
if(self.ccw((0,0), self.ob_coords(closest_green, local=True), self.ob_coords(closest_red, local=True))):
self.red_left = True
self.starting_buoys = InternalBuoyPair(
self.get_closest_to((0, 0), red_buoys, local=True),
self.get_closest_to((0, 0), green_buoys, local=True),
)
self.get_logger().debug('RED BUOYS LEFT, GREEN BUOYS RIGHT')
else:
self.red_left = False
self.starting_buoys = InternalBuoyPair(
self.get_closest_to((0, 0), green_buoys, local=True),
self.get_closest_to((0, 0), red_buoys, local=True),
)
self.get_logger().debug('GREEN BUOYS LEFT, RED BUOYS RIGHT')
self.pair_to = self.starting_buoys
return True

Expand Down Expand Up @@ -268,44 +311,57 @@ def filter_front_buoys(self, pair, buoys):
buoy
for buoy in buoys
if self.ccw(
self.ob_coords(pair[0]), self.ob_coords(pair[1]), self.ob_coords(buoy)
self.ob_coords(pair.left), self.ob_coords(pair.right), self.ob_coords(buoy)
)
]

def next_pair(self, prev_pair, red, green):
"""
Returns the next buoy pair (red left, green right) from the previous pair,
Returns the next buoy pair from the previous pair,
by checking the closest one to the middle of the previous buoy pair that's in front of the pair
"""

front_red = self.filter_front_buoys(prev_pair, red)
front_green = self.filter_front_buoys(prev_pair, green)

if not front_red or not front_green:
prev_coords = self.ob_coords(prev_pair[0]), self.ob_coords(prev_pair[1])
prev_coords = self.ob_coords(prev_pair.left), self.ob_coords(prev_pair.right)
red_coords = self.obs_to_pos(red)
green_coords = self.obs_to_pos(green)

self.get_logger().info(f"buoys: {prev_coords} \nred: {red_coords} \ngreen: {green_coords}")
self.get_logger().info(f"robot: {self.robot_pos}")
self.get_logger().info("Missing at least one front buoy")
self.get_logger().debug(f"buoys: {prev_coords} \nred: {red_coords} \ngreen: {green_coords}")
self.get_logger().debug(f"robot: {self.robot_pos}")
self.get_logger().debug("Missing at least one front buoy")
return None

if prev_pair is not None:
prev_pair_midpoint = self.midpoint_pair(prev_pair)
self.get_logger().info(f"prev pair midpoint: {prev_pair_midpoint}")
return (
self.get_closest_to(prev_pair_midpoint, front_red),
self.get_closest_to(prev_pair_midpoint, front_green),
)
self.get_logger().debug(f"prev pair midpoint: {prev_pair_midpoint}")
if(self.red_left):
return InternalBuoyPair(
self.get_closest_to(prev_pair_midpoint, front_red),
self.get_closest_to(prev_pair_midpoint, front_green),
)
else:
return InternalBuoyPair(
self.get_closest_to(prev_pair_midpoint, front_green),
self.get_closest_to(prev_pair_midpoint, front_red),
)
else:
self.get_logger().info("No previous pair!")
self.get_logger().info(f"next buoys: red: {self.get_closest_to(self.robot_pos, red)}, green: {self.get_closest_to(self.robot_pos, green)}")
self.get_logger().debug("No previous pair!")
#TODO: change those to: self.get_closest_to((0,0), red/green, local=True) to not have to use odometry position but just local obstacle positions (wrt the robot)?
self.get_logger().debug(f"next buoys: red: {self.get_closest_to(self.robot_pos, red)}, green: {self.get_closest_to(self.robot_pos, green)}")
# if there is no previous pair, just take the closest red and green buoys
return (
self.get_closest_to(self.robot_pos, red),
self.get_closest_to(self.robot_pos, green),
)
if(self.red_left):
return InternalBuoyPair(
self.get_closest_to(self.robot_pos, red),
self.get_closest_to(self.robot_pos, green),
)
else:
return InternalBuoyPair(
self.get_closest_to(self.robot_pos, green),
self.get_closest_to(self.robot_pos, red),
)

def pair_to_pose(self, pair):
return Pose(position=Point(x=pair[0], y=pair[1]))
Expand All @@ -329,27 +385,26 @@ def generate_waypoints(self):
"""
# split the buoys into red and green
green_buoys, red_buoys = self.split_buoys(self.obstacles)
self.get_logger().info(
self.get_logger().debug(
f"robot pos: {self.robot_pos}, red buoys: {self.obs_to_pos(red_buoys)}, green buoys: {self.obs_to_pos(green_buoys)}"
)
# RED BUOYS LEFT, GREEN RIGHT

# TODO: Match the previous pair of buoys to the new obstacle map (in terms of global position) to eliminate any big drift that may mess up the selection of the next pair

if self.pair_to is None:
self.get_logger().info("No pair to go to.")
self.get_logger().debug("No pair to go to.")
return
# Check if we passed that pair of buoys (the robot is in front of the pair), then move on to the next one
if self.ccw(
self.ob_coords(self.pair_to[0]),
self.ob_coords(self.pair_to[1]),
self.ob_coords(self.pair_to.left),
self.ob_coords(self.pair_to.right),
self.robot_pos,
):
new_pair = self.next_pair(self.pair_to, red_buoys, green_buoys)
if new_pair is not None:
self.pair_to = new_pair
else:
self.get_logger().info("No next buoy pair to go to.")
self.get_logger().debug("No next buoy pair to go to.")
# wait for next spin
return
# TODO: there is no longer a case where it is done wiht the task.
Expand All @@ -358,7 +413,7 @@ def generate_waypoints(self):
buoy_pairs = [self.pair_to]
waypoints = [self.midpoint_pair(self.pair_to)]

self.get_logger().info(f"pair to: {len(buoy_pairs)}")
self.get_logger().debug(f"pair to: {len(buoy_pairs)}")

# form a sequence of buoy pairs (and the respective waypoints) that form a path that the robot can follow
# will terminate if we run out of either green or red buoys
Expand All @@ -369,8 +424,6 @@ def generate_waypoints(self):
break
buoy_pairs.append(next_buoy_pair)
waypoints.append(self.midpoint_pair(next_buoy_pair))
next_buoy_pair = self.next_pair(buoy_pairs[-1], red_buoys, green_buoys)


# convert the sequence to a format appropriate to publishing for the path planner to use
waypoint_arr = WaypointArray(
Expand All @@ -379,19 +432,19 @@ def generate_waypoints(self):
point=self.pair_angle_to_pose(
pair=wpt,
angle=(
math.atan(self.ob_coords(pair[1])[1] - self.ob_coords(pair[0])[1]) /
(self.ob_coords(pair[1])[0] - self.ob_coords(pair[0])[0])
math.atan(self.ob_coords(pair.right)[1] - self.ob_coords(pair.left)[1]) /
(self.ob_coords(pair.right)[0] - self.ob_coords(pair.left)[0])
) + (math.pi / 2),
),
radius=self.norm(self.ob_coords(pair[0]), self.ob_coords(pair[1]))/2 - self.safe_margin,
radius=self.norm(self.ob_coords(pair.left), self.ob_coords(pair.right))/2 - self.safe_margin,
)
for wpt, pair in zip(waypoints, buoy_pairs)
]
)

buoy_pair_arr = BuoyPairArray(
pairs=[
BuoyPair(left=pair[0], right=pair[1], waypoint=waypoint)
BuoyPair(left=pair.left, right=pair.right, waypoint=waypoint)
for pair, waypoint in zip(buoy_pairs, waypoint_arr.waypoints)
]
)
Expand All @@ -403,14 +456,14 @@ def generate_waypoints(self):
self.waypoint_marker_pub.publish(self.buoy_pairs_to_markers(buoy_pair_arr))

# def send_path(self, msg: PointStamped):
# self.get_logger().info(f"buoy pairs: {buoy_pairs}")
# self.get_logger().debug(f"buoy pairs: {buoy_pairs}")

self.get_logger().info(f"waypoints: {waypoints}")
self.get_logger().debug(f"waypoints: {waypoints}")

if waypoints:
waypoint = waypoints[-1]
self.get_logger().info(f"cur_waypoint: {waypoint}, sent_waypoints: {self.sent_waypoints}")
self.get_logger().info(f"len(waypoints): {len(waypoints)}")
waypoint = waypoints[0]
self.get_logger().debug(f"cur_waypoint: {waypoint}, sent_waypoints: {self.sent_waypoints}")
self.get_logger().debug(f"len(waypoints): {len(waypoints)}")

#check if waypoint is close enough (check_dist) to some previous waypoint
passed_waypoint = False
Expand All @@ -420,7 +473,7 @@ def generate_waypoints(self):
passed_waypoint = True

if not passed_waypoint:
# if waypoint not in self.sent_waypoints:
self.get_logger().info(f'sending waypoint {waypoint} to action server')
self.follow_path_client.wait_for_server()
goal_msg = FollowPath.Goal()
goal_msg.planner = self.get_parameter("planner").value
Expand Down