From 8e19207001311167d862b855c1ede54d07cb39c3 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 17 Jul 2025 14:34:45 +0100 Subject: [PATCH] rework of map loading so each node takes from the same source of mapper.py --- .../{mapper => control}/__init__.py | 0 .../{tmpDogSim => control}/dog_control_lib.py | 125 +++++++++--- .../{tmpDogSim => control}/main.py | 0 .../dog_control.py | 43 ++--- .../{mapper => }/mapper.py | 96 +++++----- .../sheep_simulation/simulation.py | 1 - ...ontrol_simulator.py => sheep_simulator.py} | 67 +++++-- .../tmpDogSim/__init__.py | 0 .../utils/geo_converter.py | 2 +- auto_shepherd_simulation_ros2/setup.py | 4 +- .../tmule/connected.tmule.yaml | 13 +- .../tmule/injected.tmule.yaml | 2 +- configs/rviz/default.rviz | 178 ++++++++++++++---- 13 files changed, 376 insertions(+), 155 deletions(-) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{mapper => control}/__init__.py (100%) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{tmpDogSim => control}/dog_control_lib.py (75%) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{tmpDogSim => control}/main.py (100%) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{mapper => }/mapper.py (68%) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{dog_control_simulator.py => sheep_simulator.py} (85%) delete mode 100644 auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/__init__.py diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/__init__.py similarity index 100% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/__init__.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/__init__.py diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/dog_control_lib.py similarity index 75% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/dog_control_lib.py index fe31dc4..8b748c7 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/dog_control_lib.py @@ -1,28 +1,21 @@ import math as maths +import random + +import numpy as np import cv2 import matplotlib.pyplot as plt from matplotlib.path import Path -import numpy as np +from scipy.spatial import ConvexHull, Delaunay from sklearn.cluster import DBSCAN -from auto_shepherd_simulation_ros2.sheep_simulation.simulation import Simulation -from auto_shepherd_simulation_ros2.utils.geo_converter import load_coords_from_yaml, MapConverter -import numpy as np -from scipy.spatial import ConvexHull, Delaunay -from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import Header, ColorRGBA from builtin_interfaces.msg import Duration from geometry_msgs.msg import Point -from std_msgs.msg import Header, ColorRGBA -import random - -try: - mc = MapConverter(load_coords_from_yaml("/home/ros/map/map1.yaml")) -except: - mc = MapConverter(load_coords_from_yaml("../configs/map/map1.yaml")) +from visualization_msgs.msg import Marker, MarkerArray -map_polygon = Path(np.array(mc.map_coords_xy_meters)) -#check if points are valid using `map_polygon.contains_point(point)` +from auto_shepherd_simulation_ros2.sheep_simulation.simulation import Simulation +from auto_shepherd_simulation_ros2.utils.geo_converter import load_coords_from_yaml, MapConverter def circle_around_points(points): diff = points[:, np.newaxis, :] - points[np.newaxis, :, :] @@ -75,7 +68,7 @@ def color_for_label(label): random.seed(label) return ColorRGBA(r=random.random(), g=random.random(), b=random.random(), a=0.3) -def render_dbscan_convex_hulls(pub, db, points, frame_id="map", z=0.0): +def render_dbscan_convex_hulls(pub, db, points, frame_id="field_frame", z=0.0): marker_array = MarkerArray() labels = db.labels_ unique_labels = set(labels) @@ -120,11 +113,61 @@ def render_dbscan_convex_hulls(pub, db, points, frame_id="map", z=0.0): pub.publish(marker_array) + +def render_targets_points(targets_pub, scores, best): + marker_array = MarkerArray() + header = Header(frame_id="field_frame") + + max_score = max( + (abs(s['cost']) for s in scores.values() if s['cost'] != -1), + default=1.0 + ) + + for i, data in scores.items(): + marker = Marker() + marker.header = header + marker.ns = "target_points" + marker.id = i + marker.type = Marker.CYLINDER + marker.action = Marker.ADD + + marker.pose.position.x = data['x'] + marker.pose.position.y = data['y'] + marker.pose.position.z = 0.2 + + # Twice as big and proportional to normalized score (except for -1) + if data['cost'] == -1: + scale = 0.3 # default size for invalid scores + elif data['cost'] == -2: + scale = 0.3 # default size for invalid scores + else: + scale = 2.0 * (0.2 + 0.8 * (abs(data['cost']) / max_score)) + + marker.scale.x = marker.scale.y = scale + marker.scale.z = 0.1 # Cylinder height + + # Color conditions + if data == best: + marker.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.5) # Green + elif data['cost'] == -1: + marker.color = ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.5) # Black + elif data['cost'] == -2: + marker.color = ColorRGBA(r=0.0, g=0.0, b=1.0, a=0.5) # Blue + else: + marker.color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.5) # Yellow + + marker.lifetime.sec = 1 + marker_array.markers.append(marker) + + targets_pub.publish(marker_array) + + def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, dog, goal radius_d=1.4, n_candidates=15, early_exit_threshold=5, - default_goto=np.asarray((0,0)), boundary_pub=None): - """Return optimal dog (x_d*, y_d*) given current flock and goal.""" + default_goto=np.asarray((0,0)), + boundary_pub=None, targets_pub=None): + """Return optimal dog (x_d*, y_d*) given current flock and goal.""" points = np.stack([x, y], axis=1) goal_point = np.array([xc,yc]) @@ -150,25 +193,41 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) if np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xc, yc])) < 5: d = np.linalg.norm(np.asarray([-10, 10]) - np.asarray([xd, yd])) - if d < 3: optimal_xd, optimal_yd = xd, yd + if d < 3: + optimal_xd, optimal_yd = xd, yd else: closest_point = (xd + (-10 - xd) * radius_d / d, yd + (10 - yd) * radius_d / d) points = np.array([closest_point]) optimal_xd, optimal_yd = closest_point + scores = {0: {'x': optimal_xd, 'y': optimal_yd, 'cost': 1}} + best = min(scores.values(), key=lambda s: s['cost']) + optimal_xd, optimal_yd = best['x'], best['y'] + + # elif d > radius_d + radius_sheep: print("Moving towards sheep") closest_point = (xd + (xmean - xd) * radius_d / d, yd + (ymean - yd) * radius_d / d) points = np.array([closest_point]) optimal_xd, optimal_yd = closest_point + scores = {0: {'x': optimal_xd, 'y': optimal_yd, 'cost': 1}} + best = min(scores.values(), key=lambda s: s['cost']) + optimal_xd, optimal_yd = best['x'], best['y'] + + # elif d < abs(radius_d - radius_sheep): print("Moving away from sheep") d = np.linalg.norm(default_goto - np.asarray([xd, yd])) closest_point = (xd + (default_goto[0] - xd) * radius_d / d, yd + (default_goto[1] - yd) * radius_d / d) points = np.array([closest_point]) optimal_xd, optimal_yd = closest_point + scores = {0: {'x': optimal_xd, 'y': optimal_yd, 'cost': 1}} + best = min(scores.values(), key=lambda s: s['cost']) + optimal_xd, optimal_yd = best['x'], best['y'] + + # Otherwise is actively pressuring the closest group else: print("Herding sheep") - # get points around sheep + # get points in an arc around the sheep angle_a = np.arccos((radius_sheep**2 + d**2 - radius_d**2) / (2 * radius_sheep * d)) angle_start = np.arctan2(yd - ymean, xd - xmean) angle_range = (angle_start - angle_a, angle_start + angle_a) @@ -183,15 +242,37 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d # optimise new dog position last_update = 0 optimal_xd, optimal_yd, optimal_cost = xd, yd, cost(x, y, xd, yd, xc, yc, simulation) + scores = dict() for i, (new_xd, new_yd) in enumerate(points): - if not map_polygon.contains_point((new_xd, new_yd)): continue + scores[i] = {'x': new_xd, 'y': new_yd, 'cost': -1} + + # Check if point is within map + map_polygon = Path(np.array(field_boundary)) + if not map_polygon.contains_point((new_xd, new_yd)): + scores[i]['cost'] = -2 + continue + + # Calculate cost of point using simulation new_cost = cost(x, y, new_xd, new_yd, xc, yc, simulation) + scores[i]['cost'] = new_cost + + # If cost is better, reject old cost if new_cost < optimal_cost: optimal_cost = new_cost optimal_xd, optimal_yd = new_xd, new_yd last_update += 1 print(f"Best Cost: {optimal_cost}") - if i-last_update > early_exit_threshold: break + + # Exit once min threshold is completed + #if i-last_update > early_exit_threshold: + # break + + best = min(scores.values(), key=lambda s: s['cost']) + optimal_xd, optimal_yd = best['x'], best['y'] + + # publish the options for points + if targets_pub: + render_targets_points(targets_pub, scores, best) return optimal_xd, optimal_yd def pure_pursuit(dog_xy, target_xy, lookahead=2.0, step=0.5): diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/main.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/main.py similarity index 100% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/main.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/control/main.py diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py index 43da730..2394b5e 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py @@ -9,7 +9,7 @@ from nav_msgs.msg import Path from visualization_msgs.msg import MarkerArray -from auto_shepherd_simulation_ros2.tmpDogSim.dog_control_lib import find_best_dog_position, pure_pursuit +from auto_shepherd_simulation_ros2.control.dog_control_lib import find_best_dog_position, pure_pursuit from auto_shepherd_simulation_ros2.utils.geo_converter import load_coords_from_yaml, MapConverter class DogController(Node): @@ -22,38 +22,19 @@ def __init__(self): self.create_subscription(Path, '/sheep/poses_sim', self._sheep_cb, 10) self.create_subscription(PoseStamped, '/sheep/goal', self._goal_cb, self.get_qos()) self.marker_pub = self.create_publisher(MarkerArray, "/dbscan_hulls", 10) + self.targets_pub = self.create_publisher(MarkerArray, "/dog/options", 10) + self.create_subscription(Path, "/field/fence/path", self._fence_cb, self.get_qos()) # state caches ---------------------------------------------------- self.dog_xy = None # (x, y) self._planned_dog_xy = None # (x, y) of the last planned dog position self.sheep_xy = None # Nx2 array self.goal_xy = None # (x, y) + self.field_boundary = None # control timer ---------------------------------------------------- self.timer = self.create_timer(0.1, self._control_step) - yaml_map_file_path = "/home/ros/map/map1.yaml" - print(f"Attempting to load field coordinates from: {yaml_map_file_path}") - try: - field_coords_latlon = load_coords_from_yaml(yaml_map_file_path) - print(f"Successfully loaded {len(field_coords_latlon)} coordinates from YAML.") - except (FileNotFoundError, ValueError) as e: - print(f"Failed to load coordinates from YAML: {e}") - print("Please ensure the file path is correct and the YAML format matches 'field_boundary: - latitude: X - longitude: Y'.") - print("Exiting example.") - exit(1) # Exit if cannot load map data - - - # Create Map Bounding Box & Convert All Coords - try: - self.map_converter = MapConverter(field_coords_latlon) - map_data = self.map_converter.get_map_data() - self.field_boundary = map_data['map_coords_xy_meters'] - - except ValueError as e: - print(f"Error during map conversion: {e}") - self.map_converter = None # Ensure map_converter is not set if initialization failed - def get_qos(self): qos_profile = QoSProfile( @@ -65,6 +46,9 @@ def get_qos(self): # ------------ message callbacks ------------------------------------- + def _fence_cb(self, msg: Path): + self.field_boundary = [(p.pose.position.x, p.pose.position.y) for p in msg.poses] + def _dog_cb(self, msg: PoseStamped): self.dog_xy = (msg.pose.position.x, msg.pose.position.y) @@ -124,13 +108,14 @@ def _boundary_follow_step(self): # ------------ closed-loop control ----------------------------------- def _control_step(self): - print("_control_step") - # make sure we have the three inputs we need - if self.sheep_xy is None or self.goal_xy is None: + # make sure we have the inputs we need + opt = [self.sheep_xy, self.goal_xy, self.field_boundary] + if any(o is None for o in opt): + return + opt = [self.dog_xy, self._planned_dog_xy] + if all(o is None for o in opt): return - if self.dog_xy is None and self._planned_dog_xy is None: - return # still waiting for the very first dog pose # --------------------------------------------- # choose the starting point for optimisation @@ -144,7 +129,7 @@ def _control_step(self): xs, ys = self.sheep_xy[:, 0], self.sheep_xy[:, 1] xc, yc = self.goal_xy - xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc, self.field_boundary, boundary_pub=self.marker_pub) + xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc, self.field_boundary, boundary_pub=self.marker_pub, targets_pub=self.targets_pub) print(f"Optimised dog position: ({xd_opt:.2f}, {yd_opt:.2f})") ps = PoseStamped() diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper.py similarity index 68% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper.py index c509151..52a4ee3 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper.py @@ -4,6 +4,7 @@ from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy +from std_msgs.msg import Header from nav_msgs.msg import Path from geometry_msgs.msg import PolygonStamped, PoseStamped, Quaternion, Point32, TransformStamped @@ -19,17 +20,19 @@ def __init__(self): #TODO: Fences should not be published from here, they should be published from sim_data_loader.py instead # Configure publishers - self.publisher_path_ = self.create_publisher(Path, 'FieldBoundaryPath', self.get_qos()) - self.publisher_polygon_ = self.create_publisher(PolygonStamped, 'field', self.get_qos()) + self.gps_path_pub = self.create_publisher(Path, '/field/gps_fence/path', self.get_qos()) + self.field_path_pub = self.create_publisher(Path, '/field/fence/path', self.get_qos()) + self.gps_polygon_pub = self.create_publisher(PolygonStamped, '/field/gps_fence/polygon', self.get_qos()) + self.field_polygon_pub = self.create_publisher(PolygonStamped, '/field/fence/polygon', self.get_qos()) self.static_broadcaster = tf2_ros.StaticTransformBroadcaster(self) # Load raw (lat, lon) coordinates map_file_path = '/home/ros/map/map1.yaml' self.get_logger().info(f"Attempting to load map for MapperNode from: {map_file_path}") - raw_latlon_coords = load_coords_from_yaml(map_file_path) + self.raw_latlon_coords = load_coords_from_yaml(map_file_path) # Initialize the MapConverter - self.map_converter = MapConverter(raw_latlon_coords) + self.map_converter = MapConverter(self.raw_latlon_coords) map_data = self.map_converter.get_map_data() # Store map origin (in UTM) @@ -41,13 +44,13 @@ def __init__(self): self.get_logger().info(f"Map converted. Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") # Create messages for the path - self.path_poses = self._create_path_poses(self.relative_xy_meters) - self.polygon_points = self._create_polygon_points(self.relative_xy_meters) + self.path_gps = self.create_path_poses(self.raw_latlon_coords) + self.path_poses = self.create_path_poses(self.relative_xy_meters) + self.polygon_gps = self.create_polygon_points(self.raw_latlon_coords) + self.polygon_points = self.create_polygon_points(self.relative_xy_meters) # Publish the Path, PolygonStamped, and static transform messages - self.publish_path_once() - self.publish_polygon_once() - self.publish_static_transform() + self.publish_msgs() self.get_logger().info("Map published once as Path and Polygon. Static transform published. Node will now spin indefinitely.") def get_qos(self): @@ -58,7 +61,7 @@ def get_qos(self): ) return qos_profile - def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseStamped]: + def create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseStamped]: """ Helper to create a list of PoseStamped messages from relative X, Y meter coordinates. These poses are in the 'field_frame', which will be offset by the map's UTM origin. @@ -90,15 +93,15 @@ def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseS poses.append(closed_loop_pose) return poses - def _create_polygon_points(self, xy_meters: List[Tuple[float, float]]) -> List[Point32]: + def create_polygon_points(self, xy_meters: List[Tuple[float, float]]) -> List[Point32]: """ Helper to create a list of Point32 messages for PolygonStamped. """ points = [] for x_m, y_m in xy_meters: p32 = Point32() - p32.x = x_m - p32.y = y_m + p32.x = y_m + p32.y = x_m p32.z = 0.0 # Assuming 2D polygon points.append(p32) @@ -112,38 +115,41 @@ def _create_polygon_points(self, xy_meters: List[Tuple[float, float]]) -> List[P points.append(first_point) return points - def publish_path_once(self): - """Publishes the Path message containing the field boundary.""" - if not self.path_poses: - self.get_logger().warn("No poses to publish for Path. Skipping publication.") - return - - path_msg = Path() - path_msg.header.stamp = self.get_clock().now().to_msg() - path_msg.header.frame_id = 'map' # The Path itself is defined in the 'map' frame (absolute) - - path_msg.poses = self.path_poses - - self.publisher_path_.publish(path_msg) - self.get_logger().info(f'Published FieldBoundaryPath with {len(path_msg.poses)} poses (once, latched).') - - def publish_polygon_once(self): - """Publishes the PolygonStamped message containing the field boundary.""" - if not self.polygon_points: - self.get_logger().warn("No points to publish for PolygonStamped. Skipping publication.") - return - - polygon_msg = PolygonStamped() - polygon_msg.header.stamp = self.get_clock().now().to_msg() - # The PolygonStamped is in 'field_frame' because its points are relative to that origin - polygon_msg.header.frame_id = 'field_frame' - polygon_msg.polygon.points = self.polygon_points - - self.publisher_polygon_.publish(polygon_msg) - self.get_logger().info(f'Published PolygonStamped with {len(polygon_msg.polygon.points)} points (once, latched).') - - def publish_static_transform(self): - """Publishes the static transform from 'map' to 'field_frame'.""" + def header(self, frame): + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = frame + return header + + def publish_msgs(self): + + # Pubilsh Path (metric) + msg = Path() + msg.header = self.header(frame='map') + msg.poses = self.path_poses + self.field_path_pub.publish(msg) #BROWN + + # Pubilsh Path (gps) + msg = Path() + msg.header = self.header(frame='world') + msg.poses = self.path_gps + self.gps_path_pub.publish(msg) #GREEN + + # Pubilsh Polygon (metric) + msg = PolygonStamped() + msg.header = self.header(frame='map') + msg.polygon.points = self.polygon_points + self.field_polygon_pub.publish(msg) #ORANGE + + # Publish Polygon (gps) + msg = PolygonStamped() + msg.header = self.header(frame='world') + msg.polygon.points = self.polygon_gps + self.gps_polygon_pub.publish(msg) #BLUE + + self.get_logger().info(f'Published Field Boundary with {len(self.path_poses)}.') + + # Publish static transform t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'map' diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py index a086a73..bdab3fc 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py @@ -303,7 +303,6 @@ def __init__(self, width, height): self.LIGHTGREEN = (40,160,40) # Load map configuration - # map_file = os.path.join(os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(__file__)))), 'configs', 'map', 'map1.yaml') map_file = '/home/ros/map/map1.yaml' field_boundary = load_map_config(map_file) diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulator.py similarity index 85% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulator.py index 3cb3ac8..4d35da7 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulator.py @@ -14,7 +14,7 @@ from auto_shepherd_simulation_ros2.sheep_simulation.simulation import Simulation from auto_shepherd_simulation_ros2.utils.geo_converter import MapConverter, load_coords_from_yaml -class DogControlSimulator(Node): +class SheepSimulator(Node): def __init__(self): super().__init__('dog_control_simulator') @@ -25,6 +25,10 @@ def __init__(self): self.dog_state = None self.dog_command = None # Store the latest dog command self.simulation = None # build later when we get data + self.field_boundary = None # build later when we get data + self.map_converter = None # build later when we get data + self.simulation = None + self.num_sheep = None # Create QoS profile qos_profile = QoSProfile( @@ -48,9 +52,23 @@ def __init__(self): self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) # Load the field boundaries - yaml_map_file_path = "/home/ros/map/map1.yaml" - self._load_map(yaml_map_file_path) - #TODO: self.create_subscription(String, '/field/boundaries', self._load_map, qos_profile) + self.create_subscription(Path, "/field/gps_fence/path", self._gps_fence_cb, self.get_qos()) + self.create_subscription(Path, "/field/fence/path", self._fence_cb, self.get_qos()) + + # Start the simulator + self.start_simulation() + + def start_simulation(self): + print('Sim init attempt') + + # Exit if data not ready + if not self.field_boundary: return + if not self.map_converter: return + + # Exit if already started + if self.simulation: return + + print('Sim init') # Start Simulation self.simulation = Simulation(self.field_boundary, 800, 600, sheep_states=None, sheepdog_state=None, spawn_random=False) @@ -58,6 +76,13 @@ def __init__(self): self.sim_step_timer = self.create_timer(self.dt, self.run_sim_step, callback_group=RCG()) print('Dog Control Simulator Initialised') + # Initialise the sheep + if self.num_sheep: + self.simulation.num_sheep = self.num_sheep + self.simulation.sheep_list = [] + self.simulation._initialize_sheep(None, spawn_random=True) + self.get_logger().info(f"Initialised {self.num_sheep} sheep.") + def get_qos(self): qos_profile = QoSProfile( @@ -67,16 +92,21 @@ def get_qos(self): ) return qos_profile - def _load_map(self, yaml_map_file_path): - print(f"Attempting to load field coordinates from: {yaml_map_file_path}") - field_coords_latlon = load_coords_from_yaml(yaml_map_file_path) - - # Create Map Bounding Box & Convert All Coords + def _gps_fence_cb(self, msg): + print('GPS Fence cb') + field_coords_latlon = [(p.pose.position.x, p.pose.position.y) for p in msg.poses] self.map_converter = MapConverter(field_coords_latlon) - map_data = self.map_converter.get_map_data() - self.field_boundary = map_data['map_coords_xy_meters'] + # Start the simulator + self.start_simulation() + def _fence_cb(self, msg): + print('Fence cb') + field_coords = [(p.pose.position.y, p.pose.position.x) for p in msg.poses] + self.field_boundary = field_coords + + # Start the simulator + self.start_simulation() def _dog_command_cb(self, msg): """Callback for dog command messages""" @@ -98,6 +128,13 @@ def _dog_cb(self, msg): self.get_logger().info(f"Callback detected for dog now at x:{msg.pose.position.x}, y:{msg.pose.position.y}.") def _sheep_randomise_cb(self, msg): + print('Sheep init cb') + + # If map data not available yet, skip + if not self.simulation: + self.num_sheep = msg.data + return + self.simulation.num_sheep = msg.data self.simulation.sheep_list = [] self.simulation._initialize_sheep(None, spawn_random=True) @@ -105,14 +142,18 @@ def _sheep_randomise_cb(self, msg): def _sheep_cb(self, msg): + # Skip frames if not needed self.frame += 1 #if not str(self.frame).endswith('0'): return + # If map data not available yet, skip + if not self.map_converter: return + # Get current timestep print('_______________') self.sheep_poses = {} for sheep in msg.poses: - + # Create sheep creator name = sheep.header.frame_id if name not in self.sheep_poses: @@ -287,7 +328,7 @@ def run_sim_step(self, timestep=None): def main(): rclpy.init() - node = DogControlSimulator() + node = SheepSimulator() rclpy.spin(node) rclpy.shutdown() diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py index 6323567..157f9bb 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py @@ -93,7 +93,7 @@ def load_coords_from_yaml(yaml_file_path: str) -> List[Tuple[float, float]]: if 'latitude' in item and 'longitude' in item: coords.append((float(item['latitude']), float(item['longitude']))) return coords - + if __name__ == "__main__": # Example for the path you previously mentioned: diff --git a/auto_shepherd_simulation_ros2/setup.py b/auto_shepherd_simulation_ros2/setup.py index 7b6f10f..1245951 100644 --- a/auto_shepherd_simulation_ros2/setup.py +++ b/auto_shepherd_simulation_ros2/setup.py @@ -25,8 +25,8 @@ f'sim_data_loader.py = {pkg}.sim_data_loader:main', f'boid_training_simulator.py = {pkg}.boid_training_simulator:main', f'dog_control.py = {pkg}.dog_control:main', - f'dog_control_simulator.py = {pkg}.dog_control_simulator:main', - f'mapper.py = {pkg}.mapper.mapper:main' + f'sheep_simulator.py = {pkg}.sheep_simulator:main', + f'mapper.py = {pkg}.mapper:main' ], }, ) diff --git a/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml index a5d31b2..c0de807 100644 --- a/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml @@ -18,13 +18,16 @@ init_cmd: | windows: - - name: system - panes: - - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py - - ros2 run auto_shepherd_simulation_ros2 dog_control.py - - name: outputs panes: - rviz2 + - name: system + panes: + - ros2 run auto_shepherd_simulation_ros2 sheep_simulator.py + - ros2 run auto_shepherd_simulation_ros2 dog_control.py + + - name: inputs + panes: #TODO: Remove this once we have drone injected working + - ros2 run auto_shepherd_simulation_ros2 mapper.py diff --git a/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml index c940c43..928ebc6 100644 --- a/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml @@ -24,7 +24,7 @@ windows: - name: system panes: - - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py + - ros2 run auto_shepherd_simulation_ros2 sheep_simulator.py - ros2 run auto_shepherd_simulation_ros2 dog_control.py - name: inputs diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 66e4a8b..e13a2ed 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -4,16 +4,16 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Grid1 - - /Sheep Data1 - - /Dog Data1 - - /Field Data1 + - /TF1/Frames1 + - /TF1/Tree1 + - /TF1/Tree1/map1 Splitter Ratio: 0.5 - Tree Height: 443 + Tree Height: 591 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties - Expanded: ~ + Expanded: + - /Interact1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -96,7 +96,7 @@ Visualization Manager: Enabled: true Name: Sheep Clusters Namespaces: - dbscan_hulls: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -128,6 +128,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /dog/pose Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Dog Cmd Options + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dog/options + Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 @@ -166,6 +178,60 @@ Visualization Manager: {} Update Interval: 0 Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Goal Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/goal_marker + Value: true + Enabled: true + Name: Field Data + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/labelled_img + Value: true + Enabled: true + Name: ShLAMb Input + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + field_frame: + Value: true + map: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + field_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz_common/Group + Displays: - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -176,7 +242,7 @@ Visualization Manager: Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 - Name: Fences + Name: Field Path Offset: X: 0 Y: 0 @@ -192,26 +258,64 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /FieldBoundaryPath + Value: /field/fence/path Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Goal Marker - Namespaces: - target: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 1 + Name: GPS Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sheep/goal_marker - Value: true - Enabled: true - Name: Field Data - - Class: rviz_common/Group - Displays: + Value: /field/gps_fence/path + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 255; 255; 0 + Enabled: false + Name: Field Polygon + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /field/fence/polygon + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 0; 255; 255 + Enabled: false + Name: GPS Polygon + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /field/gps_fence/polygon + Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -222,10 +326,10 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sheep/labelled_img - Value: true - Enabled: false - Name: ShLAMb Input + Value: /drone_feed + Value: false + Enabled: true + Name: Drone Input Enabled: true Global Options: Background Color: 48; 80; 48 @@ -250,6 +354,8 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: initialpose + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true Transformation: Current: Class: rviz_default_plugins/TF @@ -266,21 +372,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 6.984107971191406 + Scale: 4.975740432739258 Target Frame: field_frame Value: TopDownOrtho (rviz_default_plugins) - X: 0.5686241984367371 - Y: -0.5041577816009521 + X: 0.13739904761314392 + Y: 0.2776811718940735 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1043 - Hide Left Dock: true + collapsed: false + Height: 1016 + Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000705fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000705000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000219000001890000002800ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000024c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a60000003efc0100000002fb0000000800540069006d00650000000000000003a60000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000003a2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000028a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cb000001120000002800fffffffb0000000a0049006d0061006700650000000312000000cb0000002800ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000024c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a60000003efc0100000002fb0000000800540069006d00650000000000000003a60000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000031d000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -289,6 +395,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 - Y: 285 + Width: 1145 + X: 775 + Y: 364