From 554375068082ca45aa2d3a09aa6445b0d2d2ff7d Mon Sep 17 00:00:00 2001 From: bethanmoncur <92383314+bethanmoncur@users.noreply.github.com> Date: Wed, 18 Jun 2025 13:52:47 +0100 Subject: [PATCH 01/59] changing sheep behaviour --- .../sheep_simulation/sheep.py | 45 ++++++++++++------- .../sheep_simulation/sheepdog.py | 2 +- .../sheep_simulation/simulation.py | 8 ++-- 3 files changed, 34 insertions(+), 21 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py index d859ea4..cf85be4 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py @@ -25,31 +25,31 @@ def __init__(self, position, velocity=None, width=None, height=None): # Movement state self.current_state = self.GRAZING # Start in grazing state - self.walking_threshold = 1.0 # Threshold to start walking + self.walking_threshold = 1.0 # Threshold to start walking was 1.0 self.moving_threshold = 2.0 # Threshold to start moving quickly - # Speed multipliers for different states - self.grazing_speed_multiplier = 0.05 # Slower when grazing - self.walking_speed_multiplier = 0.15 # Moderate speed when walking - self.moving_speed_multiplier = 1.2 # Faster when moving quickly + # Speed multipliers for different states TODO CHANGE THESE + self.grazing_speed_multiplier = 0.1 # Slower when grazing # original 0.05 + self.walking_speed_multiplier = 0.15 # Moderate speed when walking # original 0.15 + self.moving_speed_multiplier = 1.2 # Faster when moving quickly # original 1.2 - # Flocking parameters - self.alignment_weight = 1.0 - self.cohesion_weight = 0.23 + # Flocking parameters (changed these defaults to match slider defaults in simulation.py) + self.alignment_weight = 3.0 + self.cohesion_weight = 5.0 self.separation_weight = 6.0 # Physical properties - self.size = 10 + self.size = 6 # was 10 self.color = (255, 255, 255) # White self.max_speed = 4 - self.max_force = 0.2 + self.max_force = 0.2 # Perception parameters - self.perception_radius = 50 # How far the sheep can see other sheep + self.perception_radius = 100 # How far the sheep can see other sheep (was 50, needs same as dog radius) self.protected_radius = 20 # Minimum distance sheep try to maintain from each other self.boundary_margin = 40 # Distance from edge where sheep start to turn - self.dog_repulsion_weight = 8.0 # Weight for avoiding the dog - self.dog_repulsion_radius = 100 # How far the sheep can sense the dog + self.dog_repulsion_weight = 6.0 # Weight for avoiding the dog (was 8.0) + self.dog_repulsion_radius = 100 # How far the sheep can sense the dog (should be same as perception radius) def get_state(self): """Return current state in the standard format""" @@ -70,8 +70,20 @@ def update_state(self): self.current_state = self.WALKING else: self.current_state = self.GRAZING - # Reset velocity and acceleration when grazing - self.velocity = [0, 0] + # Reset velocity and acceleration when grazing to be still with some random movement + rand_move_x = random.random() + if rand_move_x < 0.99: + rand_move_x = 0 + else: + rand_move_x = random.uniform(-4, 4) + + rand_move_y = random.random() + if rand_move_y < 0.99: + rand_move_y = 0 + else: + rand_move_y = random.uniform(-4, 4) + + self.velocity = [rand_move_x, rand_move_y] self.acceleration = [0, 0] def apply_force(self, force): @@ -282,7 +294,8 @@ def avoid_dog(self, dog): # Normalize the direction if distance > 0: to_dog[0] = to_dog[0] / distance - to_dog[1] = to_dog[1] / distance + to_dog[1] = to_dog[1] / distance + # Apply repulsion force return [ to_dog[0] * strength * self.dog_repulsion_weight, diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py index bf3d616..6d1e8c4 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py @@ -14,7 +14,7 @@ def __init__(self, position, velocity=None, yaw=0): self.velocity = velocity # Physical properties - self.size = 20 # Size of the pig + self.size = 10 # Size of the pig self.color = (255, 192, 203) # Pink color for Babe # Screen boundaries diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 0774de7..d7d902e 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -167,11 +167,11 @@ def __init__(self, width, height): # Create controller for keyboard input self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) - # Create sliders + # Create sliders Slider(x, y, width, height, min_val, max_val, initial_val, name) self.sliders = { - 'alignment': Slider(50, height - 100, 200, 10, 0, 2, 1.0, "Alignment"), - 'cohesion': Slider(50, height - 70, 200, 10, 0, 2, 0.23, "Cohesion"), - 'separation': Slider(50, height - 40, 200, 10, 0, 10, 6, "Separation") + 'alignment': Slider(50, height - 100, 200, 10, 0, 10, 3.0, "Alignment"), # change from 1.0 to 2.0 + 'cohesion': Slider(50, height - 70, 200, 10, 0, 10, 5.0, "Cohesion"), # change from 0.23 to 5.0 + 'separation': Slider(50, height - 40, 200, 10, 0, 10, 6.0, "Separation") # keep at 6.0 } # Instructions From 625b83027623667ce9a9f01405c341500f192a2b Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Wed, 18 Jun 2025 14:09:37 +0100 Subject: [PATCH 02/59] config updates and adding maps (#13) --- configs/map/map1.yaml | 48 +++++ {rviz_config => configs/rviz}/default.rviz | 36 +++- .../rviz}/persistent_settings | 2 +- {rviz_config => configs/rviz}/test.rviz | 0 docker/Dockerfile | 11 +- docker/docker-compose.yml | 4 +- .../mapper/__init__.py | 0 .../auto_shepherd_simulation/mapper/mapper.py | 186 ++++++++++++++++++ ros2_package/setup.py | 3 +- 9 files changed, 278 insertions(+), 12 deletions(-) create mode 100644 configs/map/map1.yaml rename {rviz_config => configs/rviz}/default.rviz (83%) rename {rviz_config => configs/rviz}/persistent_settings (74%) rename {rviz_config => configs/rviz}/test.rviz (100%) create mode 100644 ros2_package/auto_shepherd_simulation/mapper/__init__.py create mode 100644 ros2_package/auto_shepherd_simulation/mapper/mapper.py diff --git a/configs/map/map1.yaml b/configs/map/map1.yaml new file mode 100644 index 0000000..7d9ac42 --- /dev/null +++ b/configs/map/map1.yaml @@ -0,0 +1,48 @@ +# /home/ros/map/map1.yaml +field_boundary: + - latitude: 53.26515618426661 + longitude: -0.5335800200993889 + - latitude: 53.265173581210504 + longitude: -0.5328587756531677 + - latitude: 53.26508213815604 + longitude: -0.5320326553115605 + - latitude: 53.264804599266995 + longitude: -0.5318904982397906 + - latitude: 53.26474106772158 + longitude: -0.5322397552723048 + - latitude: 53.26467626481607 + longitude: -0.5326271690121636 + - latitude: 53.264566296024434 + longitude: -0.5328143095475191 + - latitude: 53.264503456587924 + longitude: -0.532870123391397 + - latitude: 53.26433261140319 + longitude: -0.5328931055624057 + - latitude: 53.26420300425598 + longitude: -0.5328898223951187 + - latitude: 53.26415194678711 + longitude: -0.532870123391397 + - latitude: 53.26408321548277 + longitude: -0.5328307253839537 + - latitude: 53.26405965043868 + longitude: -0.5328832560605448 + - latitude: 53.26410285300956 + longitude: -0.53298503424644 + - latitude: 53.264246206682024 + longitude: -0.5331327767743521 + - latitude: 53.26437974117752 + longitude: -0.5332772361349775 + - latitude: 53.26450934778904 + longitude: -0.5333855806554465 + - latitude: 53.264601643166884 + longitude: -0.5334545271684723 + - latitude: 53.26472732165892 + longitude: -0.533516907346924 + - latitude: 53.26476659611194 + longitude: -0.5335431726852196 + - latitude: 53.26484514490975 + longitude: -0.5335497390197935 + - latitude: 53.26495118555794 + longitude: -0.5335792875253759 + - latitude: 53.26515618426661 + longitude: -0.5335800200993889 diff --git a/rviz_config/default.rviz b/configs/rviz/default.rviz similarity index 83% rename from rviz_config/default.rviz rename to configs/rviz/default.rviz index 5ce4c04..b2ae400 100644 --- a/rviz_config/default.rviz +++ b/configs/rviz/default.rviz @@ -6,6 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Path1 + - /Path1/Topic1 Splitter Ratio: 0.5 Tree Height: 555 - Class: rviz_common/Selection @@ -47,6 +49,34 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: 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: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /FieldBoundaryPath + Value: true Enabled: true Global Options: Background Color: 20; 48; 48 @@ -93,7 +123,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 477.786376953125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -108,10 +138,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 1.2203975915908813 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 6.2435832023620605 Saved: ~ Window Geometry: Displays: diff --git a/rviz_config/persistent_settings b/configs/rviz/persistent_settings similarity index 74% rename from rviz_config/persistent_settings rename to configs/rviz/persistent_settings index 0fb6b3d..967864e 100644 --- a/rviz_config/persistent_settings +++ b/configs/rviz/persistent_settings @@ -1,4 +1,4 @@ -Last Config Dir: /home/ros/.rviz2 +Last Config Dir: default.rviz Last Image Dir: "" Recent Configs: - /home/ros/.rviz2/default.rviz diff --git a/rviz_config/test.rviz b/configs/rviz/test.rviz similarity index 100% rename from rviz_config/test.rviz rename to configs/rviz/test.rviz diff --git a/docker/Dockerfile b/docker/Dockerfile index 01d2062..e33067d 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -94,13 +94,14 @@ RUN python3 -m pip install --no-cache-dir --upgrade \ setuptools==58.2.0 \ packaging==24.2 \ tmule==1.5.9 \ - pyserial\ + pyserial \ + pyproj \ pygame -# ────────────────────────────── -# 7. Drop privileges -# ────────────────────────────── -USER ${USERNAME} + # ────────────────────────────── + # 7. Drop privileges + # ────────────────────────────── + USER ${USERNAME} WORKDIR /home/${USERNAME} # ────────────────────────────── diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index f4a354a..927c397 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -22,8 +22,8 @@ services: # enable mount for gui applications - /tmp/.X11-unix:/tmp/.X11-unix:rw - - - ../rviz_config:/home/ros/.rviz2:rw + - ../configs/rviz:/home/ros/.rviz2:rw + - ../configs/map:/home/ros/map:rw environment: - BASE_WS=/home/ros/base_ws diff --git a/ros2_package/auto_shepherd_simulation/mapper/__init__.py b/ros2_package/auto_shepherd_simulation/mapper/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_package/auto_shepherd_simulation/mapper/mapper.py b/ros2_package/auto_shepherd_simulation/mapper/mapper.py new file mode 100644 index 0000000..e5ad371 --- /dev/null +++ b/ros2_package/auto_shepherd_simulation/mapper/mapper.py @@ -0,0 +1,186 @@ +import rclpy +from rclpy.node import Node +# from geometry_msgs.msg import PolygonStamped, Point32 # No longer used for main path +from nav_msgs.msg import Path # New: Import Path message +from geometry_msgs.msg import PoseStamped, Point, Quaternion # PoseStamped, Point (for position), Quaternion (for orientation) +import yaml +import os +import tf2_ros +from geometry_msgs.msg import TransformStamped +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy + +# For UTM conversion +from pyproj import CRS, Transformer +from tf_transformations import quaternion_from_euler # Helper for creating quaternions + +class MapperNode(Node): + def __init__(self): + super().__init__('mapper_node') + + self.declare_parameter('map_file_path', '/home/ros/map/map1.yaml') + + # Define the QoS profile for a latched topic + qos_profile = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL + ) + + # Change publisher type to Path + self.publisher_ = self.create_publisher( + Path, + 'FieldBoundaryPath', # New topic name for clarity + qos_profile + ) + + self.static_broadcaster = tf2_ros.StaticTransformBroadcaster(self) + + # UTM Conversion setup (for Lincoln, UK - UTM Zone 30N) + wgs84_crs = CRS("EPSG:4326") # Latitude, Longitude + utm30n_crs = CRS("EPSG:32630") # WGS 84 / UTM Zone 30N (meters) + self.transformer = Transformer.from_crs(wgs84_crs, utm30n_crs) + + self.polygon_data = self.load_map_from_yaml() + if not self.polygon_data: + self.get_logger().error("Failed to load map data from specified path. Shutting down.") + rclpy.shutdown() + return + + # Perform UTM conversion on loaded data + self.utm_poses = self.convert_to_utm_poses() + if not self.utm_poses: + self.get_logger().error("Failed to convert polygon data to UTM poses. Shutting down.") + rclpy.shutdown() + return + + # Publish the Path message immediately after loading and converting + self.publish_path_once() + + # Publish the static transform after the map data is ready + self.publish_static_transform() + + self.get_logger().info("Map published once as Path. Static transform published. Node will now spin indefinitely.") + + def load_map_from_yaml(self): + map_file_path = self.get_parameter('map_file_path').get_parameter_value().string_value + self.get_logger().info(f"Attempting to load map from: {map_file_path}") + + if not os.path.exists(map_file_path): + self.get_logger().error(f"Map file not found at: {map_file_path}") + return None + + try: + with open(map_file_path, 'r') as file: + data = yaml.safe_load(file) + if 'field_boundary' in data and isinstance(data['field_boundary'], list): + return data.get('field_boundary') + else: + self.get_logger().error("YAML file does not contain a 'field_boundary' list under the 'field_boundary' key.") + return None + except yaml.YAMLError as e: + self.get_logger().error(f"Error parsing YAML file: {e}") + return None + except Exception as e: + self.get_logger().error(f"An unexpected error occurred while loading map: {e}") + return None + + def convert_to_utm_poses(self): + if not self.polygon_data: + return [] + + converted_poses = [] + # Calculate a local origin for the 'field_frame' based on the first point's UTM coordinates + # This makes the path appear around (0,0) in RViz's 'field_frame' + # while 'field_frame' itself is transformed to its true UTM location in 'map' frame. + first_lat = float(self.polygon_data[0]['latitude']) + first_lon = float(self.polygon_data[0]['longitude']) + self.origin_utm_x, self.origin_utm_y = self.transformer.transform(first_lat, first_lon) + self.get_logger().info(f"UTM Origin for 'field_frame' set to: ({self.origin_utm_x}, {self.origin_utm_y})") + + + # Create an identity quaternion for the poses (no rotation) + # tf_transformations.quaternion_from_euler(roll, pitch, yaw) + # (0,0,0) means no rotation. + q = quaternion_from_euler(0, 0, 0) + identity_orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + + + for point_data in self.polygon_data: + lat = float(point_data['latitude']) + lon = float(point_data['longitude']) + + utm_x, utm_y = self.transformer.transform(lat, lon) + + pose_stamped = PoseStamped() + pose_stamped.header.stamp = self.get_clock().now().to_msg() + pose_stamped.header.frame_id = 'field_frame' # Poses are relative to 'field_frame' + + # Position in meters, relative to the chosen origin of 'field_frame' + pose_stamped.pose.position.x = utm_x - self.origin_utm_x + pose_stamped.pose.position.y = utm_y - self.origin_utm_y + pose_stamped.pose.position.z = 0.0 # Assuming 2D path + + pose_stamped.pose.orientation = identity_orientation # No rotation + + converted_poses.append(pose_stamped) + + # To close the loop for the path visualization in RViz + if converted_poses: + first_pose = converted_poses[0] + # Create a new PoseStamped with updated stamp if needed, or just append the first one + # Appending the first one directly is common for closing a loop. + closed_loop_pose = PoseStamped() + closed_loop_pose.header.stamp = self.get_clock().now().to_msg() + closed_loop_pose.header.frame_id = 'field_frame' + closed_loop_pose.pose = first_pose.pose + converted_poses.append(closed_loop_pose) + + + return converted_poses + + + def publish_path_once(self): + if not self.utm_poses: + self.get_logger().warn("No UTM poses to publish for Path.") + return + + path_msg = Path() + path_msg.header.stamp = self.get_clock().now().to_msg() + path_msg.header.frame_id = 'map' # The Path itself is in the 'map' frame + + path_msg.poses = self.utm_poses # Assign the list of PoseStamped messages + + self.publisher_.publish(path_msg) + self.get_logger().info(f'Published FieldBoundaryPath with {len(path_msg.poses)} poses (once, latched).') + + + def publish_static_transform(self): + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = 'field_frame' + + # This transform places 'field_frame' at the UTM coordinates of your first map point + t.transform.translation.x = self.origin_utm_x + t.transform.translation.y = self.origin_utm_y + t.transform.translation.z = 0.0 + + q = quaternion_from_euler(0, 0, 0) # Identity rotation for the frame itself + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + self.static_broadcaster.sendTransform(t) + self.get_logger().info(f"Published static transform from '{t.header.frame_id}' to '{t.child_frame_id}' at UTM origin.") + + +def main(args=None): + rclpy.init(args=args) + mapper_node = MapperNode() + rclpy.spin(mapper_node) + mapper_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ros2_package/setup.py b/ros2_package/setup.py index 767e2a9..fffd6c2 100644 --- a/ros2_package/setup.py +++ b/ros2_package/setup.py @@ -24,7 +24,8 @@ 'console_scripts': [ f'ros_interface.py = {pkg}.ros_interface:main', f'boid_training_simulator.py = {pkg}.boid_training_simulator:main', - f'dog_control_simulator.py = {pkg}.dog_control_simulator:main' + f'dog_control_simulator.py = {pkg}.dog_control_simulator:main', + f'mapper.py = {pkg}.mapper.mapper:main' ], }, ) From e4c32ef7e54b21aead935de5185527e667faad28 Mon Sep 17 00:00:00 2001 From: James Date: Wed, 18 Jun 2025 14:40:30 +0100 Subject: [PATCH 03/59] New rviz settings --- configs/rviz/default.rviz | 32 ++++++++++++++++++++++++-------- configs/rviz/persistent_settings | 2 +- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index b2ae400..ed06ee8 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Grid1 - /Path1 - /Path1/Topic1 Splitter Ratio: 0.5 @@ -32,7 +33,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 1 + Cell Size: 10 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true @@ -46,7 +47,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 100 Reference Frame: Value: true - Alpha: 1 @@ -77,6 +78,21 @@ Visualization Manager: Reliability Policy: Reliable Value: /FieldBoundaryPath Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + dog_velocity: true + sheep: true + sheep_velocity: true + sheepdog: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /simulation_markers + Value: true Enabled: true Global Options: Background Color: 20; 48; 48 @@ -123,16 +139,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 477.786376953125 + Distance: 1475.8636474609375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 332.9386291503906 + Y: 305.6679382324219 + Z: -117.16397857666016 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -159,5 +175,5 @@ Window Geometry: Views: collapsed: false Width: 1200 - X: 454 - Y: 112 + X: 250 + Y: 73 diff --git a/configs/rviz/persistent_settings b/configs/rviz/persistent_settings index 967864e..0fb6b3d 100644 --- a/configs/rviz/persistent_settings +++ b/configs/rviz/persistent_settings @@ -1,4 +1,4 @@ -Last Config Dir: default.rviz +Last Config Dir: /home/ros/.rviz2 Last Image Dir: "" Recent Configs: - /home/ros/.rviz2/default.rviz From ca6dfbc8778d331499b15f2c5a6b89d59d0ac8f8 Mon Sep 17 00:00:00 2001 From: James Date: Wed, 18 Jun 2025 14:40:44 +0100 Subject: [PATCH 04/59] Updated ROS sheepdog teleop --- .../sheepdog_teleop.py | 105 +++++++----------- 1 file changed, 39 insertions(+), 66 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/sheepdog_teleop.py b/ros2_package/auto_shepherd_simulation/sheepdog_teleop.py index 8c839fa..39f24c0 100644 --- a/ros2_package/auto_shepherd_simulation/sheepdog_teleop.py +++ b/ros2_package/auto_shepherd_simulation/sheepdog_teleop.py @@ -31,23 +31,19 @@ def __init__(self): # Initialize dog state self.position = [400.0, 300.0] # Center of the simulation area self.yaw = 0.0 # Current yaw angle in radians - self.velocity = 0.0 # Current forward velocity - self.angular_velocity = 0.0 # Current angular velocity # Control parameters - self.max_velocity = 30.0 # Increased max speed - self.acceleration = 2.0 # Increased for more responsive acceleration - self.deceleration = 1.0 # Increased for faster stopping - self.natural_deceleration = 0.5 # Increased for faster natural slowdown - self.rotation_speed = 1.0 # Increased for faster turning + self.max_speed = 5 + self.min_speed = 0 + self.current_speed = 0 + self.acceleration = 0.2 + self.deceleration = 0.3 + self.rotation_speed = 0.1 # Radians per frame self.dt = 0.1 # Time step for updates # Thread control self.running = True self.state_lock = threading.Lock() - self.forward_pressed = False # Track if forward key is pressed - self.left_pressed = False # Track if left key is pressed - self.right_pressed = False # Track if right key is pressed # Get terminal settings self.old_settings = termios.tcgetattr(sys.stdin) @@ -55,12 +51,9 @@ def __init__(self): # Create reentrant callback group for timer self.callback_group = ReentrantCallbackGroup() - # Create timer for state updates + # Create timers for state updates and keyboard input self.create_timer(self.dt, self.update_state, callback_group=self.callback_group) - - # Start keyboard input thread - self.keyboard_thread = threading.Thread(target=self.keyboard_loop) - self.keyboard_thread.start() + self.create_timer(0.01, self.check_keyboard, callback_group=self.callback_group) # Print instructions self.print_instructions() @@ -84,64 +77,49 @@ def get_key(self): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) return key - def keyboard_loop(self): - """Thread function for handling keyboard input""" - while self.running: - key = self.get_key() - if key == 'q': - print("\nQuitting teleoperation...") - self.running = False - rclpy.shutdown() - break - - with self.state_lock: - self.handle_key(key) - - # Small sleep to prevent CPU hogging - time.sleep(0.01) + def check_keyboard(self): + """Check for keyboard input in a non-blocking way""" + key = self.get_key() + if key == 'q': + print("\nQuitting teleoperation...") + self.running = False + rclpy.shutdown() + else: + self.handle_key(key) def update_state(self): """Update dog state based on current velocities""" with self.state_lock: - # Apply natural deceleration when no forward key is pressed - if not self.forward_pressed and self.velocity > 0: - self.velocity = max(0.0, self.velocity - self.natural_deceleration * self.dt) - - # Update position based on velocity and yaw - self.position[0] += self.velocity * math.cos(self.yaw) * self.dt - self.position[1] += self.velocity * math.sin(self.yaw) * self.dt + # Natural deceleration when no keys are pressed + self.current_speed = max(self.min_speed, self.current_speed - self.deceleration * 0.5) - # Update yaw based on angular velocity - self.yaw += self.angular_velocity * self.dt + # Calculate movement based on current speed and direction + dx = math.cos(self.yaw) * self.current_speed + dy = -math.sin(self.yaw) * self.current_speed - # Normalize yaw to [-pi, pi] - self.yaw = math.atan2(math.sin(self.yaw), math.cos(self.yaw)) - - # Reset angular velocity if no turning keys are pressed - if not (self.left_pressed or self.right_pressed): - self.angular_velocity = 0.0 + # Update position + self.position[0] += dx + self.position[1] += dy # Publish current state self.publish_command() def handle_key(self, key): """Handle keypress to update velocities""" - if key == 'w' or key == '\x1b[A': # W or Up Arrow - self.forward_pressed = True - self.velocity = min(self.velocity + self.acceleration * self.dt, self.max_velocity) - elif key == 's' or key == '\x1b[B': # S or Down Arrow - self.forward_pressed = False - self.velocity = max(self.velocity - self.deceleration * self.dt, 0.0) - elif key == 'a' or key == '\x1b[D': # A or Left Arrow - self.left_pressed = True - self.angular_velocity = self.rotation_speed - elif key == 'd' or key == '\x1b[C': # D or Right Arrow - self.right_pressed = True - self.angular_velocity = -self.rotation_speed - elif key == '': # No key pressed - self.forward_pressed = False - self.left_pressed = False - self.right_pressed = False + with self.state_lock: + # Handle speed control + if key == 'w' or key == '\x1b[A': # W or Up Arrow + # Increase speed + self.current_speed = min(self.max_speed, self.current_speed + self.acceleration) + elif key == 's' or key == '\x1b[B': # S or Down Arrow + # Decrease speed + self.current_speed = max(self.min_speed, self.current_speed - self.deceleration) + + # Handle rotation + if key == 'a' or key == '\x1b[D': # A or Left Arrow + self.yaw += self.rotation_speed + elif key == 'd' or key == '\x1b[C': # D or Right Arrow + self.yaw -= self.rotation_speed def publish_command(self): """Publish the current dog state as a command""" @@ -170,11 +148,6 @@ def main(): except KeyboardInterrupt: print("\nTeleoperation interrupted by user") finally: - # Stop the keyboard thread - node.running = False - if node.keyboard_thread.is_alive(): - node.keyboard_thread.join() - # Restore terminal settings termios.tcsetattr(sys.stdin, termios.TCSADRAIN, node.old_settings) rclpy.shutdown() From df8cae0b6c173edbd8bef562e6a2b78252a8b02a Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Wed, 18 Jun 2025 14:43:30 +0100 Subject: [PATCH 05/59] integrated simulation and dog control --- .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 178 bytes .../auto_shepherd_simulation/dog_control.py | 4 +- .../dog_control_simulator.py | 45 +++++++++++++----- .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 0 -> 8339 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 0 -> 3694 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 0 -> 6698 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 174 -> 0 bytes .../dog_control_lib.cpython-310.pyc | Bin 3591 -> 0 bytes .../tmpDogSim/dog_control_lib.py | 29 ++++++++--- 10 files changed, 58 insertions(+), 20 deletions(-) create mode 100644 ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc create mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc create mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc create mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc create mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/tmpDogSim/__pycache__/__init__.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/tmpDogSim/__pycache__/dog_control_lib.cpython-310.pyc diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..782a655adf4fc320f8c8a338ce30d5adabda0d17 GIT binary patch literal 178 zcmd1j<>g`kf~>^A3??A`7{oyaj6jY95Erumi4=xl22Do4l?+87VFd9jR6ip>H&wqV zzgRyhu{brp97vaD=A`H+mX_ql7iXjvWTY0Q#207gmgXduWaj7LmWYqf%*!l^kJl@x Wyv1RYo1apelWGUDt{CJ#76t$gHZCv# literal 0 HcmV?d00001 diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 8b90dd3..6394d72 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -17,7 +17,7 @@ def __init__(self): self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, 10) # :contentReference[oaicite:3]{index=3} - self.create_subscription(Path, '/sheep/poses', + self.create_subscription(Path, '/sheep/poses_sim', self._sheep_cb, 10) # :contentReference[oaicite:4]{index=4} self.create_subscription(PoseStamped, '/sheep/goal_pose', self._goal_cb, 10) @@ -63,7 +63,7 @@ def _control_step(self): self.get_logger().debug(f'Cmd ({xd_opt:.2f}, {yd_opt:.2f})') # plot - plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt, points) + # plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt, points) # ---------------------------------------------------------------------- diff --git a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py index 7387b17..c9a3fb8 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py +++ b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py @@ -16,7 +16,8 @@ def __init__(self): super().__init__('dog_control_simulator') # Default the storage points for the animal data - self.sheep_poses = [] + self.sheep_poses = {} # {sheep_name: [dict, dict, ...]} + self.dog_poses = {} # {timestep: {dog_name: PoseStamped}} self.dog_command = None # Store the latest dog command # Create QoS profile @@ -27,14 +28,14 @@ def __init__(self): ) # Subscribe to input data - self.create_subscription(Path, '/dog_poses', self._dog_cb, qos_profile) - self.create_subscription(Path, '/sheep_poses', self._sheep_cb, qos_profile) + self.create_subscription(Path, '/dog/pose', self._dog_cb, qos_profile) + self.create_subscription(Path, '/sheep/poses', self._sheep_cb, qos_profile) self.create_subscription(PoseStamped, '/dog/command', self._dog_command_cb, qos_profile, callback_group=RCG()) # Setup output channels self.create_publisher(Path, '/dog_paths', qos_profile) self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) - + self.sheep_sim_pub = self.create_publisher(Path, '/sheep/poses_sim', qos_profile) self.simulation = Simulation(800, 600, sheep_states=None, sheepdog_state=None) @@ -68,29 +69,27 @@ def _dog_cb(self, msg): def _sheep_cb(self, msg): # Get current timestep - timestep = msg.header.stamp.secs + timestep = msg.header.stamp.sec for sheep in msg.poses: # Create sheep creator name = sheep.header.frame_id if name not in self.sheep_poses: - self.sheep_poses[name] = dict() + self.sheep_poses[name] = [] # Get prior pose prior = None if self.sheep_poses[name]: - prior = self.sheep_pose[name][-1] + prior = self.sheep_poses[name][-1]['position'] # Save sheep pose data pos = sheep.pose.position - self.sheep_poses[name].append(dict()) - self.sheep_poses[name][-1]['position'] = [pos.x, pos.y] + current = {'position': [pos.x, pos.y]} if prior: px, py = prior - self.sheep_poses[name][-1]['velocity'] = [pos.x - px, pos.y - py] + current['velocity'] = [pos.x - px, pos.y - py] - # Run the simulation - self.run_simulation(msg.header.secs) + self.sheep_poses[name].append(current) def publish_simulation_state(self, state): """Convert simulation state to MarkerArray and publish for RViz visualization""" @@ -194,6 +193,26 @@ def publish_simulation_state(self, state): # Publish the markers self.marker_pub.publish(marker_array) + def publish_sheep_path(self, state): + """Convert simulation['sheep'] list → nav_msgs/Path and publish.""" + path_msg = Path() + path_msg.header.frame_id = "map" + path_msg.header.stamp = self.get_clock().now().to_msg() + + poses = [] + for i, sheep in enumerate(state['sheep']): + ps = PoseStamped() + ps.header = path_msg.header # same stamp / frame + ps.header.frame_id = f"sheep_{i}" # optional: unique id + ps.pose.position.x = sheep['position'][0] + ps.pose.position.y = sheep['position'][1] + ps.pose.position.z = 0.0 + poses.append(ps) + + path_msg.poses = poses + self.sheep_sim_pub.publish(path_msg) + + def run_sim_step(self, timestep=None): @@ -203,6 +222,8 @@ def run_sim_step(self, timestep=None): sheep_estimates = self.simulation.get_state() # Publish visualization markers self.publish_simulation_state(sheep_estimates) + # Publish sheep path + self.publish_sheep_path(sheep_estimates) def main(): diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b68bc886ec1a89279e716e312d2a24d707d40fa3 GIT binary patch literal 195 zcmd1j<>g`kf~>^A3=sVoL?8o3AjbiSi&=m~3PUi1CZpd-FBVSEpJ8>+eO;j{O8&q{l(0nU$Wr}3p zE-j0ofB^O(hX6gd=s^y8@g*n%6ljYcg7#GOKiFH*L!eWO=G3By`Y%Oxe*?X?m( z``)~-eQ#!dGjFytIa$!~xAob(&6RI!+Fz-X{WDRyj9>6|5TOaZrLC){zOHlKST|NQ zVF>fCCd@m=x+(NEtzzw?zqVQ@n*B@X`vhE1qdC?Mp<|S>ZV7YU7H5PloV)tEBXYvM ztF7lmUKH?j#e^v0nHQ5{3eSR=79~6<#Eh85vnY;9xaZ5Y|F3n%SR zXm(m6Ds6RcjdB(nveqY7(Dgh~-RiYMzuWRX87(Hm540@Avm9tB)mna|y+yCEt=?_A z-f4OPUOHjS1YWnM_K~ox!0&sJUGKCyGRkk&ZmV}u#f^@vV=G+`k1p-LDr>^;1yQLh zJE2z(@zOD8`bMYM7B#t3-Kxok-;U-)r%{z&x7W(P!5CK!uGo*xk_ z_PW`g%sAhL;Hy=??T6Ls1eOdgf)tE`?wYRd=mp&|9h4oEsWe6t9{TmC*lf=ylhw&| zlRqkxp0mA@>cQUV$CkJWoBV|Vi!l1nyz7NM*)GSbRc6&H`|Wbr^r%7yF3OP7RxPX; z@+@ZJldG6=lFCylLzl}`rc+d;f>M-kc(J~|fNDV2QZOyu`Kmvy-ac&~d3~}Uf}9j? zLTyjS>*K{w0Ev&$R)EKQ_a3QvX@ky`^eq2_>l7Egi{o4ML10gD~x z_wx5N0xDtM)Z`07-!<9}xCKJ!dw{UG2cV04te}TE(!&JEnGA0eDbN|TM#b&F@`J8` zrK+3aops#{6jISKFkCf0UHMCY?$f_r+W+^1 z2M?qR{%WNdS&&K-n*D(cWr_OEgGA0d-}@o(LXz0!apL6OyYlvTiFJ(ICB)jhddOKU$85ec2$GcVN1V^g{WsuWm!7M^si@Iyz zANpKOp9Nz|@6Red$PTWPr%C>(oT|b%P=O%fq-_m8hkekCQ1n+7*ND0BM~W*<2*T=D zu5`PtopMbbZaM6Xh$IWXLl3c9(fLfF^`zL5%?C5S>4_T6T;;YOp<6&An>QZQ`!i}^ znf8%wktL+t`UfhBL*3LCQ~=_(tF`hD3Xr%elCMO&w?S( z6M2FNqk0*o`;HQuqo#;*0)9$*B65XKxCXCd2chRl`8*9lvfUu%myaxQNv}B`y zmv%WSy=ls8iK7`E1MCO}c$a1Y8ak-~qRu9cl}EVdp@vVr$nf!G zZOrkiIW~M+6)}3o(Ygb;&dnUw{TwZ{uFLD@VECwG1@1q>rFG#Flk*26KX)YwB}bH_ z7S@~PBrGm(`(d-(rbzQfM~=J1iYtehIH6atROTCjga+_xIl zWFZPpd+iW_!1o~XT@;aj;W%+U%rlVvhnO~zZI1Mr`&RKmnBTmHYX#}1O9g!Wy_NnZC^C$#bO=Wi-)&)07 zVZbiASRG}A0lN;nW4LEnq(hXEC`UF2Tf9C%aJxx}bRCX4cap|B{KRb<;c$+9js?D( zO3fa)a|tpfxZ`Aa1EDD1vwz z_0lLx5*MQs_o1s72FM;TYVj_xQXxgHELvu{ygr91=hxC(ZB}b*w>rKk*JWp0;EH{; zwz`MLE2Lv%K}%(Vk&5+J!A`2Qks)s5x5LUCxj~3bKm#}bYc$JY;8Ymi#Q%&w0ijq{ zrHcs3=k-|*#!JSWK9^DM1gqxFdEizO?DR`XsFPgrkG`{g2!)T`td4`{fCtKXi~=AM zsEA>vjZ_V{B0)rI4pn}7*e4b@?se3P@y}DxbYMKS;>H(`zdGy_D~7%2(3~n}rq*Hf z%qsc&@F6{LJLItgBUKpG;m}mtE6b($Hr^lJ#?2^f{MO9H>W_Xe1SYhrJkH~H@%k#R z&%u0&V{s`DETwxUr>eBPt*XZ0P*-=@am>LTGVWuioTN{}!QD0P+M^UT$FC({*~M+`vjpc3grx#5=)BqSc9cloNtG1t&+Gu&{@u^JXzc5GrRd1Z|T7 z5{9jQgfgY0r-ifj-;INgK-+$aag@5|aWuA)>o2OL#RhW3l;%wxPaFuR#v3QD_b*Ah z^sH89R4Ql%75NsCcZfVjZG8Pz?!61B&CLjyD5T=maec z9}4FA&Bpi!9F1l{4UHo|>NcZ@sqC8n5isrO9G zom9YtcBDh0f_DGRAqmVRfnG*hP$epC}8dBA*2+k$TBR zA}5GU5+VN+nfO=$;6F?e%SD)Bv1DT=XNbJ1MXPeW~{pf>mc zZpkG3bIAzE;Xr?3P;X(ck!I3}0!F-PXE+ZS=%0FJSp4E z4u^vR94HPd{&q#BeKMYen>K+#$DsNWceOGRIo+K`ZOe-+fdzm9zO3UT))HDH({G1E zNy^7*>`?X$C~;oZQNhWWlaiEtyN!i{A_~i9(Ofo)`m(WXCMh3UbHwOLRH@MuOsPvS zn=W~JWK1(@3`)Q0IPBvCjSfFGJ?=$DNS^w>6@N0MAc+(4_l+S|R*cBGg-=xYtYx(+ zk3Ob|d14|((Dy!~jgY@wLTbQ*?j@cz?J=TsBk=W|N9}|@v(gptsdyI@k?I5>v|o-| zQKQ|^PUQ{SNgV_d6?mx6aJOo$9uS<`#x`NM{4GEHX?6o;Y$3s3ZmpQBtE-g-`mWW6 zK2)nwp^7gCqSvBwv0DA0S8J&j{sN&ap8X4bbC=H$VaQ;&L^pVOg~*r>V^_sa>pg1y zF%i;}s7Rl!{Q8y`HakLY5Wh)exEC(b7b%&j3A-B?S0W4BDH!fEZq7aB+U^4WEAG5| x-aX^a;rFCbD(HqYrSnKcz3C4E;zf(Rk+e*mFJwwC|^ literal 0 HcmV?d00001 diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..848d50c7e4fdf064bdb881efe376295a604fb715 GIT binary patch literal 3694 zcmai0&2JmW72nw}E-8}M2W`n};u0xpfNc?_2+&U>v}@<1u%QN0+XOmfL2*Xp%G549 zv#czFx>UaQ4`|VYbj-Df0=>1zqU|ZSITZ*{L$?6wp=i;#zc+guX9ig z>|=~E>u_3gxeMrV+UFZs;TG=YWfIRx>^Uj0rX3fnVzj4&aiXF$N@BSHPLlY4Xvt-C|JmCZ zCI@0eCTe51r$l&&eiDuNMsJcPp&E+uP)HuC=wLGH5sNQg0I?XCDa`~=2Ct5fbP$G7 z9Hn9CL8@v2)a)AbS>ObKzC(YFO_%Z)cr~Z*)6?dS@Zu2`F_%|4G5MH?nslu4k`#@&aKAd4tKO9CQ(SD5gf&$uyQ96v`OZrlq z({FE=^rzYf6P31ip;c&iS3r{nO=*A5wi8_FP1kBNO6eKnJ}P2D1cC#+h)1i_?>Lby#LW2h(2f}~G2o;UJVQkJ>FEv%&0DZMn#-lE%>e$P&=edi3p z2EBXgkw@(N2&VH}mMGmbhatcoI|U_Km(ATdjrT(Dx2(Ndn$8U^n014 zU_MhVW@=@cfkV_6(;0=ke02*$HVUtc*i5*$Pf{!p_{e z^zt6a|FS)hY0sKrBt%Kc~0hIG8n9rg3S3J2>cmf@VaTAb_c)x z4Yl*`eCFfsvux?3?laJ#{iFLVqxoZZuxHvoyMw>~joQDugN)|i`S$;IA&PwM0?bK1 z$whtG?J({4;aoy?RtXndNpF;`ND7-0 zwVWky5?Cid2QEcxa~(>Ml5CH%p`v&nSTz=`1YQu-D`usS$xDsM@8VZ40ThKB3M!Ok zD49^cQQ79fu6DjpO#Mmf5QEElS=5FBU}&(X#_ zqs)i(UcWDJa8pa^Iv1r;)0hfstc?upIe~JNwW^wg{cLc@CL1(t{R%PnY~Ih&i>7r zvuYL56`u1|sHw9qUqPq+hvB0)-}+>9*WYnd_ZfJhvC+b-+sp|a_v40fA%iX z9ZISgWtYG`0zV>RFnaKl_cn<@$yJ)ZOMsq3egi-|{X}U;p?oqmyz#`M@|c^7b{~r) zrEUHM|IrJUqW#I3BM=^dn<5`>*~_eju+nnY>=s+I*PWJggVmi^oaxQaF`I1nX2+A? zr$haKzz+$S@IY~^B=900kRR^iHziMC1*~3^G>0RT2F>M6XfU&%$3h2pvcU8PB^Xly L5}&5M{K5YLcdJ-D literal 0 HcmV?d00001 diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0f634cc80a3a0d860b5ec128afb9f228152f3c20 GIT binary patch literal 6698 zcmaJ_OOG5^6|Q$zKc}Z>VvqBzm?Tao!6qRjU`*n%J$}SVJMoMO8DdK9zBN5H>DSz< zwmq%3gfiJ6h!BDWQi9a8OSW0@0}x`5gv8yjV1arEI|xb4cTV+d20PuVQ}>>G>(;Gv zzI$Hnd_JS$*Zl12=JQW#+TW=&`!mrwjVt;sh|q-I(AJZ?zOHlMST}Gt8fMd4w{#ja z8+OxKcbcj76#CY>E9~{OaMm*-wVoC3dQPO*^A|Lc5!nwkk=-@c3+UxU9=$wzbLbVs z9C~x;6@z(Ee9w@t1w~;A8=O*58Z7K-SX*D^t2{r%?zBo-SN7^=5Om6ha=CG?U00=X z>ukFf%XXs?$k4-gg+DSh?Nd|gcyvlWHHSyR82+5NDG)I3Qk&$&1dN|jJ2xgZH1_?l zZX&cN&S^Hk5u_={w546^eLMIh#RP-;Ct~-X<48R5omdaae88 zy7CY>pK9_k+&_QVMCm#cH_1mZ3dmqKngz!o83k=4%@B! z4`6c8nVmU(=y+#GxxP=E@cqMhZ&U&)>CWW;s9w-BcYAY*(vFYk)=2Z?pM{2li?!E5 zBi6R`fzj7vv#)I#d!|0+K$lzgh~o@WI7IrMamVNz188$A{Sowqdq$WaS-%5*Vr$R5 zWA^ny7D&KaTREnA0S#{$dj=gt=+UVRm+;=6ankFV$DiL=8ngJ{Z!K+2XpLTo3h$slOS zV>~AgZpW;(gm_>|IuMWJiU?UHqhu6}M|Dr{Wk-UfE$tH`&Z$OFWFdZrMql63Vtvbi zMOef~n*$3Rdry}ohP*=`PSdB9hNss4g@U=Ek~r# zvV|+su)?sHP*hH3RwtIDQX|}rf7G7f@O^Y(hBY516jX-+)w-HI+}DK;guv84q}5?u zg#l`QY}~P819L3k3mZ6s!z`Q+ad2U{O@6X0QY|yK`sOaJYS#%@FzJI--`vx2n8~DL zqo8S#0nIS3vXvp(8-^BnJir82v)xke&7je)h4GI30gb-`0<}-t%G{~m8j_6axbXYu ztMza5@6WIPBjGHaE@vg9=MgX@30ApmF$FxT*$7%H-Rd^U;sza+No;8&jAAv;i`6z026n3?=9H-*zH^7$?*jIfUrKa zNbTEcLS>&;F5hwDbl-_HyM|mAcHgrzatuWs7>LJX7$D)nAw)7;Nc)RJ1Ar@8SX~-afo;8KmR9(#LEw2(l z#_Ke@QS99aypC+&gmn{MHS$DQBg|H17(~gdkBwh)%zH1`Nzj{zp{lm(fjoz|!Xfc) zRI1wwH`=l}^VSs}uXZ|cS&~kN%7jta!iThKh)w8iD)U1MR92*HOJ&xBxSUF$_8Lvs zedRQ(5~0iVtCW3DjGf2)w{S%-g1B&HmR>Z9y7iT7xj>kqTliRxDm2}uGy@+du z|KHxjv#1-zJd<;Ce?RyM-pBCM8Gd@K0~UFBIUTT=k708d76>?DYsl)rPa-@_`hW`e z5q@I6CZ7f08S!9q;877qZgJSLhd=`D3;_5b`@wAA@U!5++xZl7iz9nN8pHh@g^WPIc5ncGT!JnX)McnbaK+8(fud=q<5vmh-pl*v-5w* z0YJQCV>8G=ijMDFbo}1rerdz2v|GV*$sP%_uye;Euhx|ae&S@WUKn}JDm*f-Kunjb zBv?PoSjTgt+4n+w_3QRbXs2XkNF>31QNBrSLYcfu^jA!g8p_hN7-EE2J zz=4n#=&6fDE`ca}1McBH<#bsuqliFW<{=jLL2J5>A&Ow5M?qYd&|G9R=MV^FOxIv^ zn+Oj6-J74mbn=24eb4wuZE`1UN6Et^c@e;)ozuwuQH;EYtmI>ZGLhahSA)1KTdbJ< z+GuYaK-9kX(AlR5d=*zjVJ|t=>&_kIPXldF`<1bc_4zxIHKt(&5nd7G*;c6p#1TWg}UZR0x@qrApNKE;01P z22CR%kH%@}7q}uyBnqYr2i9A>*9*%=1?g`ErtG`)-1mr_B=R^BvQ(0k$P3PV7{AV= zf$xL7go`6{SIg*`2Q!c59%gR z&QkNUagLtYrO|v$)+&!Eo;WnK7|+8N8X|>c7*Q7L5@`g8xm|a}&(H8tmzeO2GyHi` z+I7Xk$FyDvJ6aelB8gkvqc<*y2gIQ|wI1NO?Qo@cB=L*q=^P?S_NFS86lF?Mx1?}G zl0ewk4h34nKT&s%2pyzR{N|ok>E&^ZH6&$^1{O~%_v|Rt8a_==ErTG+okUw@Q>7>2 znR3T*8A7_r6nPZS^q#sF1>W1stLG>hScdPsei}k-j#jk_2sjdm3`sr%1T8(MVo(; z2muw_oA?n)r<@OxT9|l>+N9LOnSx41TwRGcHI*a@m7}OYb~*Zp#x`&o@*X&dbc}w3 zE253K21Sh}J;%`_#gjR+WG-1toDdaLhGFQLuX=ex_%xa%g^hm%by74#{U>N7wShg@ z!0X4zMr|?Bk){>_YK{V#V^cbdQ35@uwxdxv>_M8JPI1)$2`xvEPi)(Q?5NpWyvSu3 zFDd1CoY_U^*AV6x@p}0kB4jJ&H$jy3*0oD(Ds_3~9ZE`-3D>0D%RY3w{3Ze?n&NP| zC*s6yEIwKVRREUtjFs_-ouADQqu+zYX1QQMijXAHf|=8eyOz-_vE=qWu+O-Y_~>Hs z-9Te3K8S8q6QIvW+ote(PvhvtMy&uggZZw|kD#zZGkl6%T#A)OeTr~kLni&4upmOa zH$RGF>QHOq<0YLlxk)4hsg(1HG;gA#vg8dqGKiu;$ZeX#7MT3#)b6t^IWa%rq(>rE z7DOMhZ$2PUUhR)qETXDH#?0}*XcqNF)Kgf1_i(=s=DN68a-X1qZ>6Fl6{Hf$m|GS= z=(3}v6hdPlkxWd;a>7ABHE5N zi7l}DAomPpM9gDE%0$@aJVPxu3w%!ALl?!d zM%XDkiO1$pVTagyD4(Nyjw)Y%)Ri06+W#e{w1vOoO27}D74)K604ERP8yI8qheqtSRYvIH?@JC_|JhO68#!c8eSDA0>wu*889Fwq-*u& zuXd9ti-p^0w>xXYQ+Q3KoEcg1I7DIBK1)7J_Arv*nLY>=tq=jFMMhtl(N3hS;5O|4 zxYktTcrzuttroo90U#?A{C{B;7D)zKl*RF+OU0%yTqmdZTowtYRm$!?1DyTakss3T zrWSk}<@gx*HNA?8_gqYR!Y#N9xg|*>RI;J~-}lRl$wB@Zxbh7ex^E%*I6Ix0LUO`L zlFt|JSC_ueQ}Cc%Y+g`kf~>^A3=sVoL?8o3AjbiSi&=m~3PUi1CZpd6hdd oxa6k?XXfh1$7kkcmc+;F6;$5hu*uC&Da}c>16ftf1SD7(02l)+#{d8T diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/__pycache__/dog_control_lib.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/tmpDogSim/__pycache__/dog_control_lib.cpython-310.pyc deleted file mode 100644 index ea12746f31712d48175f635b110b1b0320cd9052..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3591 zcmaJ^TaOz_74E9OxNWyR9*<|nlU*Wbjh+%ej8%IB&HY z7M{N^es=PuZOi%_OFud=hQ1+uQAght4bem&)LWt}+G6VycGMC(q9e93Y76$EHSAsDF6(i`sJnWI4-fA{ zyv0}1l-3ibz>nZBc*RO<#X+&3ILcD2ZwZ^|7p&rnDL&>n)p;!Gz?V!r;UXz6X)<&* z&t}@0#$l%IX}Hi{nq*-*(N31j>Co2REKjns&|Va0Wh}KPLy^pjmzHc{|Gz&vJjti= zq0EcJFiNH`7oIt zihMGP@~o73I!cq{yR)S}D5hbW#zi@bvnb7rB*T(KMrmBb#Rci2&2BKC$t?^c2hHa3 zF(7>zUERe1I^lh*a-a(vy$fwIk}`>lp|9COv!&(>q4^T8Sm0f1FFc)$Fkbgdqo#G!ro&p3xEzT@#u2F?0I47c z^dU&Y{NxQxBfQrSiALP~5U(-)1jaAfiYx1aoih&e!_3wr>sR&4R%~S}Tk*H>jaknE z1&5Ly>>N7UF6L7sm$t)Wm^7Tt;!NO!cN(VixX}Fco~~oj)zn7+2LBQmzy!9A7Tv-XJst_9lxh1z{^Y-rrj>o!HS$Z_bqeZNlrgUYoLW=C1ZJsPDgYPlYAgE< zs6IGx;BifAvD>2_aOtQ9ow)Y3s)Lm{m5fL?f-hcet8LZ69PPe^JGzjiv1+QWa1SiB z?7$Mgqrps9osiFY2J$RiMN5uv_&6gVuZHwy3Gy-&=oxO_-(j zR1Xq+PaXLwvoiMVkE*SG$6N$3TB_ZAO<^zEG;353(+k;`( zp!XV-+@eIrkmP2Pd`s8DB9t;*%C~9enlVsIMilln^ktacql}b^e5z~nEExl-br9w0 ze435SaoVQEHqwW2Xm8*Bjk(gG4e*upwTGQ&Gi6W%W^Y;Xp>mC z))J1)fjL^6(0zbK7rp#BwnQZ9=Gnu5QnkU* z&x8ozB0?6xaACp0TUA@NR<5uqfnnsWNBCZ&Ai3boD(xr0&R*WuV5gS|AfR{$cw3uqTukKh5Y0IQn@tG5x-b%b}P_p{Tx^XwAZIgUG`qgt!|gTP-ho!?V*3RuQ|#RmTcW#Ov+BGy-WGr( zv!Z)pq2F2chyvKzS?!9R+BJK2R(k?aN;NJZV)yZ@Ait!fnf%j6BzzO2A=j)xpr*5v zFzI~RvL?U#Ej@p}ubF)Rx&P4AZ-XCD3P+IUQaj@$Me<6+W8^S)e7)qs8ZZPMYRg}u zlOLhcERye`e_liFcz2dfw0)Lj!~It%BELn;>{(iBznCA-(j3KWt3c5;P9`TY-ckow z)3ZdBC%RTdVTsaqorS5sF;J7AN%CBihqbrnT-QS(P}i2x36gJ`Op$!4trg*EjPoHC z1wJqbg_M^}WgH?9lWB31pXu6Jn3O+_m&3i!1|0cgy7nj3P&mm))Les&d>>P7(`7td z$am-?MXUT8HTUSFzb=yjDVx!;=3s^j_H|<_(Jtv`bcN#tmSy}StSCr Date: Wed, 18 Jun 2025 14:53:29 +0100 Subject: [PATCH 06/59] rviz configs map improvements --- .gitignore | 2 ++ configs/rviz/default.rviz | 39 ++++++++++++++++++++------------------- 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/.gitignore b/.gitignore index c6b757c..a9e5462 100644 --- a/.gitignore +++ b/.gitignore @@ -75,3 +75,5 @@ crashlytics-build.properties *.swp *~ + +*__pycache__* diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index ed06ee8..a4a5b93 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -9,8 +9,9 @@ Panels: - /Grid1 - /Path1 - /Path1/Topic1 + - /Path1/Offset1 Splitter Ratio: 0.5 - Tree Height: 555 + Tree Height: 1204 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -35,7 +36,7 @@ Visualization Manager: - Alpha: 0.5 Cell Size: 10 Class: rviz_default_plugins/Grid - Color: 160; 160; 164 + Color: 200; 200; 200 Enabled: true Line Style: Line Width: 0.029999999329447746 @@ -43,11 +44,11 @@ Visualization Manager: Name: Grid Normal Cell Count: 0 Offset: - X: 0 - Y: 0 + X: 60 + Y: -60 Z: 0 Plane: XY - Plane Cell Count: 100 + Plane Cell Count: 14 Reference Frame: Value: true - Alpha: 1 @@ -95,7 +96,7 @@ Visualization Manager: Value: true Enabled: true Global Options: - Background Color: 20; 48; 48 + Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root @@ -139,33 +140,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1475.8636474609375 + Distance: 260.7147521972656 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 332.9386291503906 - Y: 305.6679382324219 - Z: -117.16397857666016 + X: 43.791133880615234 + Y: -48.998191833496094 + Z: -15.675467491149902 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.2203975915908813 + Pitch: 1.3597965240478516 Target Frame: Value: Orbit (rviz) - Yaw: 6.2435832023620605 + Yaw: 4.8685808181762695 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1495 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007270000003efc0100000002fb0000000800540069006d00650100000000000007270000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005cb0000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -173,7 +174,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 1200 - X: 250 - Y: 73 + collapsed: true + Width: 1831 + X: 0 + Y: 32 From af555adb7fdd81df0fe0d50d39f83a6194e06f14 Mon Sep 17 00:00:00 2001 From: James Date: Wed, 18 Jun 2025 16:54:01 +0100 Subject: [PATCH 07/59] DO NOT MERGE updating to simulation using metres and polygon field boundary --- .../sheep_simulation/sheep.py | 177 +++++++---- .../sheep_simulation/sheepdog.py | 134 ++++---- .../sheep_simulation/simulation.py | 300 ++++++++++++++---- 3 files changed, 415 insertions(+), 196 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py index cf85be4..823d193 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py @@ -1,6 +1,7 @@ import pygame import math import random +import numpy as np class Sheep: # State constants @@ -8,48 +9,48 @@ class Sheep: WALKING = 1 # Moving at normal speed MOVING = 2 # Moving faster, usually when flocking or avoiding something - def __init__(self, position, velocity=None, width=None, height=None): + def __init__(self, position, velocity=None, coord_transformer=None): # State - self.x = position[0] - self.y = position[1] + self.x = position[0] # x position in meters + self.y = position[1] # y position in meters - # Physics + # Velocity if velocity is None: velocity = [0, 0] self.velocity = velocity - self.acceleration = [0, 0] - # Screen boundaries - self.width = width - self.height = height + # Physical properties + self.size = 0.8 # Size in meters + self.max_speed = 5.5 # Maximum speed in meters per second + self.color = (255, 255, 255) # White color for sheep + + # Coordinate transformer + self.coord_transformer = coord_transformer # Movement state self.current_state = self.GRAZING # Start in grazing state self.walking_threshold = 1.0 # Threshold to start walking was 1.0 self.moving_threshold = 2.0 # Threshold to start moving quickly - # Speed multipliers for different states TODO CHANGE THESE - self.grazing_speed_multiplier = 0.1 # Slower when grazing # original 0.05 - self.walking_speed_multiplier = 0.15 # Moderate speed when walking # original 0.15 - self.moving_speed_multiplier = 1.2 # Faster when moving quickly # original 1.2 + # Speed multipliers for different states + self.grazing_speed_multiplier = 0.1 # Slower when grazing + self.walking_speed_multiplier = 0.15 # Moderate speed when walking + self.moving_speed_multiplier = 1.2 # Faster when moving quickly - # Flocking parameters (changed these defaults to match slider defaults in simulation.py) + # Flocking parameters self.alignment_weight = 3.0 self.cohesion_weight = 5.0 self.separation_weight = 6.0 # Physical properties - self.size = 6 # was 10 - self.color = (255, 255, 255) # White - self.max_speed = 4 self.max_force = 0.2 # Perception parameters - self.perception_radius = 100 # How far the sheep can see other sheep (was 50, needs same as dog radius) - self.protected_radius = 20 # Minimum distance sheep try to maintain from each other - self.boundary_margin = 40 # Distance from edge where sheep start to turn - self.dog_repulsion_weight = 6.0 # Weight for avoiding the dog (was 8.0) - self.dog_repulsion_radius = 100 # How far the sheep can sense the dog (should be same as perception radius) + self.perception_radius = 20 # How far the sheep can see other sheep in meters + self.protected_radius = 2 # Minimum distance sheep try to maintain from each other in meters + self.boundary_margin = 10 # Distance from edge where sheep start to turn in meters + self.dog_repulsion_weight = 6.0 # Weight for avoiding the dog + self.dog_repulsion_radius = 20 # How far the sheep can sense the dog in meters def get_state(self): """Return current state in the standard format""" @@ -75,13 +76,13 @@ def update_state(self): if rand_move_x < 0.99: rand_move_x = 0 else: - rand_move_x = random.uniform(-4, 4) + rand_move_x = random.uniform(-self.max_speed * self.grazing_speed_multiplier, self.max_speed * self.grazing_speed_multiplier) rand_move_y = random.random() if rand_move_y < 0.99: rand_move_y = 0 else: - rand_move_y = random.uniform(-4, 4) + rand_move_y = random.uniform(-self.max_speed * self.grazing_speed_multiplier, self.max_speed * self.grazing_speed_multiplier) self.velocity = [rand_move_x, rand_move_y] self.acceleration = [0, 0] @@ -247,40 +248,59 @@ def separation(self, sheep_list): return steer def avoid_boundaries(self): - """Avoid screen boundaries""" + """Avoid field boundaries""" steer = [0, 0] - # Check left boundary - if self.x < self.boundary_margin: - steer[0] += 1 - # Check right boundary - elif self.x > self.width - self.boundary_margin: - steer[0] -= 1 - - # Check top boundary - if self.y < self.boundary_margin: - steer[1] += 1 - # Check bottom boundary - elif self.y > self.height - self.boundary_margin: - steer[1] -= 1 - - if math.sqrt(steer[0]**2 + steer[1]**2) > 0: - # Normalize and scale - magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - steer[0] = (steer[0] / magnitude) * self.max_speed - steer[1] = (steer[1] / magnitude) * self.max_speed + # Get current position in real-world coordinates + x, y = self.x, self.y + + # Check if we're too close to the boundary + if not self.coord_transformer.is_point_in_field(x, y): + # Find closest point on boundary + min_dist = float('inf') + closest_point = None - # Calculate steering force - steer[0] -= self.velocity[0] - steer[1] -= self.velocity[1] + for i in range(len(self.coord_transformer.field_boundary)): + p1 = self.coord_transformer.field_boundary[i] + p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] + + # Calculate closest point on line segment + line_vec = np.array(p2) - np.array(p1) + point_vec = np.array([x, y]) - np.array(p1) + line_len = np.linalg.norm(line_vec) + line_unitvec = line_vec / line_len + point_proj_len = np.dot(point_vec, line_unitvec) + point_proj_len = max(0, min(point_proj_len, line_len)) + + closest = np.array(p1) + line_unitvec * point_proj_len + dist = np.linalg.norm(np.array([x, y]) - closest) + + if dist < min_dist: + min_dist = dist + closest_point = closest - # Limit force - force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if force_magnitude > self.max_force: - steer[0] = (steer[0] / force_magnitude) * self.max_force - steer[1] = (steer[1] / force_magnitude) * self.max_force + if closest_point is not None: + # Calculate steering force away from boundary + steer[0] = self.x - closest_point[0] + steer[1] = self.y - closest_point[1] - return steer + # Normalize and scale + magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if magnitude > 0: + steer[0] = (steer[0] / magnitude) * self.max_speed + steer[1] = (steer[1] / magnitude) * self.max_speed + + # Calculate steering force + steer[0] -= self.velocity[0] + steer[1] -= self.velocity[1] + + # Limit force + force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if force_magnitude > self.max_force: + steer[0] = (steer[0] / force_magnitude) * self.max_force + steer[1] = (steer[1] / force_magnitude) * self.max_force + + return steer return [0, 0] def avoid_dog(self, dog): @@ -326,7 +346,7 @@ def flock(self, sheep_list, sheepdog): self.apply_force(cohere) self.apply_force(separate) - # Apply sheepdog repulsion with original behavior + # Apply sheepdog repulsion dog_force = self.avoid_dog(sheepdog) self.apply_force(dog_force) @@ -340,14 +360,23 @@ def flock(self, sheep_list, sheepdog): elif math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) < self.max_speed * 0.2: self.current_state = self.WALKING - def update(self): - """Update the sheep's position and state""" + def update(self, dt, sheep_list, sheepdog): + """Update sheep position and velocity + + Args: + dt: Time step in seconds + sheep_list: List of other sheep + sheepdog: The sheepdog object + """ + # Calculate flocking forces + self.flock(sheep_list, sheepdog) + # Update state based on external forces self.update_state() # Update velocity based on acceleration - self.velocity[0] += self.acceleration[0] - self.velocity[1] += self.acceleration[1] + self.velocity[0] += self.acceleration[0] * dt + self.velocity[1] += self.acceleration[1] * dt # Limit speed based on state speed = math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) @@ -363,31 +392,45 @@ def update(self): self.velocity[1] = (self.velocity[1] / speed) * max_speed # Update position - self.x += self.velocity[0] - self.y += self.velocity[1] - - # Keep within screen bounds - self.x = max(0, min(self.x, self.width)) - self.y = max(0, min(self.y, self.height)) + new_x = self.x + self.velocity[0] * dt + new_y = self.y + self.velocity[1] * dt # Reset acceleration self.acceleration = [0, 0] + # Check if new position is within field boundary + if self.coord_transformer.is_point_in_field(new_x, new_y): + self.x = new_x + self.y = new_y + else: + # If outside boundary, find closest point on boundary + closest_point = self.coord_transformer.get_closest_boundary_point(new_x, new_y) + self.x = closest_point[0] + self.y = closest_point[1] + # Reflect velocity off boundary + self.velocity = [-self.velocity[0], -self.velocity[1]] + def draw(self, screen): """Draw the sheep on the screen""" + # Convert real-world coordinates to screen coordinates + screen_x, screen_y = self.coord_transformer.world_to_screen(self.x, self.y) + + # Convert size from meters to screen pixels + screen_size = self.size * self.coord_transformer.scale + # Draw the sheep body - pygame.draw.circle(screen, self.color, (int(self.x), int(self.y)), self.size) + pygame.draw.circle(screen, self.color, (int(screen_x), int(screen_y)), int(screen_size)) # Draw state indicator if self.current_state == self.GRAZING: # Draw a small green dot when grazing - pygame.draw.circle(screen, (0, 255, 0), (int(self.x), int(self.y)), 3) + pygame.draw.circle(screen, (0, 255, 0), (int(screen_x), int(screen_y)), 1) elif self.current_state == self.WALKING: # Draw a small blue dot when walking - pygame.draw.circle(screen, (0, 0, 255), (int(self.x), int(self.y)), 3) + pygame.draw.circle(screen, (0, 0, 255), (int(screen_x), int(screen_y)), 1) else: # MOVING state # Draw a small red dot when moving quickly - pygame.draw.circle(screen, (255, 0, 0), (int(self.x), int(self.y)), 3) + pygame.draw.circle(screen, (255, 0, 0), (int(screen_x), int(screen_y)), 1) @staticmethod def normalize(vector): diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py index 6d1e8c4..b3b1d66 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py @@ -1,11 +1,12 @@ import pygame import math +import numpy as np class SheepDog: - def __init__(self, position, velocity=None, yaw=0): + def __init__(self, position, velocity=None, yaw=0, coord_transformer=None): # State - self.x = position[0] - self.y = position[1] + self.x = position[0] # x position in meters + self.y = position[1] # y position in meters self.yaw = yaw # Direction in radians # Velocity @@ -14,25 +15,26 @@ def __init__(self, position, velocity=None, yaw=0): self.velocity = velocity # Physical properties - self.size = 10 # Size of the pig + self.size = 0.5 # Size in meters self.color = (255, 192, 203) # Pink color for Babe - # Screen boundaries - self.width = None - self.height = None - - def set_screen_bounds(self, width, height): - """Set the screen boundaries for position checking""" - self.width = width - self.height = height + # Coordinate transformer + self.coord_transformer = coord_transformer def set_position(self, x, y): - """Set the position while keeping within screen bounds""" - if self.width is None or self.height is None: - raise ValueError("Screen bounds must be set before setting position") + """Set the position while keeping within field bounds""" + if self.coord_transformer is None: + raise ValueError("Coordinate transformer must be set before setting position") - self.x = max(self.size, min(self.width - self.size, x)) - self.y = max(self.size, min(self.height - self.size, y)) + # Check if new position is within field boundary + if self.coord_transformer.is_point_in_field(x, y): + self.x = x + self.y = y + else: + # If outside boundary, find closest point on boundary + closest_point = self.coord_transformer.get_closest_boundary_point(x, y) + self.x = closest_point[0] + self.y = closest_point[1] def get_state(self): """Return current state in the standard format""" @@ -42,71 +44,84 @@ def get_state(self): } def draw(self, screen): - # Create a surface for the pig - pig_surface = pygame.Surface((self.size * 2, self.size * 2), pygame.SRCALPHA) + # Convert world coordinates to screen coordinates + screen_x, screen_y = self.coord_transformer.world_to_screen(self.x, self.y) + screen_size = self.size * self.coord_transformer.scale + + # Create a surface for the dog + dog_surface = pygame.Surface((screen_size * 2, screen_size * 2), pygame.SRCALPHA) # Draw the body (oval) - pygame.draw.ellipse(pig_surface, self.color, (0, 0, self.size * 2, self.size * 2)) + pygame.draw.ellipse(dog_surface, self.color, (0, 0, screen_size * 2, screen_size * 2)) # Draw the snout (smaller oval) snout_color = (255, 182, 193) # Lighter pink for snout - pygame.draw.ellipse(pig_surface, snout_color, - (self.size * 1.2, self.size * 0.8, - self.size * 0.8, self.size * 0.6)) + pygame.draw.ellipse(dog_surface, snout_color, + (screen_size * 1.2, screen_size * 0.8, + screen_size * 0.8, screen_size * 0.6)) # Draw the ears (triangles) ear_color = (255, 182, 193) # Lighter pink for ears - pygame.draw.polygon(pig_surface, ear_color, [ - (self.size * 0.3, self.size * 0.3), - (self.size * 0.5, self.size * 0.1), - (self.size * 0.7, self.size * 0.3) + pygame.draw.polygon(dog_surface, ear_color, [ + (screen_size * 0.3, screen_size * 0.3), + (screen_size * 0.5, screen_size * 0.1), + (screen_size * 0.7, screen_size * 0.3) ]) - pygame.draw.polygon(pig_surface, ear_color, [ - (self.size * 1.3, self.size * 0.3), - (self.size * 1.5, self.size * 0.1), - (self.size * 1.7, self.size * 0.3) + pygame.draw.polygon(dog_surface, ear_color, [ + (screen_size * 1.3, screen_size * 0.3), + (screen_size * 1.5, screen_size * 0.1), + (screen_size * 1.7, screen_size * 0.3) ]) # Draw the eyes (small circles) eye_color = (0, 0, 0) # Black for eyes - pygame.draw.circle(pig_surface, eye_color, - (int(self.size * 0.7), int(self.size * 0.7)), - int(self.size * 0.1)) - pygame.draw.circle(pig_surface, eye_color, - (int(self.size * 1.3), int(self.size * 0.7)), - int(self.size * 0.1)) + pygame.draw.circle(dog_surface, eye_color, + (int(screen_size * 0.7), int(screen_size * 0.7)), + int(screen_size * 0.1)) + pygame.draw.circle(dog_surface, eye_color, + (int(screen_size * 1.3), int(screen_size * 0.7)), + int(screen_size * 0.1)) # Draw the nostrils (small circles) nostril_color = (0, 0, 0) # Black for nostrils - pygame.draw.circle(pig_surface, nostril_color, - (int(self.size * 1.3), int(self.size * 1.0)), - int(self.size * 0.05)) - pygame.draw.circle(pig_surface, nostril_color, - (int(self.size * 1.5), int(self.size * 1.0)), - int(self.size * 0.05)) + pygame.draw.circle(dog_surface, nostril_color, + (int(screen_size * 1.3), int(screen_size * 1.0)), + int(screen_size * 0.05)) + pygame.draw.circle(dog_surface, nostril_color, + (int(screen_size * 1.5), int(screen_size * 1.0)), + int(screen_size * 0.05)) # Convert yaw to degrees for pygame rotation angle_degrees = math.degrees(self.yaw) - rotated_pig = pygame.transform.rotate(pig_surface, angle_degrees) + rotated_dog = pygame.transform.rotate(dog_surface, angle_degrees) # Get the rect of the rotated surface - pig_rect = rotated_pig.get_rect(center=(self.x, self.y)) + dog_rect = rotated_dog.get_rect(center=(screen_x, screen_y)) - # Draw the rotated pig - screen.blit(rotated_pig, pig_rect) + # Draw the rotated dog + screen.blit(rotated_dog, dog_rect) class SheepDogController: def __init__(self, sheepdog, width, height): self.sheepdog = sheepdog - self.sheepdog.set_screen_bounds(width, height) - self.max_speed = 5 + self.max_speed = 8.0 # Maximum speed in meters per second self.min_speed = 0 self.current_speed = 0 - self.acceleration = 0.2 - self.deceleration = 0.3 + self.acceleration = 6.0 # meters per second per second + self.deceleration = 6.0 # meters per second per second self.rotation_speed = 0.1 # Radians per frame - def update(self, keys): + def update(self, keys, dt): + """Update sheepdog position and velocity + + Args: + keys: Dictionary of pressed keys + dt: Time step in seconds + """ + # Skip update if dt is too small + if dt < 0.001: # Skip updates smaller than 1ms + return + # Handle rotation if keys[pygame.K_LEFT] or keys[pygame.K_a]: self.sheepdog.yaw += self.rotation_speed @@ -116,20 +131,21 @@ def update(self, keys): # Handle speed control if keys[pygame.K_UP] or keys[pygame.K_w]: # Increase speed - self.current_speed = min(self.max_speed, self.current_speed + self.acceleration) + self.current_speed = min(self.max_speed, self.current_speed + self.acceleration * dt) elif keys[pygame.K_DOWN] or keys[pygame.K_s]: # Decrease speed - self.current_speed = max(self.min_speed, self.current_speed - self.deceleration) + self.current_speed = max(self.min_speed, self.current_speed - self.deceleration * dt) else: # Natural deceleration when no keys are pressed - self.current_speed = max(self.min_speed, self.current_speed - self.deceleration * 0.5) + self.current_speed = max(self.min_speed, self.current_speed - self.deceleration * 0.5 * dt) # Calculate movement based on current speed and direction - dx = math.cos(self.sheepdog.yaw) * self.current_speed - dy = -math.sin(self.sheepdog.yaw) * self.current_speed + dx = math.cos(self.sheepdog.yaw) * self.current_speed * dt + dy = -math.sin(self.sheepdog.yaw) * self.current_speed * dt - # Update velocity - self.sheepdog.velocity = [dx, dy] + # Update velocity (in meters per second) + self.sheepdog.velocity = [self.current_speed * math.cos(self.sheepdog.yaw), + -self.current_speed * math.sin(self.sheepdog.yaw)] # Update position using the sheepdog's set_position method self.sheepdog.set_position(self.sheepdog.x + dx, self.sheepdog.y + dy) \ No newline at end of file diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index d92815a..6e43055 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -1,6 +1,9 @@ import pygame import sys import random +import numpy as np +import yaml +import os from auto_shepherd_simulation.sheep_simulation.sheep import Sheep from auto_shepherd_simulation.sheep_simulation.sheepdog import SheepDog, SheepDogController @@ -15,6 +18,7 @@ GREEN = (34, 139, 34) # Grass color WHITE = (255, 255, 255) GRAY = (128, 128, 128) +LIGHTGREEN = (40, 160, 40) @@ -51,21 +55,137 @@ def handle_event(self, event): self.value = self.min_val + (rel_x / self.rect.width) * (self.max_val - self.min_val) self.handle_rect.x = self.rect.x + rel_x - 5 +class CoordinateTransformer: + def __init__(self, field_boundary, screen_width, screen_height): + """Initialize coordinate transformer with field boundary and screen dimensions + + Args: + field_boundary: List of [x, y] points in meters defining the field boundary + screen_width: Width of the screen in pixels + screen_height: Height of the screen in pixels + """ + self.field_boundary = np.array(field_boundary) + + # Calculate field bounds in real-world coordinates + self.field_min_x = np.min(self.field_boundary[:, 0]) + self.field_max_x = np.max(self.field_boundary[:, 0]) + self.field_min_y = np.min(self.field_boundary[:, 1]) + self.field_max_y = np.max(self.field_boundary[:, 1]) + + # Screen dimensions + self.screen_width = screen_width + self.screen_height = screen_height + + # Calculate scaling factors + self.scale_x = screen_width / (self.field_max_x - self.field_min_x) + self.scale_y = screen_height / (self.field_max_y - self.field_min_y) + + # Use the smaller scale to maintain aspect ratio + self.scale = min(self.scale_x, self.scale_y) + + # Calculate offsets to center the field + self.offset_x = (screen_width - (self.field_max_x - self.field_min_x) * self.scale) / 2 + self.offset_y = (screen_height - (self.field_max_y - self.field_min_y) * self.scale) / 2 + + def world_to_screen(self, x, y): + """Convert real-world coordinates to screen coordinates""" + screen_x = (x - self.field_min_x) * self.scale + self.offset_x + screen_y = (y - self.field_min_y) * self.scale + self.offset_y + return screen_x, screen_y + + def screen_to_world(self, screen_x, screen_y): + """Convert screen coordinates to real-world coordinates""" + x = (screen_x - self.offset_x) / self.scale + self.field_min_x + y = (screen_y - self.offset_y) / self.scale + self.field_min_y + return x, y + + def is_point_in_field(self, x, y): + """Check if a point is inside the field boundary using ray casting algorithm""" + point = np.array([x, y]) + n = len(self.field_boundary) + inside = False + + p1x, p1y = self.field_boundary[0] + for i in range(n + 1): + p2x, p2y = self.field_boundary[i % n] + if y > min(p1y, p2y): + if y <= max(p1y, p2y): + if x <= max(p1x, p2x): + if p1y != p2y: + xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x + if p1x == p2x or x <= xinters: + inside = not inside + p1x, p1y = p2x, p2y + + return inside + + def get_closest_boundary_point(self, x, y): + """Find the closest point on the field boundary to the given point""" + point = np.array([x, y]) + min_dist = float('inf') + closest_point = None + + # Check each line segment of the boundary + for i in range(len(self.field_boundary)): + p1 = self.field_boundary[i] + p2 = self.field_boundary[(i + 1) % len(self.field_boundary)] + + # Vector from p1 to p2 + line_vec = p2 - p1 + # Vector from p1 to point + point_vec = point - p1 + + # Project point_vec onto line_vec + line_len = np.linalg.norm(line_vec) + line_unitvec = line_vec / line_len + projection = np.dot(point_vec, line_unitvec) + + # Clamp projection to line segment + projection = max(0, min(line_len, projection)) + + # Calculate closest point on line segment + closest = p1 + projection * line_unitvec + + # Calculate distance to closest point + dist = np.linalg.norm(point - closest) + + if dist < min_dist: + min_dist = dist + closest_point = closest + + return closest_point + class Simulation: - def __init__(self, width, height, sheep_states=None, sheepdog_state=None): - self.width = width - self.height = height + def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None): + """Initialize simulation with field boundary + + Args: + field_boundary: List of [x, y] points in meters defining the field boundary + screen_width: Width of the screen in pixels + screen_height: Height of the screen in pixels + sheep_states: Optional list of sheep states + sheepdog_state: Optional sheepdog state + """ + self.screen_width = screen_width + self.screen_height = screen_height + + # Initialize coordinate transformer + self.coord_transformer = CoordinateTransformer(field_boundary, screen_width, screen_height) # Create the sheepdog if sheepdog_state is None: + # Place sheepdog at center of field in real-world coordinates + center_x = (self.coord_transformer.field_max_x + self.coord_transformer.field_min_x) / 2 + center_y = (self.coord_transformer.field_max_y + self.coord_transformer.field_min_y) / 2 sheepdog_state = { - 'position': [width/2, height/2], + 'position': [center_x, center_y], 'velocity': [0, 0] } self.sheepdog = SheepDog( position=sheepdog_state['position'], velocity=sheepdog_state['velocity'], - yaw=0 # Initial yaw + yaw=0, # Initial yaw + coord_transformer=self.coord_transformer ) # Create a list of sheep @@ -81,20 +201,23 @@ def __init__(self, width, height, sheep_states=None, sheepdog_state=None): def _initialize_sheep(self, sheep_states=None): """Initialize sheep with given states or random positions""" if sheep_states is None: - # Initialize with random positions + # Initialize with random positions within field boundary for _ in range(self.num_sheep): - margin = 50 - position = [ - random.uniform(margin, self.width - margin), - random.uniform(margin, self.height - margin) - ] - velocity = [0, 0] # Start with zero velocity - self.sheep_list.append(Sheep( - position=position, - velocity=velocity, - width=self.width, - height=self.height - )) + while True: + # Generate random position in real-world coordinates + x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) + y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) + + # Check if position is inside field boundary + if self.coord_transformer.is_point_in_field(x, y): + position = [x, y] + velocity = [0, 0] # Start with zero velocity + self.sheep_list.append(Sheep( + position=position, + velocity=velocity, + coord_transformer=self.coord_transformer + )) + break else: # Initialize with given states for state in sheep_states: @@ -105,37 +228,18 @@ def _initialize_sheep(self, sheep_states=None): self.sheep_list.append(Sheep( position=position, velocity=velocity, - width=self.width, - height=self.height + coord_transformer=self.coord_transformer )) - def update(self, sheepdog_state=None): + def update(self, dt): """Update the simulation state Args: - sheepdog_state: Optional dictionary with 'position' and 'velocity' keys. - If None, the sheepdog's current state is maintained. + dt: Time step in seconds """ - # Update sheepdog state if provided - if sheepdog_state is not None: - if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: - raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") - self.sheepdog.x = sheepdog_state['position'][0] - self.sheepdog.y = sheepdog_state['position'][1] - if 'velocity' in sheepdog_state: - self.sheepdog.velocity = sheepdog_state['velocity'] - self.sheepdog.set_screen_bounds(self.width, self.height) - # Update each sheep for sheep in self.sheep_list: - # Update weights - sheep.alignment_weight = self.alignment_weight - sheep.cohesion_weight = self.cohesion_weight - sheep.separation_weight = self.separation_weight - - # Update position - sheep.flock(self.sheep_list, self.sheepdog) - sheep.update() + sheep.update(dt, self.sheep_list, self.sheepdog) def get_state(self): """Return the current state of the simulation""" @@ -144,34 +248,59 @@ def get_state(self): 'sheepdog': self.sheepdog.get_state() } + def draw(self, screen): + """Draw the simulation state""" + # Fill the screen with grass color + screen.fill(GREEN) + + # Draw field boundary + screen_points = [] + for point in self.coord_transformer.field_boundary: + screen_x, screen_y = self.coord_transformer.world_to_screen(point[0], point[1]) + screen_points.append((screen_x, screen_y)) + pygame.draw.polygon(screen, BLACK, screen_points, 2) + + # Draw each sheep + for sheep in self.sheep_list: + sheep.draw(screen) + + # Draw the sheepdog + self.sheepdog.draw(screen) + class Game: def __init__(self, width, height): # Initialize Pygame pygame.init() - - # Screen setup self.width = width self.height = height self.screen = pygame.display.set_mode((width, height)) - pygame.display.set_caption("Sheep Flocking Simulation") + pygame.display.set_caption("Sheep Simulation") + self.clock = pygame.time.Clock() + self.running = True # Colors self.BLACK = (0, 0, 0) self.GREEN = (34, 139, 34) # Grass color self.WHITE = (255, 255, 255) self.GRAY = (128, 128, 128) + 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') + field_boundary = load_map_config(map_file) - # Create simulation - self.simulation = Simulation(width, height) + # Initialize simulation + self.simulation = Simulation(field_boundary, width, height) - # Create controller for keyboard input + # Initialize sheepdog controller self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) - # Create sliders Slider(x, y, width, height, min_val, max_val, initial_val, name) + # Initialize sliders self.sliders = { - 'alignment': Slider(50, height - 100, 200, 10, 0, 10, 3.0, "Alignment"), # change from 1.0 to 2.0 - 'cohesion': Slider(50, height - 70, 200, 10, 0, 10, 5.0, "Cohesion"), # change from 0.23 to 5.0 - 'separation': Slider(50, height - 40, 200, 10, 0, 10, 6.0, "Separation") # keep at 6.0 + 'alignment': Slider(10, height - 100, 200, 20, 0, 10, 3.0, "Alignment"), + 'cohesion': Slider(10, height - 70, 200, 20, 0, 10, 5.0, "Cohesion"), + 'separation': Slider(10, height - 40, 200, 20, 0, 10, 2.5, "Separation") } # Instructions @@ -197,33 +326,42 @@ def handle_events(self): def update(self): """Update game state""" - # Get keyboard state for continuous movement - keys = pygame.key.get_pressed() + # Get time step in seconds + dt = self.clock.get_time() / 1000.0 # Convert milliseconds to seconds - # Update sheepdog position based on keyboard input - self.sheepdog_controller.update(keys) + # Update simulation + self.simulation.update(dt) - # Get current sheepdog state - sheepdog_state = self.simulation.sheepdog.get_state() + # Update sheepdog controller + keys = pygame.key.get_pressed() + self.sheepdog_controller.update(keys, dt) - # Update simulation with current sheepdog state - self.simulation.update(sheepdog_state) + # Update sliders + for slider in self.sliders.values(): + slider.handle_event(pygame.event.Event(pygame.MOUSEMOTION, {'pos': pygame.mouse.get_pos()})) - # Update simulation parameters from sliders + # Update simulation parameters self.simulation.alignment_weight = self.sliders['alignment'].value self.simulation.cohesion_weight = self.sliders['cohesion'].value self.simulation.separation_weight = self.sliders['separation'].value def draw(self): """Draw the game state""" - # Fill the screen with grass color self.screen.fill(self.GREEN) - # Draw each sheep + # Draw field boundary as a visible polygon + boundary_points = [ + self.simulation.coord_transformer.world_to_screen(x, y) + for x, y in self.simulation.coord_transformer.field_boundary + ] + pygame.draw.polygon(self.screen, self.BLACK, boundary_points, width=4) # Thicker black outline + pygame.draw.polygon(self.screen, self.LIGHTGREEN, boundary_points, width=0) # Fill with light blue for visibility + + # Draw sheep for sheep in self.simulation.sheep_list: sheep.draw(self.screen) - # Draw the sheepdog + # Draw sheepdog self.simulation.sheepdog.draw(self.screen) # Draw sliders @@ -235,23 +373,45 @@ def draw(self): text_surface = self.font.render(text, True, self.WHITE) self.screen.blit(text_surface, (self.width - 250, 20 + i * 25)) - # Update the display pygame.display.flip() def run(self): """Run the game loop""" - clock = pygame.time.Clock() - running = True - - while running: - running = self.handle_events() + while self.running: + self.running = self.handle_events() self.update() self.draw() - clock.tick(60) + self.clock.tick(60) pygame.quit() sys.exit() +def load_map_config(map_file): + """Load map configuration from YAML file and convert coordinates to meters""" + with open(map_file, 'r') as f: + config = yaml.safe_load(f) + + # Convert lat/lon to meters (approximate conversion) + # Using a simple conversion where 1 degree of latitude ≈ 111,320 meters + # and 1 degree of longitude ≈ 111,320 * cos(latitude) meters + boundary_points = [] + for point in config['field_boundary']: + lat = point['latitude'] + lon = point['longitude'] + # Convert to meters relative to the first point + if not boundary_points: + ref_lat = lat + ref_lon = lon + x = 0 + y = 0 + else: + # Convert lat/lon differences to meters + x = (lon - ref_lon) * 111320 * np.cos(np.radians(ref_lat)) + y = (lat - ref_lat) * 111320 + boundary_points.append([x, y]) + + return boundary_points + if __name__ == "__main__": # Create and run the game game = Game(800, 600) From e6690eae078d9087bf2f80d2ddb503823ed3a3e1 Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 09:29:21 +0100 Subject: [PATCH 08/59] map converter tools --- docker/Dockerfile | 45 ++++- docker/docker_start.sh | 6 +- .../utils/geo_converter.py | 176 ++++++++++++++++++ 3 files changed, 219 insertions(+), 8 deletions(-) create mode 100644 ros2_package/auto_shepherd_simulation/utils/geo_converter.py diff --git a/docker/Dockerfile b/docker/Dockerfile index e33067d..d5797fb 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -69,6 +69,32 @@ RUN apt-get update && \ vim nano htop tmux net-tools tree && \ rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get install -y --no-install-recommends \ + libxkbcommon-x11-0 \ + libxcb-icccm4 \ + libxcb-image0 \ + libxcb-keysyms1 \ + libxcb-randr0 \ + libxcb-render-util0 \ + libxcb-xinerama0 \ + libxcb-xinput0 \ + libxcb-xfixes0 \ + libxcb-shape0 \ + libxcb-cursor0 \ + libgl1-mesa-glx \ + libegl1 \ + libxcb-xkb1 \ + libfontconfig1 \ + libfreetype6 \ + xterm \ + # Optional: If you need to debug locale issues with Qt + # locales \ + # locales-all \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + + # 3. Extra ROS debs used by your workspace RUN apt-get update && \ apt-get install -y --no-install-recommends \ @@ -93,15 +119,20 @@ RUN python3 -m pip install --no-cache-dir --upgrade \ pip wheel \ setuptools==58.2.0 \ packaging==24.2 \ - tmule==1.5.9 \ - pyserial \ + numpy \ + scipy \ + matplotlib \ pyproj \ - pygame + pygame \ + scikit-learn \ + opencv-python \ + pyserial \ + tmule==1.5.9 - # ────────────────────────────── - # 7. Drop privileges - # ────────────────────────────── - USER ${USERNAME} +# ────────────────────────────── +# 7. Drop privileges +# ────────────────────────────── +USER ${USERNAME} WORKDIR /home/${USERNAME} # ────────────────────────────── diff --git a/docker/docker_start.sh b/docker/docker_start.sh index fd5b848..6f88f60 100755 --- a/docker/docker_start.sh +++ b/docker/docker_start.sh @@ -4,6 +4,8 @@ COMPOSE_FILE="docker-compose.yml" SERVICE_NAME="auto_shepherd_simulation_ros2_humble" +xhost +local:docker + if [[ "$OSTYPE" == "linux-gnu"* ]]; then HOST_DISPLAY_VAR="$DISPLAY" elif [[ "$OSTYPE" == "darwin"* || "$OSTYPE" == "msys"* || "$OSTYPE" == "win32"* ]]; then @@ -30,7 +32,9 @@ echo "Starting Docker Compose service '${SERVICE_NAME}' in interactive mode..." docker compose -f "${COMPOSE_FILE}" run \ -v "$HOME/.Xauthority:/root/.Xauthority:rw" \ -e "HOST_DISPLAY_VAR=${HOST_DISPLAY_VAR}" \ + -e QT_X11_NO_MITSHM=1 \ "${DOCKER_RUN_ARGS[@]}" \ - "${SERVICE_NAME}" bash + "${SERVICE_NAME}" \ + bash echo "Docker container session ended." diff --git a/ros2_package/auto_shepherd_simulation/utils/geo_converter.py b/ros2_package/auto_shepherd_simulation/utils/geo_converter.py new file mode 100644 index 0000000..7e97ec8 --- /dev/null +++ b/ros2_package/auto_shepherd_simulation/utils/geo_converter.py @@ -0,0 +1,176 @@ +import math +from typing import List, Tuple, Dict, Optional +from pyproj import CRS, Transformer +import yaml +import os + +# --- Global Configuration (for UTM projection) --- +# UTM Zone for Lincoln, UK is typically Zone 30N. +# EPSG:4326 is WGS84 Geographic (latitude, longitude) +# EPSG:32630 is WGS 84 / UTM Zone 30N (Easting, Northing in meters) +WGS84_CRS = CRS("EPSG:4326") +UTM_CRS = CRS("EPSG:32630") # WGS 84 / UTM Zone 30N +_transformer_wgs84_to_utm = Transformer.from_crs(WGS84_CRS, UTM_CRS, always_xy=False) +_transformer_utm_to_wgs84 = Transformer.from_crs(UTM_CRS, WGS84_CRS, always_xy=False) + +class MapConverter: + """ + A utility class to handle coordinate conversions (LatLong <-> Meters) + relative to a fixed origin, defined by the top-left corner of a map's bounding box. + """ + def __init__(self, map_coords_latlon: List[Tuple[float, float]]): + if not map_coords_latlon: + raise ValueError("map_coords_latlon cannot be empty to define a map.") + + min_lat = min(c[0] for c in map_coords_latlon) + max_lat = max(c[0] for c in map_coords_latlon) + min_lon = min(c[1] for c in map_coords_latlon) + max_lon = max(c[1] for c in map_coords_latlon) + + self.bounding_box_corners_latlon = { + 'top_left': (max_lat, min_lon), + 'top_right': (max_lat, max_lon), + 'bottom_left': (min_lat, min_lon), + 'bottom_right': (min_lat, max_lon) + } + + self.origin_lat = self.bounding_box_corners_latlon['top_left'][0] + self.origin_lon = self.bounding_box_corners_latlon['top_left'][1] + + self.origin_utm_y, self.origin_utm_x = _transformer_wgs84_to_utm.transform(self.origin_lat, self.origin_lon) + + self.map_coords_xy_meters: List[Tuple[float, float]] = [] + for lat, lon in map_coords_latlon: + utm_y, utm_x = _transformer_wgs84_to_utm.transform(lat, lon) + + relative_x = utm_x - self.origin_utm_x + relative_y = utm_y - self.origin_utm_y + self.map_coords_xy_meters.append((relative_x, relative_y)) + + print(f"MapConverter initialized. Origin (Lat, Lon): ({self.origin_lat:.7f}, {self.origin_lon:.7f})") + print(f"MapConverter Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") + + def get_map_data(self) -> Dict[str, any]: + return { + 'bounding_box_corners_latlon': self.bounding_box_corners_latlon, + 'map_coords_xy_meters': self.map_coords_xy_meters, + 'origin_lat': self.origin_lat, + 'origin_lon': self.origin_lon, + 'origin_utm_x': self.origin_utm_x, + 'origin_utm_y': self.origin_utm_y, + } + + def latlon_to_xy(self, lat: float, lon: float) -> Tuple[float, float]: + utm_y, utm_x = _transformer_wgs84_to_utm.transform(lat, lon) + relative_x = utm_x - self.origin_utm_x + relative_y = utm_y - self.origin_utm_y + return (relative_x, relative_y) + + def xy_to_latlon(self, x_meter: float, y_meter: float) -> Tuple[float, float]: + abs_utm_x = x_meter + self.origin_utm_x + abs_utm_y = y_meter + self.origin_utm_y + lat, lon = _transformer_utm_to_wgs84.transform(abs_utm_y, abs_utm_x) + return (lat, lon) + + +# --- Helper Function to Load from YAML --- +def load_coords_from_yaml(yaml_file_path: str) -> List[Tuple[float, float]]: + """ + Loads latitude/longitude coordinates from a YAML file. + Assumes YAML structure: + field_boundary: + - latitude: float + longitude: float + """ + if not os.path.exists(yaml_file_path): + raise FileNotFoundError(f"YAML file not found: {yaml_file_path}") + + try: + with open(yaml_file_path, 'r') as file: + data = yaml.safe_load(file) + + if 'field_boundary' not in data or not isinstance(data['field_boundary'], list): + raise ValueError("YAML file must contain a 'field_boundary' list.") + + coords = [] + for item in data['field_boundary']: + if 'latitude' in item and 'longitude' in item: + coords.append((float(item['latitude']), float(item['longitude']))) + else: + print(f"Warning: Skipping malformed point in YAML: {item}") + return coords + except yaml.YAMLError as e: + raise ValueError(f"Error parsing YAML file: {e}") + except Exception as e: + raise ValueError(f"An unexpected error occurred while loading YAML: {e}") + + +if __name__ == "__main__": + # Example for the path you previously mentioned: + 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 + + + print("\n--- Case 1: Create Map Bounding Box & Convert All Coords ---") + try: + map_converter = MapConverter(field_coords_latlon) + map_data = map_converter.get_map_data() + + print("\nMap Data (from Case 1):") + print(f" Bounding Box Corners (Lat, Lon):") + for k, v in map_data['bounding_box_corners_latlon'].items(): + print(f" {k}: ({v[0]:.7f}, {v[1]:.7f})") + + print(f" Origin (Lat, Lon): ({map_data['origin_lat']:.7f}, {map_data['origin_lon']:.7f})") + print(f" Origin (UTM X, Y): ({map_data['origin_utm_x']:.3f}, {map_data['origin_utm_y']:.3f})") + + print("\n Converted Map Coords (relative X, Y meters): (showing first 5)") + for i in range(min(5, len(map_data['map_coords_xy_meters']))): # Print first 5 for brevity + x, y = map_data['map_coords_xy_meters'][i] + print(f" Point {i}: X={x:.3f} m, Y={y:.3f} m") + if len(map_data['map_coords_xy_meters']) > 5: + print(f" ... and {len(map_data['map_coords_xy_meters'])-5} more points.") + + except ValueError as e: + print(f"Error during Case 1: {e}") + map_converter = None # Ensure map_converter is not set if initialization failed + + + if map_converter: + print("\n--- Case 2: Convert New LatLong Point to XY ---") + new_lat_lon = (53.264500000, -0.532500000) # A point somewhere near your field + try: + new_x, new_y = map_converter.latlon_to_xy(new_lat_lon[0], new_lat_lon[1]) + print(f"New LatLong point ({new_lat_lon[0]:.7f}, {new_lat_lon[1]:.7f}) converts to X={new_x:.3f} m, Y={new_y:.3f} m") + except Exception as e: + print(f"Error during Case 2: {e}") + + + print("\n--- Case 3: Convert XY Point in Bounding Box to LatLong ---") + # Let's take the very first converted point from the map for testing + if map_data['map_coords_xy_meters']: # Ensure there are points + test_x_meter, test_y_meter = map_data['map_coords_xy_meters'][0] + try: + converted_lat, converted_lon = map_converter.xy_to_latlon(test_x_meter, test_y_meter) + print(f"XY point ({test_x_meter:.3f} m, {test_y_meter:.3f} m) converts to Lat={converted_lat:.7f}, Lon={converted_lon:.7f}") + + # Verify accuracy by comparing with original first point from field_coords_latlon + original_first_map_lat, original_first_map_lon = field_coords_latlon[0] + print(f"Original LatLong of this point: ({original_first_map_lat:.7f}, {original_first_map_lon:.7f})") + print(f"Difference (Lat): {abs(original_first_map_lat - converted_lat):.10f}") + print(f"Difference (Lon): {abs(original_first_map_lon - converted_lon):.10f}") + + except Exception as e: + print(f"Error during Case 3: {e}") + else: + print("No map_coords_xy_meters available for Case 3 test.") + else: + print("\nSkipping Case 2 and 3 tests because MapConverter initialization failed.") From e2c722b9a27e67e3a9fe497b267848faf44d7af9 Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Thu, 19 Jun 2025 09:38:00 +0100 Subject: [PATCH 09/59] sim and sheepdog integration --- .../auto_shepherd_simulation/dog_control.py | 13 +++--- .../dog_control_simulator.py | 43 +++++++++++++------ .../auto_shepherd_simulation/ros_interface.py | 15 +++---- 3 files changed, 45 insertions(+), 26 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 6394d72..92c4d12 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -1,6 +1,6 @@ import rclpy from rclpy.node import Node -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, Point, Quaternion from nav_msgs.msg import Path from std_msgs.msg import Float64MultiArray import numpy as np @@ -13,7 +13,7 @@ def __init__(self): # publishers / subscribers --------------------------------------- self.cmd_pub = self.create_publisher( - Float64MultiArray, '/dog/command', 10) # :contentReference[oaicite:2]{index=2} + PoseStamped, '/dog/command', 10) # :contentReference[oaicite:2]{index=2} self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, 10) # :contentReference[oaicite:3]{index=3} @@ -57,9 +57,12 @@ def _control_step(self): xd_opt, yd_opt, points = find_best_dog_position(xs, ys, xd, yd, xc, yc) - cmd = Float64MultiArray() - cmd.data = [float(xd_opt), float(yd_opt)] - self.cmd_pub.publish(cmd) + ps = PoseStamped() + ps.header.stamp = self.get_clock().now().to_msg() + ps.header.frame_id = "map" # or any frame you prefer + ps.pose.position = Point(x=float(xd_opt), y=float(yd_opt), z=0.0) + ps.pose.orientation = Quaternion(w=1.0) # identity; adjust if needed + self.cmd_pub.publish(ps) self.get_logger().debug(f'Cmd ({xd_opt:.2f}, {yd_opt:.2f})') # plot diff --git a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py index c9a3fb8..e361663 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py +++ b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py @@ -17,8 +17,10 @@ def __init__(self): # Default the storage points for the animal data self.sheep_poses = {} # {sheep_name: [dict, dict, ...]} - self.dog_poses = {} # {timestep: {dog_name: PoseStamped}} + # self.dog_poses = {} # {timestep: {dog_name: PoseStamped}} + self.dog_state = None self.dog_command = None # Store the latest dog command + self.simulation = None # build later when we get data # Create QoS profile qos_profile = QoSProfile( @@ -28,7 +30,7 @@ def __init__(self): ) # Subscribe to input data - self.create_subscription(Path, '/dog/pose', self._dog_cb, qos_profile) + self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, qos_profile) self.create_subscription(Path, '/sheep/poses', self._sheep_cb, qos_profile) self.create_subscription(PoseStamped, '/dog/command', self._dog_command_cb, qos_profile, callback_group=RCG()) @@ -37,12 +39,9 @@ def __init__(self): self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) self.sheep_sim_pub = self.create_publisher(Path, '/sheep/poses_sim', qos_profile) - self.simulation = Simulation(800, 600, sheep_states=None, sheepdog_state=None) - self.sim_step_timer = self.create_timer(0.1, self.run_sim_step, callback_group=RCG()) - print('Dog Control Simulator Initialised') - + # print('Dog Control Simulator Initialised') def _dog_command_cb(self, msg): @@ -57,15 +56,20 @@ def _dog_command_cb(self, msg): self.get_logger().info(f'Received new dog command: {self.dog_command}') def _dog_cb(self, msg): - timestep = msg.header.secs - dog_name = msg.header.frame_id or 'dog' + # timestep = msg.header.secs + # dog_name = msg.header.frame_id or 'dog' # Initialise storage - if timestep not in self.dog_poses: - self.dog_poses[timestep] = dict() + # if timestep not in self.dog_poses: + # self.dog_poses[timestep] = dict() # Save dog position at timestep - self.dog_poses[timestep][dog_name] = msg + # self.dog_poses[timestep][dog_name] = msg + + self.dog_state = { + 'position': [msg.pose.position.x, msg.pose.position.y], + 'velocity': [0.0, 0.0] # you can compute real velocity later + } def _sheep_cb(self, msg): # Get current timestep @@ -90,6 +94,19 @@ def _sheep_cb(self, msg): current['velocity'] = [pos.x - px, pos.y - py] self.sheep_poses[name].append(current) + print("sheep poses ready") + print(self.simulation, self.dog_state) + # ------------- create Simulation once ----------------------------- + if self.simulation is None and self.dog_state is not None: + # flatten current sheep snapshot into list of dicts + sheep_states = [hist[-1] for hist in self.sheep_poses.values()] + self.simulation = Simulation( + 800, 600, + sheep_states=sheep_states, + sheepdog_state=self.dog_state + ) + self.get_logger().info('Simulation initialised with ' + f'{len(sheep_states)} sheep.') def publish_simulation_state(self, state): """Convert simulation state to MarkerArray and publish for RViz visualization""" @@ -214,10 +231,10 @@ def publish_sheep_path(self, state): def run_sim_step(self, timestep=None): - + if self.simulation is None: # wait until we have initialised it + return # Execute simulation step - self.simulation.update(self.dog_command) sheep_estimates = self.simulation.get_state() # Publish visualization markers diff --git a/ros2_package/auto_shepherd_simulation/ros_interface.py b/ros2_package/auto_shepherd_simulation/ros_interface.py index 37d5b9a..a08fd89 100644 --- a/ros2_package/auto_shepherd_simulation/ros_interface.py +++ b/ros2_package/auto_shepherd_simulation/ros_interface.py @@ -13,7 +13,7 @@ def __init__(self): self.dog_publisher = self.create_publisher(PoseStamped, '/dog/pose', 10) self.sheep_publisher = self.create_publisher(Path, '/sheep/poses', 10) self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/sheep/goal_pose', 10) - self.dog_command_subscription = self.create_subscription(Float64MultiArray, '/dog/command', self.dog_command_callback, 10) + self.dog_command_subscription = self.create_subscription(PoseStamped, '/dog/command', self.dog_command_callback, 10) self.timer = self.create_timer(0.1, self.timer_callback) def create_header(self): @@ -54,13 +54,12 @@ def timer_callback(self): self.sheep_goal_publisher.publish(sheep_goal_pose) def dog_command_callback(self, msg): - # Handle incoming dog command - if len(msg.data) >= 2: - target_x = msg.data[0] - target_y = msg.data[1] - self.get_logger().info(f'Received dog command: Move to ({target_x}, {target_y})') - else: - self.get_logger().warn('Received invalid dog command format') + target_x = msg.pose.position.x + target_y = msg.pose.position.y + # orientation is already in quaternion form if you need it + self.get_logger().info( + f"Received dog command: Move to ({target_x:.2f}, {target_y:.2f})" + ) def main(args=None): rclpy.init(args=args) From 011a88573d632e0d42a0bb167db0ad21c3e3d6d0 Mon Sep 17 00:00:00 2001 From: James Date: Thu, 19 Jun 2025 09:42:24 +0100 Subject: [PATCH 10/59] Simulation updates --- .gitignore | 2 ++ .../sheep_simulation/simulation.py | 15 ++++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index c6b757c..b0ea4c8 100644 --- a/.gitignore +++ b/.gitignore @@ -75,3 +75,5 @@ crashlytics-build.properties *.swp *~ + +*.pyc diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 6e43055..41f388c 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -231,12 +231,25 @@ def _initialize_sheep(self, sheep_states=None): coord_transformer=self.coord_transformer )) - def update(self, dt): + def update(self, dt=0.02, sheepdog_state=None): """Update the simulation state Args: dt: Time step in seconds + sheepdog_state: Optional dictionary with 'position' and 'velocity' keys. + If None, the sheepdog's current state is maintained. """ + + # Update sheepdog state if provided + if sheepdog_state is not None: + if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: + raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") + self.sheepdog.x = sheepdog_state['position'][0] + self.sheepdog.y = sheepdog_state['position'][1] + if 'velocity' in sheepdog_state: + self.sheepdog.velocity = sheepdog_state['velocity'] + self.sheepdog.set_screen_bounds(self.width, self.height) + # Update each sheep for sheep in self.sheep_list: sheep.update(dt, self.sheep_list, self.sheepdog) From c57fda36337b542cc505ae13b6b41294e57eb110 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 09:45:51 +0100 Subject: [PATCH 11/59] Added check that dog positions are within the field polygon --- .../auto_shepherd_simulation/tmpDogSim/dog_control_lib.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 082f749..affd027 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -2,8 +2,14 @@ import math as maths import cv2 import matplotlib.pyplot as plt +from matplotlib.path import Path import numpy as np from auto_shepherd_simulation.sheep_simulation.simulation import Simulation +from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter + +mc = MapConverter(load_coords_from_yaml("../configs/map/map1.yaml")) +map_polygon = Path(np.array(mc.map_coords_xy_meters)) +#check if points are valid using `map_polygon.contains_point(point)` def smallest_enclosing_circle(points): center = np.mean(points, axis=0) @@ -77,6 +83,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal last_update = 0 optimal_xd, optimal_yd, optimal_cost = xd, yd, cost(x, y, xd, yd, xc, yc, simulation) for i, (new_xd, new_yd) in enumerate(points): + if not map_polygon.contains_point((new_xd, new_yd)): continue new_cost = cost(x, y, new_xd, new_yd, xc, yc, simulation) if new_cost < optimal_cost: optimal_cost = new_cost From 012144ff71cad246337b1d0200f2d4c68de26b24 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 09:52:41 +0100 Subject: [PATCH 12/59] Ensured the dog stays in the field even if the sheep don't --- .../auto_shepherd_simulation/tmpDogSim/dog_control_lib.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index affd027..cd8b48f 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -66,6 +66,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal 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 + if not map_polygon.contains_point((optimal_xd, optimal_yd)): optimal_xd, optimal_yd = xd, yd else: # get points around sheep angle_a = np.arccos((radius_sheep**2 + d**2 - radius_d**2) / (2 * radius_sheep * d)) From 505d0dd74b66c3f7c6b3ae2847c4be2398786b2b Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Thu, 19 Jun 2025 10:08:03 +0100 Subject: [PATCH 13/59] fix sheepdog movement --- .../auto_shepherd_simulation/dog_control.py | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 92c4d12..86ceb27 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -24,6 +24,7 @@ def __init__(self): # 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) @@ -45,17 +46,24 @@ def _goal_cb(self, msg: PoseStamped): def _control_step(self): print("_control_step") - if self.dog_xy is None or self.sheep_xy is None or self.goal_xy is None: - print(self.dog_xy) - print(self.sheep_xy) - print(self.goal_xy) - return # wait for all data + # make sure we have the three inputs we need + if self.sheep_xy is None or self.goal_xy is None: + 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 + # --------------------------------------------- + if self._planned_dog_xy is None: + xd_start, yd_start = self.dog_xy # FIRST call → live pose + else: + xd_start, yd_start = self._planned_dog_xy # LATER calls → last plan xs, ys = self.sheep_xy[:, 0], self.sheep_xy[:, 1] - xd, yd = self.dog_xy xc, yc = self.goal_xy - xd_opt, yd_opt, points = find_best_dog_position(xs, ys, xd, yd, xc, yc) + xd_opt, yd_opt, points = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc) ps = PoseStamped() ps.header.stamp = self.get_clock().now().to_msg() @@ -65,6 +73,8 @@ def _control_step(self): self.cmd_pub.publish(ps) self.get_logger().debug(f'Cmd ({xd_opt:.2f}, {yd_opt:.2f})') + self._planned_dog_xy = (xd_opt, yd_opt) + # plot # plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt, points) From ddaf261aab27f829f46e7a9f5a4d8e5a5cf39fb4 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 10:24:08 +0100 Subject: [PATCH 14/59] find_best_dog_position no longer returns sampled points --- ros2_package/auto_shepherd_simulation/dog_control.py | 4 ++-- .../auto_shepherd_simulation/tmpDogSim/dog_control_lib.py | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 86ceb27..3f03cab 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -63,7 +63,7 @@ def _control_step(self): xs, ys = self.sheep_xy[:, 0], self.sheep_xy[:, 1] xc, yc = self.goal_xy - xd_opt, yd_opt, points = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc) + xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc) ps = PoseStamped() ps.header.stamp = self.get_clock().now().to_msg() @@ -76,7 +76,7 @@ def _control_step(self): self._planned_dog_xy = (xd_opt, yd_opt) # plot - # plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt, points) + # plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt) # ---------------------------------------------------------------------- diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index cd8b48f..a6a9cb1 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -92,9 +92,9 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal last_update += 1 print(f"Best Cost: {optimal_cost}") if i-last_update > early_exit_threshold: break - return optimal_xd, optimal_yd, points + return optimal_xd, optimal_yd -def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, points, radius_d=1.5): +def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, radius_d=1.5): (xmean, ymean), radius_sheep = smallest_enclosing_circle(np.stack([x, y], axis=1)) fig, ax = plt.subplots() @@ -119,7 +119,6 @@ def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, points, rad circle_b = plt.Circle((xmean, ymean), radius_sheep, color='b', fill=False, label='Circle B') ax.add_patch(circle_a) ax.add_patch(circle_b) - plt.scatter(points[:,0], points[:,1]) plt.scatter([optimal_xd], [optimal_yd], s=100, alpha=0.5, c="r") From 016e6512745942c694e821d7ab36d067e9ec5582 Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 10:47:56 +0100 Subject: [PATCH 15/59] Dockernumpy version fix --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index d5797fb..1eaf5fe 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -119,7 +119,7 @@ RUN python3 -m pip install --no-cache-dir --upgrade \ pip wheel \ setuptools==58.2.0 \ packaging==24.2 \ - numpy \ + numpy==1.23.5 \ scipy \ matplotlib \ pyproj \ From 56cc1c7b882870cfffde34248929caf471197bb5 Mon Sep 17 00:00:00 2001 From: James Date: Thu, 19 Jun 2025 10:50:51 +0100 Subject: [PATCH 16/59] Sheep fencing in ROS --- .../dog_control_simulator.py | 38 +++++++++++++++++-- .../sheep_simulation/simulation.py | 12 +++--- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py index e361663..d2ae141 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py +++ b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py @@ -10,6 +10,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup as RCG from auto_shepherd_simulation.sheep_simulation.simulation import Simulation +from auto_shepherd_simulation.utils.geo_converter import MapConverter, load_coords_from_yaml class DogControlSimulator(Node): def __init__(self): @@ -39,9 +40,39 @@ def __init__(self): self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) self.sheep_sim_pub = self.create_publisher(Path, '/sheep/poses_sim', qos_profile) - self.sim_step_timer = self.create_timer(0.1, self.run_sim_step, callback_group=RCG()) - # print('Dog Control Simulator Initialised') + 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: + map_converter = MapConverter(field_coords_latlon) + map_data = map_converter.get_map_data() + + field_boundary = map_data['map_coords_xy_meters'] + + except ValueError as e: + print(f"Error during map conversion: {e}") + map_converter = None # Ensure map_converter is not set if initialization failed + + + map_converter = MapConverter + + self.simulation = Simulation(field_boundary, 800, 600, sheep_states=None, sheepdog_state=None) + self.dt = 0.05 + self.sim_step_timer = self.create_timer(0.05, self.run_sim_step, callback_group=RCG()) + + print('Dog Control Simulator Initialised') + def _dog_command_cb(self, msg): @@ -235,8 +266,9 @@ def run_sim_step(self, timestep=None): return # Execute simulation step - self.simulation.update(self.dog_command) + self.simulation.update(self.dt, self.dog_command) sheep_estimates = self.simulation.get_state() + # Publish visualization markers self.publish_simulation_state(sheep_estimates) # Publish sheep path diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 41f388c..1505454 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -201,6 +201,7 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st def _initialize_sheep(self, sheep_states=None): """Initialize sheep with given states or random positions""" if sheep_states is None: + print('random sheep init') # Initialize with random positions within field boundary for _ in range(self.num_sheep): while True: @@ -244,11 +245,12 @@ def update(self, dt=0.02, sheepdog_state=None): if sheepdog_state is not None: if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") - self.sheepdog.x = sheepdog_state['position'][0] - self.sheepdog.y = sheepdog_state['position'][1] + + self.sheepdog.set_position(sheepdog_state['position'][0], sheepdog_state['position'][1]) + if 'velocity' in sheepdog_state: self.sheepdog.velocity = sheepdog_state['velocity'] - self.sheepdog.set_screen_bounds(self.width, self.height) + # Update each sheep for sheep in self.sheep_list: @@ -299,8 +301,8 @@ 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 = 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) # Initialize simulation From 49d8cee1b9e9b72a8ebd5e4c11754161a6380337 Mon Sep 17 00:00:00 2001 From: James Date: Thu, 19 Jun 2025 10:58:43 +0100 Subject: [PATCH 17/59] Merge compatibility --- .../auto_shepherd_simulation/dog_control_simulator.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py index d2ae141..5ff0ed3 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py +++ b/ros2_package/auto_shepherd_simulation/dog_control_simulator.py @@ -58,16 +58,14 @@ def __init__(self): map_converter = MapConverter(field_coords_latlon) map_data = map_converter.get_map_data() - field_boundary = map_data['map_coords_xy_meters'] + self.field_boundary = map_data['map_coords_xy_meters'] except ValueError as e: print(f"Error during map conversion: {e}") map_converter = None # Ensure map_converter is not set if initialization failed - map_converter = MapConverter - - self.simulation = Simulation(field_boundary, 800, 600, sheep_states=None, sheepdog_state=None) + self.simulation = Simulation(self.field_boundary, 800, 600, sheep_states=None, sheepdog_state=None) self.dt = 0.05 self.sim_step_timer = self.create_timer(0.05, self.run_sim_step, callback_group=RCG()) @@ -132,6 +130,7 @@ def _sheep_cb(self, msg): # flatten current sheep snapshot into list of dicts sheep_states = [hist[-1] for hist in self.sheep_poses.values()] self.simulation = Simulation( + self.field_boundary, 800, 600, sheep_states=sheep_states, sheepdog_state=self.dog_state From 4c7daaaad079d003348570901cac5bf20c59bb5d Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 11:05:05 +0100 Subject: [PATCH 18/59] corrected mapper node to use the new utils mapper tool --- .../auto_shepherd_simulation/mapper/mapper.py | 176 ++++++++---------- 1 file changed, 81 insertions(+), 95 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/mapper/mapper.py b/ros2_package/auto_shepherd_simulation/mapper/mapper.py index e5ad371..406740e 100644 --- a/ros2_package/auto_shepherd_simulation/mapper/mapper.py +++ b/ros2_package/auto_shepherd_simulation/mapper/mapper.py @@ -1,22 +1,20 @@ import rclpy from rclpy.node import Node -# from geometry_msgs.msg import PolygonStamped, Point32 # No longer used for main path -from nav_msgs.msg import Path # New: Import Path message -from geometry_msgs.msg import PoseStamped, Point, Quaternion # PoseStamped, Point (for position), Quaternion (for orientation) +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Point, Quaternion import yaml import os import tf2_ros from geometry_msgs.msg import TransformStamped from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +from tf_transformations import quaternion_from_euler +from typing import List, Tuple -# For UTM conversion -from pyproj import CRS, Transformer -from tf_transformations import quaternion_from_euler # Helper for creating quaternions +from auto_shepherd_simulation.utils.geo_converter import MapConverter, load_coords_from_yaml class MapperNode(Node): def __init__(self): super().__init__('mapper_node') - self.declare_parameter('map_file_path', '/home/ros/map/map1.yaml') # Define the QoS profile for a latched topic @@ -26,144 +24,133 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL ) - # Change publisher type to Path + # Publisher for the Path message self.publisher_ = self.create_publisher( Path, - 'FieldBoundaryPath', # New topic name for clarity + 'FieldBoundaryPath', qos_profile ) + # Static Transform Broadcaster for the 'field_frame' self.static_broadcaster = tf2_ros.StaticTransformBroadcaster(self) - # UTM Conversion setup (for Lincoln, UK - UTM Zone 30N) - wgs84_crs = CRS("EPSG:4326") # Latitude, Longitude - utm30n_crs = CRS("EPSG:32630") # WGS 84 / UTM Zone 30N (meters) - self.transformer = Transformer.from_crs(wgs84_crs, utm30n_crs) + # --- Integration with geo_converter.py --- + + # 1. Get the map file path from parameters + map_file_path = self.get_parameter('map_file_path').get_parameter_value().string_value + self.get_logger().info(f"Attempting to load map for MapperNode from: {map_file_path}") + + # 2. Load raw (lat, lon) coordinates from the YAML file using the utility function + try: + raw_latlon_coords = load_coords_from_yaml(map_file_path) + if not raw_latlon_coords: + self.get_logger().error("Loaded map data is empty. Shutting down.") + rclpy.shutdown() + return + except (FileNotFoundError, ValueError) as e: + self.get_logger().error(f"Failed to load map data from YAML: {e}. Shutting down.") + rclpy.shutdown() + return + + # 3. Initialize the MapConverter with the raw LatLon coordinates + try: + self.map_converter = MapConverter(raw_latlon_coords) + map_data = self.map_converter.get_map_data() # Get processed data from the converter + + # Store the map's calculated origin (in UTM) + self.origin_utm_x = map_data['origin_utm_x'] + self.origin_utm_y = map_data['origin_utm_y'] + + # Get the relative X, Y meters for the path + self.path_xy_meters = map_data['map_coords_xy_meters'] - self.polygon_data = self.load_map_from_yaml() - if not self.polygon_data: - self.get_logger().error("Failed to load map data from specified path. Shutting down.") + self.get_logger().info(f"Map converted. Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") + + except ValueError as e: + self.get_logger().error(f"Error initializing MapConverter: {e}. Shutting down.") rclpy.shutdown() return - # Perform UTM conversion on loaded data - self.utm_poses = self.convert_to_utm_poses() - if not self.utm_poses: - self.get_logger().error("Failed to convert polygon data to UTM poses. Shutting down.") + # 4. Create PoseStamped messages for the Path from the relative meter coordinates + self.path_poses = self._create_path_poses(self.path_xy_meters) + if not self.path_poses: + self.get_logger().error("Failed to create Path poses from converted data. Shutting down.") rclpy.shutdown() return + # --- Publish Data --- + # Publish the Path message immediately after loading and converting self.publish_path_once() - # Publish the static transform after the map data is ready + # Publish the static transform for 'field_frame' self.publish_static_transform() self.get_logger().info("Map published once as Path. Static transform published. Node will now spin indefinitely.") - def load_map_from_yaml(self): - map_file_path = self.get_parameter('map_file_path').get_parameter_value().string_value - self.get_logger().info(f"Attempting to load map from: {map_file_path}") - - if not os.path.exists(map_file_path): - self.get_logger().error(f"Map file not found at: {map_file_path}") - return None - - try: - with open(map_file_path, 'r') as file: - data = yaml.safe_load(file) - if 'field_boundary' in data and isinstance(data['field_boundary'], list): - return data.get('field_boundary') - else: - self.get_logger().error("YAML file does not contain a 'field_boundary' list under the 'field_boundary' key.") - return None - except yaml.YAMLError as e: - self.get_logger().error(f"Error parsing YAML file: {e}") - return None - except Exception as e: - self.get_logger().error(f"An unexpected error occurred while loading map: {e}") - return None - - def convert_to_utm_poses(self): - if not self.polygon_data: - return [] - - converted_poses = [] - # Calculate a local origin for the 'field_frame' based on the first point's UTM coordinates - # This makes the path appear around (0,0) in RViz's 'field_frame' - # while 'field_frame' itself is transformed to its true UTM location in 'map' frame. - first_lat = float(self.polygon_data[0]['latitude']) - first_lon = float(self.polygon_data[0]['longitude']) - self.origin_utm_x, self.origin_utm_y = self.transformer.transform(first_lat, first_lon) - self.get_logger().info(f"UTM Origin for 'field_frame' set to: ({self.origin_utm_x}, {self.origin_utm_y})") - - + 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. + """ + poses = [] # Create an identity quaternion for the poses (no rotation) - # tf_transformations.quaternion_from_euler(roll, pitch, yaw) - # (0,0,0) means no rotation. q = quaternion_from_euler(0, 0, 0) identity_orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + current_stamp = self.get_clock().now().to_msg() # Use a single stamp for all poses in this path - for point_data in self.polygon_data: - lat = float(point_data['latitude']) - lon = float(point_data['longitude']) - - utm_x, utm_y = self.transformer.transform(lat, lon) - + for x_m, y_m in xy_meters: pose_stamped = PoseStamped() - pose_stamped.header.stamp = self.get_clock().now().to_msg() + pose_stamped.header.stamp = current_stamp pose_stamped.header.frame_id = 'field_frame' # Poses are relative to 'field_frame' - - # Position in meters, relative to the chosen origin of 'field_frame' - pose_stamped.pose.position.x = utm_x - self.origin_utm_x - pose_stamped.pose.position.y = utm_y - self.origin_utm_y + pose_stamped.pose.position.x = x_m + pose_stamped.pose.position.y = y_m pose_stamped.pose.position.z = 0.0 # Assuming 2D path + pose_stamped.pose.orientation = identity_orientation + poses.append(pose_stamped) - pose_stamped.pose.orientation = identity_orientation # No rotation - - converted_poses.append(pose_stamped) - - # To close the loop for the path visualization in RViz - if converted_poses: - first_pose = converted_poses[0] - # Create a new PoseStamped with updated stamp if needed, or just append the first one - # Appending the first one directly is common for closing a loop. + # Close the loop for the path visualization in RViz by adding the first point again + if poses: + # Create a new PoseStamped to avoid modifying the original first pose object closed_loop_pose = PoseStamped() - closed_loop_pose.header.stamp = self.get_clock().now().to_msg() + closed_loop_pose.header.stamp = current_stamp closed_loop_pose.header.frame_id = 'field_frame' - closed_loop_pose.pose = first_pose.pose - converted_poses.append(closed_loop_pose) - - - return converted_poses + closed_loop_pose.pose.position.x = poses[0].pose.position.x + closed_loop_pose.pose.position.y = poses[0].pose.position.y + closed_loop_pose.pose.position.z = poses[0].pose.position.z + closed_loop_pose.pose.orientation = poses[0].pose.orientation + poses.append(closed_loop_pose) + return poses def publish_path_once(self): - if not self.utm_poses: - self.get_logger().warn("No UTM poses to publish for Path.") + """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 in the 'map' frame + path_msg.header.frame_id = 'map' # The Path itself is defined in the 'map' frame - path_msg.poses = self.utm_poses # Assign the list of PoseStamped messages + path_msg.poses = self.path_poses self.publisher_.publish(path_msg) self.get_logger().info(f'Published FieldBoundaryPath with {len(path_msg.poses)} poses (once, latched).') - def publish_static_transform(self): + """Publishes the static transform from 'map' to 'field_frame'.""" t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'map' t.child_frame_id = 'field_frame' - # This transform places 'field_frame' at the UTM coordinates of your first map point + # Set the translation to the absolute UTM coordinates of the bounding box origin. + # This places 'field_frame' (and thus the relative Path) at its real-world location in 'map'. t.transform.translation.x = self.origin_utm_x t.transform.translation.y = self.origin_utm_y - t.transform.translation.z = 0.0 + t.transform.translation.z = 0.0 # Assuming 2D environment q = quaternion_from_euler(0, 0, 0) # Identity rotation for the frame itself t.transform.rotation.x = q[0] @@ -172,8 +159,7 @@ def publish_static_transform(self): t.transform.rotation.w = q[3] self.static_broadcaster.sendTransform(t) - self.get_logger().info(f"Published static transform from '{t.header.frame_id}' to '{t.child_frame_id}' at UTM origin.") - + self.get_logger().info(f"Published static transform from '{t.header.frame_id}' to '{t.child_frame_id}' at bounding box UTM origin.") def main(args=None): rclpy.init(args=args) From 484ceaf408b680a3df4a8c12f88a70f2539c7019 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 11:38:40 +0100 Subject: [PATCH 19/59] Added default dog position to go to when surronded --- .../tmpDogSim/dog_control_lib.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index a6a9cb1..04cc6a2 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -56,17 +56,22 @@ def cost(x, y, xd, yd, xc, yc, simulation): def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal - radius_d=1.5, n_candidates=30, early_exit_threshold=10): + radius_d=1.5, n_candidates=15, early_exit_threshold=5, + default_goto=np.asarray((0,0))): """Return optimal dog (x_d*, y_d*) given current flock and goal.""" (xmean, ymean), radius_sheep = smallest_enclosing_circle(np.stack([x, y], axis=1)) radius_sheep += .5 d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) - if d > radius_d + radius_sheep or d < abs(radius_d - radius_sheep): + if d > radius_d + radius_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 - if not map_polygon.contains_point((optimal_xd, optimal_yd)): optimal_xd, optimal_yd = xd, yd + elif d < abs(radius_d - radius_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 else: # get points around sheep angle_a = np.arccos((radius_sheep**2 + d**2 - radius_d**2) / (2 * radius_sheep * d)) @@ -92,6 +97,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal last_update += 1 print(f"Best Cost: {optimal_cost}") if i-last_update > early_exit_threshold: break + if not map_polygon.contains_point((optimal_xd, optimal_yd)): optimal_xd, optimal_yd = xd, yd return optimal_xd, optimal_yd def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, radius_d=1.5): From 98f5e754959009c8292df88db64509b1da28084b Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 12:47:06 +0100 Subject: [PATCH 20/59] simulation changes and docker config changes --- bash_scripts/container/launcher.yaml | 30 +++++++++++++++++++ configs/rviz/default.rviz | 29 +++++++++++++++--- docker/Dockerfile | 9 ++++++ .../auto_shepherd_simulation/dog_control.py | 3 +- .../tmpDogSim/dog_control_lib.py | 6 +++- ros2_package/setup.py | 1 + 6 files changed, 72 insertions(+), 6 deletions(-) create mode 100644 bash_scripts/container/launcher.yaml diff --git a/bash_scripts/container/launcher.yaml b/bash_scripts/container/launcher.yaml new file mode 100644 index 0000000..0496361 --- /dev/null +++ b/bash_scripts/container/launcher.yaml @@ -0,0 +1,30 @@ +session: simulator +init_cmd: | + echo " " + echo " " + echo " " + echo "Running Initialisation" | awk '{ gsub("Running Initialisation", "\033[1;21m\033[1;30m&\033[0m"); print }' ; + + #exec /bin/bash + cd @TMULE_CONFIG_DIR@ || true + set -o pipefail + function export_default () { + var_name="$1" + var_default="$2" + eval $var_name="${!var_name:-$var_default}" + export $var_name + echo " $0 -> $var_name=${!var_name}" + } + +windows: + - name: simulator + tags: ["ros_interface", "dog_control_simulator", "dog_control"] + panes: + - ros2 run auto_shepherd_simulation ros_interface.py + - ros2 run auto_shepherd_simulation dog_control_simulator.py + - ros2 run auto_shepherd_simulation dog_control.py + - name: rviz + tags: ["viz", "mapper"] + panes: + - "ros2 run auto_shepherd_simulation mapper.py" + - "rviz2" diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index a4a5b93..e24b2e0 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -10,6 +10,7 @@ Panels: - /Path1 - /Path1/Topic1 - /Path1/Offset1 + - /Pose1 Splitter Ratio: 0.5 Tree Height: 1204 - Class: rviz_common/Selection @@ -94,6 +95,26 @@ Visualization Manager: Reliability Policy: Reliable Value: /simulation_markers Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 0; 25; 255 + Enabled: true + Head Length: 1 + Head Radius: 1 + Name: Pose + Shaft Length: 5 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dog/command + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -140,16 +161,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 260.7147521972656 + Distance: 173.1657257080078 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 43.791133880615234 - Y: -48.998191833496094 - Z: -15.675467491149902 + X: -44.59016036987305 + Y: 47.47283935546875 + Z: 7.680665493011475 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false diff --git a/docker/Dockerfile b/docker/Dockerfile index 1eaf5fe..e057f4d 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -146,6 +146,15 @@ RUN echo '[ -f $HOME/bash_scripts/bashrc_extensions.sh ] && \ source $HOME/bash_scripts/bashrc_extensions.sh' \ >> /home/${USERNAME}/.bashrc +RUN echo "function s(){ tmule -c ~/bash_scripts/launcher.yaml -W 3 launch ; }" \ + >> /home/ros/.bashrc + +RUN echo "function t(){ tmule -c ~/bash_scripts/launcher.yaml terminate ; }" \ + >> /home/ros/.bashrc + +RUN echo "function r(){ tmule -c ~/bash_scripts/launcher.yaml -W 3 relaunch ; }" \ + >> /home/ros/.bashrc + # ────────────────────────────── # 9. Default shell # ────────────────────────────── diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 3f03cab..a9cce92 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -5,7 +5,8 @@ from std_msgs.msg import Float64MultiArray import numpy as np import math -from tmpDogSim.dog_control_lib import find_best_dog_position, plot_current_state +from auto_shepherd_simulation.tmpDogSim.dog_control_lib import find_best_dog_position, plot_current_state +from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter class DogController(Node): def __init__(self): diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 04cc6a2..0385da6 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -7,7 +7,11 @@ from auto_shepherd_simulation.sheep_simulation.simulation import Simulation from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter -mc = MapConverter(load_coords_from_yaml("../configs/map/map1.yaml")) +try: + mc = MapConverter(load_coords_from_yaml("/home/ros/map/map1.yaml")) +except: + mc = MapConverter(load_coords_from_yaml("../configs/map/map1.yaml")) + map_polygon = Path(np.array(mc.map_coords_xy_meters)) #check if points are valid using `map_polygon.contains_point(point)` diff --git a/ros2_package/setup.py b/ros2_package/setup.py index fffd6c2..47fa0ac 100644 --- a/ros2_package/setup.py +++ b/ros2_package/setup.py @@ -24,6 +24,7 @@ 'console_scripts': [ f'ros_interface.py = {pkg}.ros_interface: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' ], From 1932b969c3c86ae1b1ccc91cde871f864f8b061c Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 12:53:20 +0100 Subject: [PATCH 21/59] dog control and sim changes --- bash_scripts/container/bashrc_extensions.sh | 4 +- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 178 bytes .../auto_shepherd_simulation/dog_control.py | 28 +++++++- .../dog_control_simulator.py | 62 +++++++++--------- .../auto_shepherd_simulation/ros_interface.py | 8 +-- .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 8339 -> 9138 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3694 -> 3823 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 6698 -> 11562 bytes .../tmpDogSim/dog_control_lib.py | 15 +++-- 10 files changed, 73 insertions(+), 44 deletions(-) diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index 2d6b861..f84081d 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -44,4 +44,6 @@ source ${BASE_WS}/install/setup.bash echo "ROS 2 workspace setup and sourced. Happy robot wrangling!" # Mark that the setup has been run in this shell session -export _ROS_WORKSPACE_SETUP_RUN=true \ No newline at end of file +export _ROS_WORKSPACE_SETUP_RUN=true + +cd ~/base_ws/src/ros2_package/auto_shepherd_simulation diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc index 782a655adf4fc320f8c8a338ce30d5adabda0d17..f8a6096bd727c9e77a09c95064a7f89f9e9107ee 100644 GIT binary patch delta 21 bcmdnQxQUTBpO=@50SMN|1ZFT!7*=b?)l!>FN35a7c|tl*CnLwX5Asb}c2!#=F*TywZB3jpH@rEb|9K!C*Ai z95ywao~`Z?#X&PMLLtZ@0ep?^0HooIF1`c^k^ljW1PBly=N$T!AO}OI1o6!WJKuZN z-Rv2PyU8^arhZ=ibydChy`NIBv{W_l_b30hx%2%G4dXwkQv9<}xr<+T8;LZe7#UkS z7h8hs=9bC1wPk-~NK;x*4QU;jTO}zrjjsJ15nGltw`^%) z^bJ{(&Qr1F$g*^w8e3&qkyYfbtjRj^id>Qna>;C8VX8C{AqbmXW&>rCvo+xhh?xHCaJgmsO-ovWB!F>qwX764It@AYGBmNLOVO z=>@rhw8gC#nckKLHnzIKTT*l(Kl=7x(BJd-hp6DMeYa~p{}~F($&w!t-7x+u>tfs; z>Op_|{_vOi?;o?mkF!4R-E_WIR#>~6F7?mvKW||=MJM;1II(*v5l<3vkhqV%Q84hM zgTxN~nU_@d`cHb{#PeiQ8U&+2B`bp;?{NW4mKZ#s(n$;ijFYuWHKEtk?+ z&NMXpBY!yFqfglP{NYZNEDwSmFZ6?PMpy~GNniD&0*NYKu1-)>`o9uYFg3!ld@kY*XKxc1K z?8$_i6WnyK=a2oU*Q;a6@E4I(&8l!MS2)74@I&gRxvr(DrirSUHCQ%pW)lraKC?e6 z6LYgr60>GRIgwr zKCrH(YE-7L*94)iQ<+XsCpS<^Dnl=2$G1@pNusKjEu5dumi5=??Q@?`l2O8sKmT7* zh>T;QKm0P__5y7M=%LG(j1zGvVrsV@*=ij*AxcS>Hi1B>IiS9doNz=_ja@>MQdBvv zoEU^g(tc#9FG_J}j+a3NzUd7Fy$=YD&hJblgjJt6am(vr2jbl*=spHss3DAwig9fC zgOC1w*80JJzQ56JBsT7A2dw_7ijsPNFz`m67CLni{S)WwzxlgB40VA96MH1(-@Etm z*NC*r?SK~<>?P&t*r&@^%ha(=35iSB=F3i&`9mOvL$LBxdP$AO_2~P1Pl}a;MZTy{ zdD|MPzKkiu4wCCu)vANF>%ukh4?=J)&#Pub%vLqi6bBDT_hf%m&iRIf3ik%}T{57B zSS!5^sd!IQ&6ElarK!@wjo7p9y~$*B(CO<_?nJ@d-OQaONg1F0Mpvj`Km{T~QjnDR zU=}xB>(k6VZreh)knXN+UJ3+v`Gt*Cn$sAFzk)#g?0 zd{ujH)lv-Q&SDtG+$S#ax(?Pw9V>AEIV!CS8$=5XU(Ah7&wJE)=AKP<5QJtb zg^}keb&CcsU>p>R_o#=4AJbgx<~49{TeL;fY6|byrTyTHCf6&=h!QOdStbTS+glAS&g=T5(Z zMiMdBHh%`8jz#EXjF(xgg!8fZ(D;LQ4(%BF<&mZCXId$?cg?j-FIlKry2c`>nV)pU zeG)Q89b1c1ibl|6ITboBrCQQ@+c>m7ZKNXm5BX^8C5>X)xU|k+qXnNdlEc2>m7JOQ@Jg1f27eJ?XB=a{1(4R$)2!Q9#Rqn*x}tj>plTGWkQSDniKsp0Yxu?C|6 z;{l0px1yn-8nWp&g8V~)J|6b6g~$+}jw1j9dq!Wv7&6mS4XEzWlrKVw65Gaqj^^+k zBm@VpX_KN8uR-MzM7YBIrfW7uQ(S``$Pi+mfsk6ND(Fb&xR^LN=i(ot@|j%xR?fvA z{2#gagPae+^=CL3Jj)t(k$WHHqt4;p*f=i5<_X;AO!AP$UGO5}yO~$>*Jun$<1b$4s#q;tCk*@C zT+fEi-5s%Hes_QKc?h(T-`xX97t|z~0f(ew1PYXC-@yO z^CoT>Au2FL6yO8u@FH-ko_gi~f<7V1LXoBGFv;6um5uSH*%GY+b7xr9w%Wk0%-ESV zGgBvX_vaiu@-oW!vC3V8*`BIdfrDp*$Al#dg$p;2tl8K)F~223H-epbBn}ZPJ1j+| zh}hNKEgy*^Qp^tz(|YV&HV%o!(FXRnFBzvj+i(RA-N-$z#O{gUynI3;Q{gf@YIF2z zvQL>BeE{)0bnY)~WcGP^35w)WFDW55F^wvANQZ5VoqvH#R%cLOkk*QPQ@yz7=70ID=~s^ zyBGKi9GVq^w8CF-Vc`pkRQ`g35^!eidn`KJzBiJcH1^}C?r&oG1C0n#2inOQeUb5LE?;j_}s(9 z9>Wn%EEzN$rP-=xM-GsY35ek1LL^2Jvcas#@4dh#FQZ9ysJ)~OO)%6jaS&Lf zdDz(ab?UOkpLf`DqMIk@&k1>

;XMggE@Iy&tG zD`kOV9G3z3LeMvrV>>UE(h{r^d=tPPtxig#I36G$6hN;qB7$th>p{YgoujVB&D{D zjqsXQY>R9-7Cbg*1&&|kF)^_DE|>`hpQ%1XPMWvUf>AuakhWCO@>^+3ja#Tc^+Qk? z!ePfHFj9!RHdkRf#{?u?XT7Z>W<|QmbUa(7W3U(43N$a6k#?i^02+*TdNc#85x9}Y zg|U8Os_!62NSTazJcA)t#;q&56{fq(XGA?*o(;WtW}F;{wt0;jVul!x05}-9H2eEB z9L)^V$=*mhi0bB^cD0lK<%N4{7~aWi)GDJpo!|ENJVda)2|Xu--XIvuaIO=t(IXH0 zPUmB!f6AWr6l`Wlkt3cXrn3Ott=R>fKTz?6};%;^;n!2){2<3*Cv*xN^R zoU;S<`!wkS*gHDD{fLC@Cfe&2*IKvM#dU~M1EPcZX)oS2Ps45yPh}X=C!p*Jjf5Ob*hxB0uB}m(Y%<3kYU}bn{N6v2f*yV?mN>q(ue>ehAGY}d`}Ru=1|FPU?^;A)*h?FXpn=nv~jvaO&k0h3w8fa1;q z*K{#92wpVkk_k_}VsRDVv1vTB)7Nx*&R|pgnRzN>T{CfDsYZcPo7BF*>lL!95gMC$ ze?@yCm54VVVS~9dQzq{*$IEnuxz6|vR6cVf^dAl=^zll%2@{(D;-(u(YqS~Ks5+z% z{X-;Ri9bT7-{BwkM^o>Gn<$O^JwN)p;s!d{LZIJtlGN#G z*B|V8(M}-MH;Mkol$_oNmu?V^n1n=uRlJKxfQ{W$P4^AA?5?{d_Y(ef-L`wveaCIN zH{9E1yDChlA$TM#-zm+)M#>jZ0@h9UCI{*pM5Vc3v}oeKr;p>yBzhtJw+Pojj1>P? G&Hn*wTc?Zw delta 3404 zcmai0&2JmW72lct;POMXz9mbEOe+@cwNu-z4ciIiI8NdcMiLc`o4QGp2sCFzQldz8 zb|qUbRkl?N6m8MK*hA2sq~3B1tmIMz1zPk{6zCtYha7rnp?fa{TByG_OL1dHO-bx; z-@f-|-pss@d5=rs&3tQMAYH4GmQ|#=cbJO{fA-FJqVhD4azimEx=kHTC zX?1@@b38BO@gIlpu=5=zSe3VpMYC-Jt+oZU+s3kkcS;b?6~s#ma2GRz_*szx=7b9z z5NTjuWPry+7C0z!z#%aJ92R-th&Tou6@$PrF$5gf`V->VFg_-QamVn67Zk&?lo_7q zuio2tKzru?0kKktuy29-$8Ua3zdzsD7x>yAB{&1{jz{c4#5N;$LpEBKdaxPg%9V<2 zNL3E%t!8B3uZy4-IW<|Y)`IY_{0|eBUwDdE-=66hQDyW z_2T}&4-O8Li=`}hxyV{A<1{V*dn!&~ z>bT`usuw}5xrIxD5{`1L} z*VY=Fi(2;9-cQ|SR!q+IE3}~V1SbfzFAJbPbg-VWOmXTGvHKQ<2#_o9?Y)`#K0Eas z$)7(;X{jOQItgCf`)8K%bBU?M{7I5)quP7{pI#c43;Aa+UdIPTU$t2e)`e6rknT~- zvCEBmwfW1vc7B~L{!c46Xm0F^W_gLGb}F(NNTpsO&MgA6*uPh|(yB=xasCN}{%rW8 z{K#HqaGrOPO(Z5~X@VyY(#s|$<1O^;MO-|`>xU_0pB=hbR{m z=bF}m1Vv7dzXTco!g!Keo!$hmBueoFfxY9RY^?s zg%j!o$#pV}9T_R%!T8wOe?iGV3vhYP9A?9OkR85f`3xUt_?zQw#?0)!J&|JXQWPOO zjtc5IvUiH)c3fsm(W*vH&|1Vh&Y9OprLRb&%XA2&Gqd;m$v?49HNK(tZ>TiX7rddk zK9*?gZGzVcrU<49o+7wL@J)gRg5+3|qH%!)y0BgZ<+(||A*X6muF*0}M);qpi{pK3 zw3l!tu6!+gdHSUeU9*Qj6GShsE<@5bc8pzv>70tQ330+H*5aJPR}yWUf<5L3<__8f z$PN=h>m{k6GgUVhofWR0hbr!=Wp~ur;>{6IBuYb4>Pj+Nf^I|WW$6pI97n*`!P92i z3?a)Anfe>C*){nDpA+9QmeG^Mqpo()0xjYbOtdJp8`=YS13_M}0IiBv9^B^J)EMP@ zT+$xuatA(lL0U)!`{CT1`u-{eHCfQkIPDh>JBLEKi6o(m8;Q|@6Y1D>W-NHa>I!{m zFY9YG@w|5R2uXEnOb|t$SCa(C33_21ndn{s^=8tcps%15ifrA~#FLycZeW;>4&75Y z@wIywuJ@^^yXK@h$!5(Q%QOAp4xVI_Y}6dpd6YN9&t|4O)J0NTrt9%XAOf7^4!H>R zG@wYZx0!xB+iZ#IZ)(@lQH7>aJNntf5jgWN=nGET!bWR@Ain3oVDvOJcfrd>&e~?RyecD0sPcW>rgc?8Q#k|m$gDSm z#0lzYnk;dfPSbA7;^-pjHT$ZEdJjV$(COzu6wRDjqDcyS`Xe2zcV|- z&4uv(tk=$)|@-)&bcS~V1{ug&$I-MaUU8MA>J)$3PPzk2oY zUcL7wX*O#H%1=Jp?f+=qFg~WnV&Q0Pp{B2b2t%;Q*ekwlkLk9#$KN!BDfm-E@KbZo zLdz01S~gm?V7o@!xxj2=Cm_05X3}q?R~jU7&lGjRh4qx}abXMRsj+8?ig59@MOD=B zb+pVz?(X)b9Be1O9mOy|uopxVZ=&8uP2T{SFu^vAiQyAxYK~1f+UCR*#?Vk#1rR;+ z1a0(kL1UJ%K)+OpF8A5pKMi zK@+F!$BcT;%n6&A=cZc67x%BVV3DTvs9lVn@Ch`dwMU$#IgM)DW7i^p5TOG-1CmYtRKc+H4 z`=S%@bMr&IZ`hD=fERsW9M~=6#LAqr$^a>h*iLGdC0}^S)ibjEUNwTY3)oH6XAM0^i>C(*ld=xVq zmu5ESW5rCdbmHpSO|0u?OPB%ar)f11zd&REQ~57PFQ2VI1FPrkq=t3Y&aU7bHRz{4sUNi9CwSKst3PEYjR`xuI;l?@ z=O#2m-g<54g3LodR9GJcg^OPi`7KEA>$?1Pv-gXiQ@Plj_IzA2f7(>!xjrezf4$l3 z>hkB!-XDHXNKz;pwM{>$G<0Q>g7?sUV+>4|? z-DO@cb|rjB&0P{rTl+6psJ@I-Qzns8n7L*yu^aprx8VO8_?a)P7nV)Fkbcg;Yj(+5 zl^VMSPiZ}`@#0Ik%RLRCSNIAaH)k%0ZoJKld|(F=AHgz)c6>jt`Tk)dhSS5>DPX|&8u!xZb+nh#~X|vG@0*OuPEGBOV zxzJkDMuF_6z?Ok?6AR_`Q7B;MPG5$-{$~t@)DWVklvE*+nf+>P{JB!W^Ba13Vnh0& zs;}w;3N{FlPYmSU+%oVVX4-{L@oR)%FY(Kl#{@?J zSs2f*oV75IA6VxUNGaH0^n|1TYS~|>KwFMbXl*Rc!VL7(jl~%)Lnjs7zeY07t`j|2 zpYZF1$;)cIHerc_-F481SJ`z@yv2C57~xS=GQfET4u~W)l2WrS~U7Zj?%l{ zb{J4vjHJs;y52y^G?hY+T>^{juJ>+uNLh^>P==GrAVJzV1NHv(?9)GfZ~xKej*b-V z8|sU&h}?PTzy08icXR&G?cx) zWL6|-`tDYsK=v}@<1u%QN0+XOmfL2*Xp%G549 zv#czFx>UaQ4`|VYbj-Df0=>1zqU|ZSITZ*{L$?6wp=i;#zc+guX9ig z>|=~E>u_3gxeMrV+UFZs;TG=YWfIRx>^Uj0rX3fnVzj4&aiXF$N@BSHPLlY4Xvt-C|JmCZ zCI@0eCTe51r$l&&eiDuNMsJcPp&E+uP)HuC=wLGH5sNQg0I?XCDa`~=2Ct5fbP$G7 z9Hn9CL8@v2)a)AbS>ObKzC(YFO_%Z)cr~Z*)6?dS@Zu2`F_%|4G5MH?nslu4k`#@&aKAd4tKO9CQ(SD5gf&$uyQ96v`OZrlq z({FE=^rzYf6P31ip;c&iS3r{nO=*A5wi8_FP1kBNO6eKnJ}P2D1cC#+h)1i_?>Lby#LW2h(2f}~G2o;UJVQkJ>FEv%&0DZMn#-lE%>e$P&=edi3p z2EBXgkw@(N2&VH}mMGmbhatcoI|U_Km(ATdjrT(Dx2(Ndn$8U^n014 zU_MhVW@=@cfkV_6(;0=ke02*$HVUtc*i5*$Pf{!p_{e z^zt6a|FS)hY0sKrBt%Kc~0hIG8n9rg3S3J2>cmf@VaTAb_c)x z4Yl*`eCFfsvux?3?laJ#{iFLVqxoZZuxHvoyMw>~joQDugN)|i`S$;IA&PwM0?bK1 z$whtG?J({4;aoy?RtXndNpF;`ND7-0 zwVWky5?Cid2QEcxa~(>Ml5CH%p`v&nSTz=`1YQu-D`usS$xDsM@8VZ40ThKB3M!Ok zD49^cQQ79fu6DjpO#Mmf5QEElS=5FBU}&(X#_ zqs)i(UcWDJa8pa^Iv1r;)0hfstc?upIe~JNwW^wg{cLc@CL1(t{R%PnY~Ih&i>7r zvuYL56`u1|sHw9qUqPq+hvB0)-}+>9*WYnd_ZfJhvC+b-+sp|a_v40fA%iX z9ZISgWtYG`0zV>RFnaKl_cn<@$yJ)ZOMsq3egi-|{X}U;p?oqmyz#`M@|c^7b{~r) zrEUHM|IrJUqW#I3BM=^dn<5`>*~_eju+nnY>=s+I*PWJggVmi^oaxQaF`I1nX2+A? zr$haKzz+$S@IY~^B=900kRR^iHziMC1*~3^G>0RT2F>M6XfU&%$3h2pvcU8PB^Xly L5}&5M{K5YLcdJ-D diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc index 0f634cc80a3a0d860b5ec128afb9f228152f3c20..de784c5cd83ef2932a3a85ce15a43329063c3816 100644 GIT binary patch literal 11562 zcmds7TaX;rS?>GvTy}PLHM&|>Iiq?cbT1{+Y;}!xjDmh_Cs2OIu0q`id@j zW5tlWxnjxPUa{rwtT?zEt<;*k;_B>Uw$f{vmCRapC5ybZlJo7AIp0~y`>B6HaPv$A+a^Rs^L^O~RAGFA>CH|OV(%Oh7rZr(2-S3vF{atr<(!1L% zD1cOP&HV77WZgf6M4)l4vCg`0;QdVB^ex=+ZoY%N?Wb;NB{z0%v>JY(s+_~<$8-wU z1zcebB+|C^fj%$>=D^yq23pw|*r4XX0ksAxP?0c5fIgxP%%XM} zdy5i-)W=x9d03P2TwlX%tfkAlHXgT2_9~@gr@K+FtOc?4N>GhrOOdA58tw9(N(+g~ zJxSQe_0Xuzrmn9l^?IXSkF%?lw%-cM?1izcFlg0c{a&nZ#P;2WAFamDYS5^!M(PNj zrjCNd*+#n&H7YHRs~$ngJDTEJ{$Ta=YG*Attvcc9H!ERKzKeUW(eh7MdQqnwt_I!J zK>6jcvDRx9?_8 zxMzIxZR;E0pUB!VH_d^*odqXgtY%K=oR1x@89N3g7W?Qd?Q%`~##yg#o;p)o-Yp_q z%BzF8#TK^ct~xBqE3ce?J+^OMy|#QwJt`$NNKb5CCUsQM25ZVjsAE$5W~&jY4R3#85LUPukv1sIt6S}AOv%`htlx7wYf~%1g zWf9*+VxTv*NN*ZY5kfusqZjlmxC}yA;}efQtu_n3TFGy@}qAx)r5; zGszIJ^nHX(-~jkauQvN<0_r-@j}-j&Q`uk7GXfU#{Aw95DP zY*U;Y%B-|GjX}agAUQIr1 zPy*#RS1zyjDy^hOJEvtGH%s{Z4{&oDcpTRxlXR%ee_~3<3NBNC*B?^{Rr5)BfY7J_c!aVvhJ?6>pA(N=t9jaUd zX%R`Nd;OQ8U~9%vO=>LEKv_3w2xe=XJ$=k^A1d(aK9d@k99d3N{L2~PPMClu_>UY~ zPSgTbM`~D-6Qz?y3CuED%uV+}>0D8RDX^q>UX;qP_nx|W8sU;%`54UTwM0ob`hiy+ zFPaxkR?WNHh*rH?BWU^Fo1I?Uuc!?V*1${jgy%Qbf_4Zk9cH}YZ=5)<>fzaOj;d$a|v%%(0ZjZl_iY zg9tUF)W%T3tIK$}dXeNMl8LyAq*9oF23JU?>ds z6H1Lg`ccZbNTU5I)BrzP6PyaF7fGRYVF;<6&8bogr4~U_%>cLR{f`0*-U(FXsi4w& z>TXAYAtx*wdQoSz)l;PlIS?EK`j>o9_*vpyk1C^2VLS-D`CWRrB&}JDB`st z2zdePQc9_dxT#AZvDpG6Xzy zxxaioFy0JD6fiDjOf<;1k=V--zApS=Y}%0t9smsALJ4@_+9oZkJ#da{n>bi8#beT_ z4fz6G;&M+nX@_~w0OA9_57OYaEcP<{h_;!-6J6|m4uF0RXww4P%nlN4IR{Re8{~*2 zH}k{3<^b3d{pSbs1M9Y?-Wbe7m*qF@^`dZu-GBaa12&ZQsoLs<0j!`bsMl%l#}LqM zY+7#s?|)@cd?%>J>4YnoObWr3aZajx02(aM zbXBK`V_V+@|Lm&=aTVZcU@9YKcV`F%V-d>kZixL~(MGea0_x<<})!Dt0D zXBLpNP-lDwZ1?p3Z1?a7ER|gKvlw4JNAf%gAE%Z{UL|>xBqW(wP)UgyJ&%gBxI$_( z0lXh@9oNYf6>F-vW`48>=WvDWGX}l$be?=(hr6NRJAMi&3(gioT^%k5anJzzifabY z*Mz27j59aJ$7V$>swg;1RnpOq7@T<#r*9!a(_4m50aS;272m`bbLPPI^`53qQIR$& zSZ33v-vJ^)!7`f(O%2OZP>89z0q=+jYJHso>0%5Uhcb9dj?zzoUViE;^n0YCtU3A% zzT4*U&DtXP&ejQjaTea5gMUQFDL9RM-p_!}`vAY?g22~Y<T(>wsyC7L3#F-WvT&K{w9Gt;_|97sN>f zPZbV9!|DPu^)?PRQTeVM3F7%`XEoq4XH>8d2HlDh|J1MuDnkI}WlmxYumRwwHY3iD zrj#sk>{juRKoO`9Q+qL|D4svSnBfy3PR7ymrfU|BqV5=QX1#A%bZHq-b?y%jxeiYE z1nU_cXZo=!m>_clV~l>uVSK0vzKhZUPPGuoixEV83!)bhsUrnT`q51ztwr?Iz>134 z+szcLp*b!i+OnHzlu4WI46*x;q0aGMhbn+JX;Pw>j?Au>4Dd=r|y|6w< z;U4gbCxPekgH$xXWvEa1P%L+Jbu%jXsV)6;`XEJh0<4wVUhv&yWIXwyW3OKprIGIc z#MFUTwjxm$iDgYB!s{qtueRS=gQ-mzI$Y{642zOZbSo|=d~Z(pI$=k$dH?(k?)p&1 zytN(-{F{LX+jj?;&-W@g+BU=pR#YPhlS!N$J?^A;JJ?91W-+$A3Mecw=AXjQaOKJ5 z;uK6OJsZhWh5tnog#Sen#6}IIPo>+115k1M<4gni67Sls!qdxX#FB^xpAsfZRV-Mhn_VB;S76(eWs)@;tIu)bL0jZUXT4>(R=`1EBRDU@pxSBs zd%PDz_4a_e9`n}51Dr+I&B>8^7DfwCjpsodF#Ee!^Oid8;K}4ghR-`0dexqSM>$Fu zoTr(!3b+(k;7^8I64B^i5Lp;okA0<~GLeS|=S*q9k^(A_h&6H5mze%l5{df|5<=Gk zGLEE38mff-d$JoNAGhI0>T_&wi!|@$;jf|UKXFOKz|D|9PZ+?Kdd4ckb=qH;;>~2> zDrYzPF-`L$_=+yyBx(ds(JpGlIE*0d01JJcjt#5-#4AD6Qwi>$UDlBYYplm-oBELAErpu5cd$ZN{mm)o-A~o)?cZ&(a`PJ8RwN z9K1!yCls~*N2cC)Jj|(QVc{#S@Hr$#ok~VBEcXIFYA(HETD=AGj-HrD0m(u{VXz9a zqlFY*DhcQPesRAC&V&?FXgrp6K8Nod=`sAhYX9eV8?*F zz=H8G;Fz@rHli))x1NohW(vnVJxW93%^gTF`eran3WkM8LK%OZXgE8{`4;xj?jOB~ zpvS(#KE=3Xh$XnEty-hiitQ_}T)MQBC~tub%cxdgBsoY@0*O=IPHUqML}kD5()kN7 zPL>phe*qg$v(?BeD5tMbj&5q=w%KQ=W$+AFoj&-CX_+}V`FLi}AXP9C6dUv0DaLak zIg`@_g0@T8>aU{f!_G-G1B!AaTc1P+CL2H5{RmBdAJ_1ll+iO!WFB*mE61k;5u;lN zng_=vhoZwc&KQ0MetMkWaCS@kd4GP(9hMiS$`Mu{mmioaFZu_!T>sG5Ij19-D+22X zH6GsKn;rF!_($tZJt9%7V@v%)0z@YJxvVHWir`OirPLhBJV^iWBr>~J>7K@Q=F~=I zt)+^n-StVn!^R&Wp_;4X!p}Vyr_YZN^DdcrSW!F|yBCJQS;FkYa_bxps$=|IGUN0Z za03SqI{>7Qo2Zi*wEy_cFz{}jzj2We?L5%RUGK$ULtMXWojZxKg_-GRFWtC6fAD&* z5tWV)Bc%ip>Vp_YJq8k|;Bx4;;I4ORGZ5Mel*m$+*=j|OObGJ91jqITsJh#+4POLq zDb;H;B%$8sSiej1V)jttC(YKFdgcFo} zzOBm>1f)KPKTgzzp~NwVF;>M<5EPe9*lRE6ro*M-Rjw)}B@d`TeX%49P4XF#QOxVtk&Z3wc&PrCb$eujQ+buU zM{9^3GczW9*XCoegay5bFB@d<_JMjSw5K8^?}y(5`J!wZs4VfXXf>Ru@yU>j5;)MB zHUZ!!;tb7HlVxn5#;F^hgM0;WW2PMZj?asvTzY4*t*(4dl44KU!qZZd+a8x7KewIV z0nkKSIiif)UW(?CF34&A(F6b?zp%YX*Bnv@@HqFKPz(=4BeQnhBeT zhi4hRAdtU_+tgX~x0#i#6wOAV>2epjBaY@6`TNfO{-d&Y#4!6I)?JESNl)wowb}CS z4?xAj?ea6GrP2|x3+i`REZSbUSEfZvHvwzv_m~x{@Mla-;P04|<8%NYQ~5gT%vsSlF;2?&52s+Xz3rqGq3ku zd1W+t7^vMdg@58aCYogY81)TQJj%z6o&Qg`dG}!Mgw)KeCjl7QRo0W*621zY7q^VB z&OuGcmu3hf8Mm-X77lw`CfsE187(|`7eJN&*Ky&*0~GUEkSEy~G#l)fD_@ub+SUC48;{+e~)@8c5(?{Jrv3P6FBF6OUL| zOG$jn<=1{u&ky2stJAJaQcz~&U*FYXcw(yqA69G;k;dt;QVYsNSQ7V&&1xqEG^_Xx zeD5QtD8PWf!h*(nEp~^sOOfh`lyLd=vgC>7Mo%q59VdrHAfHdo~5&(ZxY?yHAH z4o3d?eG803CbA^fWWnGc=K%_N#n$|OijI+Tx%5b)DE<+p>IVvqBzm?Tao!6qRjU`*n%J$}SVJMoMO8DdK9zBN5H>DSz< zwmq%3gfiJ6h!BDWQi9a8OSW0@0}x`5gv8yjV1arEI|xb4cTV+d20PuVQ}>>G>(;Gv zzI$Hnd_JS$*Zl12=JQW#+TW=&`!mrwjVt;sh|q-I(AJZ?zOHlMST}Gt8fMd4w{#ja z8+OxKcbcj76#CY>E9~{OaMm*-wVoC3dQPO*^A|Lc5!nwkk=-@c3+UxU9=$wzbLbVs z9C~x;6@z(Ee9w@t1w~;A8=O*58Z7K-SX*D^t2{r%?zBo-SN7^=5Om6ha=CG?U00=X z>ukFf%XXs?$k4-gg+DSh?Nd|gcyvlWHHSyR82+5NDG)I3Qk&$&1dN|jJ2xgZH1_?l zZX&cN&S^Hk5u_={w546^eLMIh#RP-;Ct~-X<48R5omdaae88 zy7CY>pK9_k+&_QVMCm#cH_1mZ3dmqKngz!o83k=4%@B! z4`6c8nVmU(=y+#GxxP=E@cqMhZ&U&)>CWW;s9w-BcYAY*(vFYk)=2Z?pM{2li?!E5 zBi6R`fzj7vv#)I#d!|0+K$lzgh~o@WI7IrMamVNz188$A{Sowqdq$WaS-%5*Vr$R5 zWA^ny7D&KaTREnA0S#{$dj=gt=+UVRm+;=6ankFV$DiL=8ngJ{Z!K+2XpLTo3h$slOS zV>~AgZpW;(gm_>|IuMWJiU?UHqhu6}M|Dr{Wk-UfE$tH`&Z$OFWFdZrMql63Vtvbi zMOef~n*$3Rdry}ohP*=`PSdB9hNss4g@U=Ek~r# zvV|+su)?sHP*hH3RwtIDQX|}rf7G7f@O^Y(hBY516jX-+)w-HI+}DK;guv84q}5?u zg#l`QY}~P819L3k3mZ6s!z`Q+ad2U{O@6X0QY|yK`sOaJYS#%@FzJI--`vx2n8~DL zqo8S#0nIS3vXvp(8-^BnJir82v)xke&7je)h4GI30gb-`0<}-t%G{~m8j_6axbXYu ztMza5@6WIPBjGHaE@vg9=MgX@30ApmF$FxT*$7%H-Rd^U;sza+No;8&jAAv;i`6z026n3?=9H-*zH^7$?*jIfUrKa zNbTEcLS>&;F5hwDbl-_HyM|mAcHgrzatuWs7>LJX7$D)nAw)7;Nc)RJ1Ar@8SX~-afo;8KmR9(#LEw2(l z#_Ke@QS99aypC+&gmn{MHS$DQBg|H17(~gdkBwh)%zH1`Nzj{zp{lm(fjoz|!Xfc) zRI1wwH`=l}^VSs}uXZ|cS&~kN%7jta!iThKh)w8iD)U1MR92*HOJ&xBxSUF$_8Lvs zedRQ(5~0iVtCW3DjGf2)w{S%-g1B&HmR>Z9y7iT7xj>kqTliRxDm2}uGy@+du z|KHxjv#1-zJd<;Ce?RyM-pBCM8Gd@K0~UFBIUTT=k708d76>?DYsl)rPa-@_`hW`e z5q@I6CZ7f08S!9q;877qZgJSLhd=`D3;_5b`@wAA@U!5++xZl7iz9nN8pHh@g^WPIc5ncGT!JnX)McnbaK+8(fud=q<5vmh-pl*v-5w* z0YJQCV>8G=ijMDFbo}1rerdz2v|GV*$sP%_uye;Euhx|ae&S@WUKn}JDm*f-Kunjb zBv?PoSjTgt+4n+w_3QRbXs2XkNF>31QNBrSLYcfu^jA!g8p_hN7-EE2J zz=4n#=&6fDE`ca}1McBH<#bsuqliFW<{=jLL2J5>A&Ow5M?qYd&|G9R=MV^FOxIv^ zn+Oj6-J74mbn=24eb4wuZE`1UN6Et^c@e;)ozuwuQH;EYtmI>ZGLhahSA)1KTdbJ< z+GuYaK-9kX(AlR5d=*zjVJ|t=>&_kIPXldF`<1bc_4zxIHKt(&5nd7G*;c6p#1TWg}UZR0x@qrApNKE;01P z22CR%kH%@}7q}uyBnqYr2i9A>*9*%=1?g`ErtG`)-1mr_B=R^BvQ(0k$P3PV7{AV= zf$xL7go`6{SIg*`2Q!c59%gR z&QkNUagLtYrO|v$)+&!Eo;WnK7|+8N8X|>c7*Q7L5@`g8xm|a}&(H8tmzeO2GyHi` z+I7Xk$FyDvJ6aelB8gkvqc<*y2gIQ|wI1NO?Qo@cB=L*q=^P?S_NFS86lF?Mx1?}G zl0ewk4h34nKT&s%2pyzR{N|ok>E&^ZH6&$^1{O~%_v|Rt8a_==ErTG+okUw@Q>7>2 znR3T*8A7_r6nPZS^q#sF1>W1stLG>hScdPsei}k-j#jk_2sjdm3`sr%1T8(MVo(; z2muw_oA?n)r<@OxT9|l>+N9LOnSx41TwRGcHI*a@m7}OYb~*Zp#x`&o@*X&dbc}w3 zE253K21Sh}J;%`_#gjR+WG-1toDdaLhGFQLuX=ex_%xa%g^hm%by74#{U>N7wShg@ z!0X4zMr|?Bk){>_YK{V#V^cbdQ35@uwxdxv>_M8JPI1)$2`xvEPi)(Q?5NpWyvSu3 zFDd1CoY_U^*AV6x@p}0kB4jJ&H$jy3*0oD(Ds_3~9ZE`-3D>0D%RY3w{3Ze?n&NP| zC*s6yEIwKVRREUtjFs_-ouADQqu+zYX1QQMijXAHf|=8eyOz-_vE=qWu+O-Y_~>Hs z-9Te3K8S8q6QIvW+ote(PvhvtMy&uggZZw|kD#zZGkl6%T#A)OeTr~kLni&4upmOa zH$RGF>QHOq<0YLlxk)4hsg(1HG;gA#vg8dqGKiu;$ZeX#7MT3#)b6t^IWa%rq(>rE z7DOMhZ$2PUUhR)qETXDH#?0}*XcqNF)Kgf1_i(=s=DN68a-X1qZ>6Fl6{Hf$m|GS= z=(3}v6hdPlkxWd;a>7ABHE5N zi7l}DAomPpM9gDE%0$@aJVPxu3w%!ALl?!d zM%XDkiO1$pVTagyD4(Nyjw)Y%)Ri06+W#e{w1vOoO27}D74)K604ERP8yI8qheqtSRYvIH?@JC_|JhO68#!c8eSDA0>wu*889Fwq-*u& zuXd9ti-p^0w>xXYQ+Q3KoEcg1I7DIBK1)7J_Arv*nLY>=tq=jFMMhtl(N3hS;5O|4 zxYktTcrzuttroo90U#?A{C{B;7D)zKl*RF+OU0%yTqmdZTowtYRm$!?1DyTakss3T zrWSk}<@gx*HNA?8_gqYR!Y#N9xg|*>RI;J~-}lRl$wB@Zxbh7ex^E%*I6Ix0LUO`L zlFt|JSC_ueQ}Cc%Y+ 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 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 else: + print("Herding sheep") # get points around 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) @@ -87,7 +90,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, # ← flock, dog, goal # set up simulation sheep_states = [{'position': [x, y], 'velocity': [0, 0]} for x, y in zip(x, y)] sheepdog_state = {'position': [xd, yd], 'velocity': [0, 0]} - simulation = Simulation(800, 600, sheep_states=sheep_states, sheepdog_state=sheepdog_state) + simulation = Simulation(field_boundary, 800, 600, sheep_states=sheep_states, sheepdog_state=sheepdog_state) # optimise new dog position last_update = 0 @@ -138,4 +141,4 @@ def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, radius_d=1. plt.close() im=cv2.imread("tmp.png") cv2.imshow("win", im) - cv2.waitKey(1) \ No newline at end of file + cv2.waitKey(1) From 4e91b4d3345f76fe2ea723253328c6db3647f885 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 12:55:17 +0100 Subject: [PATCH 22/59] Updated sheep radius detection code --- .../tmpDogSim/dog_control_lib.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 3786aa1..2815a5d 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -15,10 +15,13 @@ map_polygon = Path(np.array(mc.map_coords_xy_meters)) #check if points are valid using `map_polygon.contains_point(point)` -def smallest_enclosing_circle(points): - center = np.mean(points, axis=0) - radius = np.max(np.linalg.norm(points - center, axis=1)) - return center, radius +def circle_around_points(points): + diff = points[:, np.newaxis, :] - points[np.newaxis, :, :] + distances = np.sqrt(np.sum(diff**2, axis=-1)) + max_distance_indices = np.unravel_index(np.argmax(distances), distances.shape) + midpoint = (points[max_distance_indices[0]] + points[max_distance_indices[1]])/2 + radius = distances[max_distance_indices[0], max_distance_indices[1]] / 2 + return midpoint, radius def get_direction(x, y, xd, yd): dx = x - xd @@ -63,7 +66,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d radius_d=1.4, n_candidates=15, early_exit_threshold=5, default_goto=np.asarray((0,0))): """Return optimal dog (x_d*, y_d*) given current flock and goal.""" - (xmean, ymean), radius_sheep = smallest_enclosing_circle(np.stack([x, y], axis=1)) + (xmean, ymean), radius_sheep = circle_around_points(np.stack([x, y], axis=1)) radius_sheep += .5 d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) From 61a26c8500a97f205a3f5698aad2c0f8758b1060 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 13:07:28 +0100 Subject: [PATCH 23/59] fix --- .../auto_shepherd_simulation/tmpDogSim/dog_control_lib.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 2815a5d..ec084cb 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -111,7 +111,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d return optimal_xd, optimal_yd def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, radius_d=1.5): - (xmean, ymean), radius_sheep = smallest_enclosing_circle(np.stack([x, y], axis=1)) + (xmean, ymean), radius_sheep = circle_around_points(np.stack([x, y], axis=1)) fig, ax = plt.subplots() fig.set_figheight(6) From 70587fc190727c9090e28a84a7de4e527f2e1949 Mon Sep 17 00:00:00 2001 From: Yi Zhang Date: Thu, 19 Jun 2025 13:41:04 +0100 Subject: [PATCH 24/59] sheepdog go around boundary first implemented now --- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 178 bytes .../auto_shepherd_simulation/dog_control.py | 63 ++++++++++++++++-- .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 9138 -> 9138 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3823 -> 3823 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11562 -> 11562 bytes .../tmpDogSim/dog_control_lib.py | 40 +++++++++++ 7 files changed, 98 insertions(+), 5 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc index f8a6096bd727c9e77a09c95064a7f89f9e9107ee..782a655adf4fc320f8c8a338ce30d5adabda0d17 100644 GIT binary patch delta 21 bcmdnQxQUTBpO=@50SK}Z12dQ=@-77cGBO0v delta 21 bcmdnQxQUTBpO=@50SMN|1ZFT! LOOKAHEAD: + break # keep this wp + # reached => advance along polygon + self.wp_index = (self.wp_index + self.wp_dir) % len(self.field_boundary) + + # lap-complete test: wrapped around & close to start + if self.wp_index == 0 and d < LAP_THRESH: + self.lap_done = True + self.get_logger().info("Boundary lap completed!") + return dog_x, dog_y + + # ---------- 2. call path controller -------------------------------- + xd_opt, yd_opt, _ = pure_pursuit((dog_x, dog_y), tgt, LOOKAHEAD, step=STEP) + return xd_opt, yd_opt + # ------------ closed-loop control ----------------------------------- def _control_step(self): print("_control_step") @@ -82,13 +129,19 @@ def _control_step(self): # --------------------------------------------- if self._planned_dog_xy is None: xd_start, yd_start = self.dog_xy # FIRST call → live pose + self._init_boundary_follow() else: xd_start, yd_start = self._planned_dog_xy # LATER calls → last plan 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) + if not self.lap_done: + xd_opt, yd_opt = self._boundary_follow_step() + print(f"Boundary follow: ({xd_opt:.2f}, {yd_opt:.2f})") + else: + xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc, self.field_boundary) + print(f"Optimised dog position: ({xd_opt:.2f}, {yd_opt:.2f})") ps = PoseStamped() ps.header.stamp = self.get_clock().now().to_msg() diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc index fca4ab38e2cd0381185794f793db745b527d17db..b68bc886ec1a89279e716e312d2a24d707d40fa3 100644 GIT binary patch delta 18 YcmX@ic$kqZpO=@50SK}ZCvxos04MAOMF0Q* delta 18 YcmX@ic$kqZpO=@50SMN|Oyt@L04Xs9g#Z8m diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc index 00003505efd7a0df327266de1d8b7cff64a4abb5..30807e1af7689e5ab3907899388883da4d260d01 100644 GIT binary patch delta 23 dcmdnwzR8_0pO=@50SKml56(z(+sL;>830ju2MYiI delta 23 dcmdnwzR8_0pO=@50SMY21ZOO9*~qs<830j%2O|Ig diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc index 6073e43978fb529bc61ef1ee198f6e8fcf4fe872..3815cf9de3f3f0e04ea407fc027d29029cdf2e7c 100644 GIT binary patch delta 23 dcmaDa`(BnWpO=@50SKml56%#j+{pKk4**iG2HOAt delta 23 dcmaDa`(BnWpO=@50SMY21ZPxBY~*{$2LMy72OIzZ diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc index de784c5cd83ef2932a3a85ce15a43329063c3816..1a0e98d760de0a6aa40a714ab894ea944c321524 100644 GIT binary patch delta 23 dcmZ1#wJM4)pO=@50SKml56-Z0-pD7T3jk3)26_Mh delta 23 dcmZ1#wJM4)pO=@50SMY21ZVU(Zse2E1prZ+2A}`{ diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index ec084cb..bac4a72 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -110,6 +110,46 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d if not map_polygon.contains_point((optimal_xd, optimal_yd)): optimal_xd, optimal_yd = xd, yd return optimal_xd, optimal_yd +def pure_pursuit(dog_xy, target_xy, lookahead=2.0, step=0.5): + """ + Minimal pure-pursuit helper. + + Parameters + ---------- + dog_xy : (x, y) tuple – current dog position. + target_xy : (x, y) tuple – look-ahead goal on the path. + lookahead : float – distance the controller ‘looks’ ahead (m). + step : float – how far the dog moves this control cycle (m). + + Returns + ------- + x_next, y_next : the next set-point for the dog (often just published + as a PoseStamped). + """ + xd, yd = dog_xy + xt, yt = target_xy + + # 1. distance and heading to the look-ahead point + dx, dy = xt - xd, yt - yd + dist = maths.hypot(dx, dy) + + if dist < 1e-6: # already there → hold position + return xt, yt, None + + # 2. normalised direction vector + ux, uy = dx / dist, dy / dist + + # 3. advance by `step` (or full distance if closer) + move = min(step, dist) + x_next = xd + ux * move + y_next = yd + uy * move + + # (optional) diagnostics – could plot candidate points + dbg_pts = np.array([[xt, yt], [x_next, y_next]]) + + return x_next, y_next, dbg_pts + + def plot_current_state(x, y, xd, yd, xc, yc, optimal_xd, optimal_yd, radius_d=1.5): (xmean, ymean), radius_sheep = circle_around_points(np.stack([x, y], axis=1)) From b8d2276136322da24bbf37431be53d190e1ca598 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 13:58:33 +0100 Subject: [PATCH 25/59] Made the dog go away when appropriate --- .../tmpDogSim/dog_control_lib.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index bac4a72..a34e0e4 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -67,10 +67,14 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d default_goto=np.asarray((0,0))): """Return optimal dog (x_d*, y_d*) given current flock and goal.""" (xmean, ymean), radius_sheep = circle_around_points(np.stack([x, y], axis=1)) - radius_sheep += .5 + #radius_sheep += .5 d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) - - if d > radius_d + radius_sheep: + if np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xc, yc])) < 1: + d = np.linalg.norm(np.asarray([-10, 10]) - np.asarray([xd, yd])) + 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 + 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]) From 428832e6d2a5618f2c7be31110a17ab9c792bdbb Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:12:28 +0100 Subject: [PATCH 26/59] sheep sim --- configs/rviz/default.rviz | 27 ++- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 178 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 9138 -> 9114 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3823 -> 3823 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11562 -> 11481 bytes .../sheep_simulation/sheep.py | 213 +++++++++--------- .../sheep_simulation/simulation.py | 98 ++++---- 8 files changed, 181 insertions(+), 157 deletions(-) diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index e24b2e0..c2fe930 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -11,6 +11,7 @@ Panels: - /Path1/Topic1 - /Path1/Offset1 - /Pose1 + - /Pose2 Splitter Ratio: 0.5 Tree Height: 1204 - Class: rviz_common/Selection @@ -115,6 +116,26 @@ Visualization Manager: Reliability Policy: Reliable Value: /dog/command Value: true + - Alpha: 1 + Axes Length: 2.5 + Axes Radius: 0.5 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 2 + Name: Pose + Shaft Length: 5 + Shaft Radius: 0.5 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/goal_pose + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -161,7 +182,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 173.1657257080078 + Distance: 135.89871215820312 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -187,7 +208,7 @@ Window Geometry: Height: 1495 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007270000003efc0100000002fb0000000800540069006d00650100000000000007270000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005cb0000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008220000003efc0100000002fb0000000800540069006d00650100000000000008220000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006c60000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -196,6 +217,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1831 + Width: 2082 X: 0 Y: 32 diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc index 782a655adf4fc320f8c8a338ce30d5adabda0d17..f8a6096bd727c9e77a09c95064a7f89f9e9107ee 100644 GIT binary patch delta 21 bcmdnQxQUTBpO=@50SMN|1ZFT!7%Q6rNfCt?hXI6UT9y#Ic(nFin9H5Cl@1K#^1R2O**GBO%N2P8=t;*BOt~ zgzjQc%B8nzB~W`n$pMKT!N36|?g+$z3uq5q5m(@f!h2)zCLws`Z)e`T`QA74-pqbh zxL?qWNF+qy_hsrN*)2S!X`qYUb=sMoD z8e`iy@Lc!LFjdaE0okN zrG6F(VF5VxBJ%mmtRWcZm(a3^fMJd;>tbwG>9`VI?s4=yzB&1@%iR;$Cw3Hl2mZS< zfTOkqXN`(j=S#?Pav5NALM_%T@sg+-jqmNuAg6x&?%+sAD7vyoEwApee8MBVCfV?9 z`1j}@y-eO(wfw!l=^r6h;NQ0H9VV`4iolpE0{g0|9#HuX9MCkeg0iico{DnJ9uk3# z;C{#q9#E?jHiKq(lvo4c95zGdz=2{##DGpk#Eh8XJ!KF3NDidtfEh`WVPa`LilYHc zElq$hYDP`(CgHctsO2>^%eOUR`gBj5@6 zh{4oD`}gD<{}|I+m{@T$4nu>RUjCOYhBk8j>_OevjFNDCaD*z%@zhvD9$Gt*nt$BQ zZ03~ACVH8bRoCOQw^L&1)b!bhsWYL=K%y#|NA=m$!`UHKNhk@L@rGy$A5V&AJ)!QK z!x~-b%j2$uEJ~OF$oqOH)}tF0$1jW`yn97jK$jy25V}sVv~5&%^qK2^lm3x*@X+%Y z5Ks?aMo1%!Ae=#%L(maQ2(AYAAU=Wv@;*8Tt-R`1IaKmGV@(u=QQ$xs>x?rn%j&hG zqnXqRjhKV-W}*Ay=+BgXVULYZ+J*6tS2r-m1q2ya3U{^0;CTHVx$RI3PazB=xZ8;L zuoJJgicO>15XEYvxK?E~-CoR0T*0#s=TOM=&l&c7ttMl-BzAlacC)zCd+qWSJHx|V(9?;U;M7a*RlaO5dPWF8A zcnsbK|BvkNviIitJCIi#|28WNgY#ETQ59^!m!MRuzRf)@G;9_tmQPs`%P|{p@n5FOSQ-Pb1X~!UY6bE_jjh1%wl|B}+&alg#H9y5MS@ zftG-~tXyYe-Ozal?e8JDE0?V)LhOmj*~+|{RHxM^lvIc+evNVkeBtI6I7RNsEVLj^ j?Rs;U-#{yRoWe#Nb)jzP?HYTYf=3HeqY{7%Q6yBNj&-yn`;@J6XVkdE^X`+hI0;L5K3i(kBMJg&k60#ic#M#8&HRE-f zlwDHca^Vzd#ZN0C#Rm>Of^SGjT#-0&0qvnDB-A6~0OEjnZyaSEw_VHM&b;^LeQ)Ny zd3$f=yOp>Ri-je4zWn?8=KAut@dqT>Z%Kuu8gu?4cSeU7|G0M-pPycNm;^KK19*rl z+MiRK#9}Dx2e>csnf9b_lFUz|2<3U1M{pcH77#v#hQX`64-HcYg3ZULoVDQDNSAk~8T@tPX|Tvs z!R-2BVTqjxU|0Ew%zAO2Mt^w(;a==-9H#dJ;C>`Ln>dTQE{^uK@TX+vJW8MIQNh&p zZB!WBLB*x^KM_LDcmy8lDbx!pL5h6&5b({!=W-X&OQ>2b>zdh68GjLN9|@unzfG<_ z(e*`i2phnqFQdOeiRmWed=Vu}2uO2cTT6*`y6=VP`JO<_lZTU!c)mLUE}{`ScB#4x z|2;wADs_`Hy=-psSJ9w%8)zS{vs#fCP0i4McBTgT{xFp!m8hf)^=3(168s6JHnn9K z(L>pymfWHTq^_7gl{Sg(Q)Sz4`pp39w}MU5B(F&ytlLL(D=;G2!H&)fsJ^u1S*9e= zDW-B5vXmAP@9kiV3`=%MOn|CPj}#qw*i^Q{hY>5(A~v8E8I$ZN*ha0e6>U*FX2w)v z#;uqYY0@Un(K)QOqE;*^r6oJw@qltiPo{^OR-f&&;Xnw+4kK;A{+So%5217u4<7>V=x2>1ILG3mY1%sBj%B zM$vTraI?zHl`Bkl{v5i{RiZs<9_XZ5lwilZ^M1;__{3Z$iaUm;7S@f1t`@n*>ipP$!tq429_c9Uv2On56J0r(hgyUrNWx4Q~%#0cK%6&j958om(xdM%fLR zMgiWPLS99au1gA2H`ECRY!=#w!@rOUHU@qf;dO*Hgb{>MgfWD32pb3`1TO~fvYtVO z=vi|x$}8SYfNI}ltcF2db{>pAlj;cwPrtOi+|G^tM#vA&f8*0i=IC*7CfBeJz@0QjFW_aI`Rg^tRN>0?>+EJ> zxAWTL@8Z&)$ODaT7V?fOPn#ii#+a}&t%6FNFzh}bq-5J zE=~-R=Bamk@etNR)~w z8j(ScCb%0nS~sDum8#6#G*o^Y<)0vUI~SuIq$xN(xAKCLRC3A!O@#>!#0iH!h+Vva bOT?MX!wAyUsMhxQTPQ_~N4z<=rZfKoBp9GS diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc index 3815cf9de3f3f0e04ea407fc027d29029cdf2e7c..6073e43978fb529bc61ef1ee198f6e8fcf4fe872 100644 GIT binary patch delta 23 dcmaDa`(BnWpO=@50SMY21ZPxBY~*{$2LMy72OIzZ delta 23 dcmaDa`(BnWpO=@50SKml56%#j+{pKk4**iG2HOAt diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc index 1a0e98d760de0a6aa40a714ab894ea944c321524..6e0f413b7d2a8e61aaac08d54ab8f9d86b3f99c4 100644 GIT binary patch delta 1095 zcmaKqTWl0%6vyYx_1jC=V!P=sUAA|(LrX#o!N^T&s3s;{0z6O>v)MS?bv2!KzfQT- z*^tmsZW4r}tqm`&5)uTIIzq(oATPcs3Ha7YO!U>n;2RHE>p9az5+9sNe&7E)=X~d! z@63F0cxfm#+RzYJ=(!a?`0m!7iz!#Y2CR#{SZn$%xD%U>!BO1Sv>~W9WwaCDQ=ktH zOek`br#R{qfZ~C#yhm!6@^*J!jVX>^*Bqm+lU1A7ea+VwRJoDYidr`u(=ll^$!pA; zp5+@pYt6APXbY6hg<4~+!GQx*hcXuGmM476i%qF=zw7t%!heh z*CC||;8&$r2eGUg>Yr+({zNpRmVK;F{)J1~98Ih>$gJ&z#A!Z#hH(}LTlX3lN&bOz zt-r!0oXq7x;9Tw%(vNd#ahZJALFVy+-DL2_2+VbHfN5Nl-%l}Yw#l4CKmQtB!29|3 z1m}|+D{Xw#--3VV(^j|0BrWOTvY87ux1CbWIrh)ugN|>Y9glSW4(G7cRe(i2+x0rU z75vmCVE1Pn^Eu-xVUOJ`Z_pZwO4%!V)seFNihWVJG^@MpO|W-NmdD4-j=at0MaH+- z(KDF*h$Hm(%g4v%y{LCSd?+~Aa~U>8$&PXCL4vJGWZlMSXMDowX7n&VX5hZU(Eb@# zvyA5{-r#N26i-Kf%80Ifi6~2(%#bepCb?XL*?WZO4J65 zGM*$M6#A#ui}zSlRu-?e}2d6AHn-R#(G9HtruAhFgyk~ kDY=5&P6+i%iN?kkcnMy{r~QSLaw4UQqhf7bO@hAs55F=K_W%F@ delta 1273 zcmZuvTWB0*6rO)ByR(-}(`3tTvfDMcb~{b#1>=JfQ?*fBAuU=JtLU<1ChIia+0DPR zwW+fN43ySHN_t{zEQJKBmO?LaA`~Mc`XY#k;EVHAU+shFiy#y{=Z~xxoPlq?`OZ1t zf6kft@yOMejcQw4T*B+Z?}uL>*|%o6I(rD3`XRmNuM0|@KLyufr($dthCBDP^q|z4 zQD$#rsn0CdkocG)#0WblRRZbMMXOr5g1*Sn@UU5g%(3i+Xef?b%t@C+E4g?vg*F+-uEAzcpQkf*dd{?fuX#4=#;LEN)mN-LBZxhVJFWrZ@;j_e~tkd`812e&ir2WEC%81)=Z*F1=IyrE#x0yeZlAXkL1WjQkG$=cb(;-r|)OB z0Ezwr3*f>2C)obrc)!lZFH_D31Ro+CG*kQe z+(*z$a0R|9^hLxs*?>ER-YudF(SV8ZDah^$4qm6s&j`e3D6qLV)pI7MIBhCaryGtJ zD)pIqsM;qS?;qo|W(pPFaL+)_e5Cz9Zr{OTK9jwKM%(G9Ng~lD{9kwLhQ2MC=a+(I zv&=H5Q8yF(6FQJhhLMS?JL#19*N_<;)o!A?1g{L5dx(X4u~;cNZn0=)MTh)bRQV|i zX}Fx)3?C!m2Us7RAKm~_3oc=6*W2l diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py index 823d193..e4e474b 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py @@ -13,43 +13,43 @@ def __init__(self, position, velocity=None, coord_transformer=None): # State self.x = position[0] # x position in meters self.y = position[1] # y position in meters - + # Velocity if velocity is None: velocity = [0, 0] self.velocity = velocity - + # Physical properties self.size = 0.8 # Size in meters self.max_speed = 5.5 # Maximum speed in meters per second self.color = (255, 255, 255) # White color for sheep - + # Coordinate transformer self.coord_transformer = coord_transformer - + # Movement state self.current_state = self.GRAZING # Start in grazing state self.walking_threshold = 1.0 # Threshold to start walking was 1.0 self.moving_threshold = 2.0 # Threshold to start moving quickly - + # Speed multipliers for different states self.grazing_speed_multiplier = 0.1 # Slower when grazing self.walking_speed_multiplier = 0.15 # Moderate speed when walking self.moving_speed_multiplier = 1.2 # Faster when moving quickly - + # Flocking parameters self.alignment_weight = 3.0 self.cohesion_weight = 5.0 - self.separation_weight = 6.0 - + self.separation_weight = 8.0 + # Physical properties - self.max_force = 0.2 - + self.max_force = 0.2 + # Perception parameters self.perception_radius = 20 # How far the sheep can see other sheep in meters self.protected_radius = 2 # Minimum distance sheep try to maintain from each other in meters - self.boundary_margin = 10 # Distance from edge where sheep start to turn in meters - self.dog_repulsion_weight = 6.0 # Weight for avoiding the dog + self.boundary_margin = 2 # Distance from edge where sheep start to turn in meters + self.dog_repulsion_weight = 4.0 # Weight for avoiding the dog self.dog_repulsion_radius = 20 # How far the sheep can sense the dog in meters def get_state(self): @@ -63,7 +63,7 @@ def update_state(self): """Update the sheep's movement state based on external forces""" # Calculate the magnitude of external forces force_magnitude = math.sqrt(self.acceleration[0]**2 + self.acceleration[1]**2) - + # State transitions based purely on force magnitude if force_magnitude > self.moving_threshold: self.current_state = self.MOVING @@ -96,22 +96,22 @@ def seek(self, target): """Seek behavior - move towards a target""" desired = [target[0] - self.x, target[1] - self.y] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -119,22 +119,22 @@ def flee(self, target): """Flee behavior - move away from a target""" desired = [self.x - target[0], self.y - target[1]] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -142,10 +142,10 @@ def align(self, sheep_list): """Alignment behavior - match velocity with neighbors""" if not sheep_list: return [0, 0] - + avg_velocity = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -153,29 +153,29 @@ def align(self, sheep_list): avg_velocity[0] += sheep.velocity[0] avg_velocity[1] += sheep.velocity[1] count += 1 - + if count > 0: avg_velocity[0] /= count avg_velocity[1] /= count - + # Normalize and scale magnitude = math.sqrt(avg_velocity[0]**2 + avg_velocity[1]**2) if magnitude > 0: avg_velocity[0] = (avg_velocity[0] / magnitude) * self.max_speed avg_velocity[1] = (avg_velocity[1] / magnitude) * self.max_speed - + # Calculate steering force steer = [ avg_velocity[0] - self.velocity[0], avg_velocity[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -183,10 +183,10 @@ def cohesion(self, sheep_list): """Cohesion behavior - move towards center of neighbors""" if not sheep_list: return [0, 0] - + center = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -194,7 +194,7 @@ def cohesion(self, sheep_list): center[0] += sheep.x center[1] += sheep.y count += 1 - + if count > 0: center[0] /= count center[1] /= count @@ -205,16 +205,16 @@ def separation(self, sheep_list): """Separation behavior - avoid crowding neighbors""" if not sheep_list: return [0, 0] - + steer = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: dx = self.x - sheep.x dy = self.y - sheep.y distance = math.sqrt(dx**2 + dy**2) - + if distance > 0 and distance < self.protected_radius: # Calculate repulsion force dx = dx / distance @@ -224,98 +224,99 @@ def separation(self, sheep_list): steer[0] += dx * strength steer[1] += dy * strength count += 1 - + if count > 0: steer[0] /= count steer[1] /= count - + # Normalize and scale magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if magnitude > 0: steer[0] = (steer[0] / magnitude) * self.max_speed steer[1] = (steer[1] / magnitude) * self.max_speed - + # Calculate steering force steer[0] -= self.velocity[0] steer[1] -= self.velocity[1] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer def avoid_boundaries(self): """Avoid field boundaries""" steer = [0, 0] - + # Get current position in real-world coordinates x, y = self.x, self.y - + # Check if we're too close to the boundary - if not self.coord_transformer.is_point_in_field(x, y): - # Find closest point on boundary - min_dist = float('inf') - closest_point = None - - for i in range(len(self.coord_transformer.field_boundary)): - p1 = self.coord_transformer.field_boundary[i] - p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] - - # Calculate closest point on line segment - line_vec = np.array(p2) - np.array(p1) - point_vec = np.array([x, y]) - np.array(p1) - line_len = np.linalg.norm(line_vec) - line_unitvec = line_vec / line_len - point_proj_len = np.dot(point_vec, line_unitvec) - point_proj_len = max(0, min(point_proj_len, line_len)) - - closest = np.array(p1) + line_unitvec * point_proj_len - dist = np.linalg.norm(np.array([x, y]) - closest) - - if dist < min_dist: - min_dist = dist - closest_point = closest - - if closest_point is not None: - # Calculate steering force away from boundary - steer[0] = self.x - closest_point[0] - steer[1] = self.y - closest_point[1] - - # Normalize and scale - magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if magnitude > 0: - steer[0] = (steer[0] / magnitude) * self.max_speed - steer[1] = (steer[1] / magnitude) * self.max_speed - - # Calculate steering force - steer[0] -= self.velocity[0] - steer[1] -= self.velocity[1] - - # Limit force - force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if force_magnitude > self.max_force: - steer[0] = (steer[0] / force_magnitude) * self.max_force - steer[1] = (steer[1] / force_magnitude) * self.max_force - - return steer + #if not self.coord_transformer.is_point_in_field(x, y): + # Find closest point on boundary + min_dist = float('inf') + closest_point = None + + for i in range(len(self.coord_transformer.field_boundary)): + p1 = self.coord_transformer.field_boundary[i] + p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] + + # Calculate closest point on line segment + line_vec = np.array(p2) - np.array(p1) + point_vec = np.array([x, y]) - np.array(p1) + line_len = np.linalg.norm(line_vec) + line_unitvec = line_vec / line_len + point_proj_len = np.dot(point_vec, line_unitvec) + point_proj_len = max(0, min(point_proj_len, line_len)) + + closest = np.array(p1) + line_unitvec * point_proj_len + dist = np.linalg.norm(np.array([x, y]) - closest) + + if dist < min_dist: + min_dist = dist + closest_point = closest + + if closest_point is not None: + # Calculate steering force away from boundary + steer[0] = self.x - closest_point[0] + steer[1] = self.y - closest_point[1] + + + # Normalize and scale + magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if magnitude < self.boundary_margin: + steer[0] = (steer[0] / magnitude) * self.max_speed + steer[1] = (steer[1] / magnitude) * self.max_speed + + # Calculate steering force + steer[0] -= self.velocity[0] + steer[1] -= self.velocity[1] + + # Limit force + force_magnitude = -math.sqrt(steer[0]**2 + steer[1]**2) + if force_magnitude > self.max_force: + steer[0] = (steer[0] / force_magnitude) * self.max_force + steer[1] = (steer[1] / force_magnitude) * self.max_force + + return steer return [0, 0] def avoid_dog(self, dog): """Avoid the sheepdog""" to_dog = [self.x - dog.x, self.y - dog.y] distance = math.sqrt(to_dog[0]**2 + to_dog[1]**2) - + if distance < self.dog_repulsion_radius: # Stronger repulsion when closer to the dog strength = 1.0 - (distance / self.dog_repulsion_radius) # Normalize the direction if distance > 0: to_dog[0] = to_dog[0] / distance - to_dog[1] = to_dog[1] / distance - + to_dog[1] = to_dog[1] / distance + # Apply repulsion force return [ to_dog[0] * strength * self.dog_repulsion_weight, @@ -327,12 +328,12 @@ def flock(self, sheep_list, sheepdog): """Apply flocking behaviors""" # Reset acceleration self.acceleration = [0, 0] - + # Get forces from each behavior align = self.align(sheep_list) cohere = self.cohesion(sheep_list) separate = self.separation(sheep_list) - + # Apply weights align[0] *= self.alignment_weight align[1] *= self.alignment_weight @@ -340,20 +341,20 @@ def flock(self, sheep_list, sheepdog): cohere[1] *= self.cohesion_weight separate[0] *= self.separation_weight separate[1] *= self.separation_weight - + # Apply forces self.apply_force(align) self.apply_force(cohere) self.apply_force(separate) - + # Apply sheepdog repulsion dog_force = self.avoid_dog(sheepdog) self.apply_force(dog_force) - + # Apply boundary avoidance boundary_force = self.avoid_boundaries() self.apply_force(boundary_force) - + # Update state based on movement if math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) > self.max_speed * 0.8: self.current_state = self.MOVING @@ -362,7 +363,7 @@ def flock(self, sheep_list, sheepdog): def update(self, dt, sheep_list, sheepdog): """Update sheep position and velocity - + Args: dt: Time step in seconds sheep_list: List of other sheep @@ -373,11 +374,11 @@ def update(self, dt, sheep_list, sheepdog): # Update state based on external forces self.update_state() - + # Update velocity based on acceleration self.velocity[0] += self.acceleration[0] * dt self.velocity[1] += self.acceleration[1] * dt - + # Limit speed based on state speed = math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) if self.current_state == self.GRAZING: @@ -386,15 +387,15 @@ def update(self, dt, sheep_list, sheepdog): max_speed = self.max_speed * self.walking_speed_multiplier else: # MOVING state max_speed = self.max_speed * self.moving_speed_multiplier - + if speed > max_speed: self.velocity[0] = (self.velocity[0] / speed) * max_speed self.velocity[1] = (self.velocity[1] / speed) * max_speed - + # Update position new_x = self.x + self.velocity[0] * dt new_y = self.y + self.velocity[1] * dt - + # Reset acceleration self.acceleration = [0, 0] @@ -414,13 +415,13 @@ def draw(self, screen): """Draw the sheep on the screen""" # Convert real-world coordinates to screen coordinates screen_x, screen_y = self.coord_transformer.world_to_screen(self.x, self.y) - + # Convert size from meters to screen pixels screen_size = self.size * self.coord_transformer.scale - + # Draw the sheep body pygame.draw.circle(screen, self.color, (int(screen_x), int(screen_y)), int(screen_size)) - + # Draw state indicator if self.current_state == self.GRAZING: # Draw a small green dot when grazing @@ -444,4 +445,4 @@ def limit(vector, max_value): norm = math.sqrt(vector[0]**2 + vector[1]**2) if norm > max_value: return [vector[0] / norm * max_value, vector[1] / norm * max_value] - return vector \ No newline at end of file + return vector diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 1505454..0ba8fa3 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -58,53 +58,53 @@ def handle_event(self, event): class CoordinateTransformer: def __init__(self, field_boundary, screen_width, screen_height): """Initialize coordinate transformer with field boundary and screen dimensions - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels screen_height: Height of the screen in pixels """ self.field_boundary = np.array(field_boundary) - + # Calculate field bounds in real-world coordinates self.field_min_x = np.min(self.field_boundary[:, 0]) self.field_max_x = np.max(self.field_boundary[:, 0]) self.field_min_y = np.min(self.field_boundary[:, 1]) self.field_max_y = np.max(self.field_boundary[:, 1]) - + # Screen dimensions self.screen_width = screen_width self.screen_height = screen_height - + # Calculate scaling factors self.scale_x = screen_width / (self.field_max_x - self.field_min_x) self.scale_y = screen_height / (self.field_max_y - self.field_min_y) - + # Use the smaller scale to maintain aspect ratio self.scale = min(self.scale_x, self.scale_y) - + # Calculate offsets to center the field self.offset_x = (screen_width - (self.field_max_x - self.field_min_x) * self.scale) / 2 self.offset_y = (screen_height - (self.field_max_y - self.field_min_y) * self.scale) / 2 - + def world_to_screen(self, x, y): """Convert real-world coordinates to screen coordinates""" screen_x = (x - self.field_min_x) * self.scale + self.offset_x screen_y = (y - self.field_min_y) * self.scale + self.offset_y return screen_x, screen_y - + def screen_to_world(self, screen_x, screen_y): """Convert screen coordinates to real-world coordinates""" x = (screen_x - self.offset_x) / self.scale + self.field_min_x y = (screen_y - self.offset_y) / self.scale + self.field_min_y return x, y - + def is_point_in_field(self, x, y): """Check if a point is inside the field boundary using ray casting algorithm""" point = np.array([x, y]) n = len(self.field_boundary) inside = False - + p1x, p1y = self.field_boundary[0] for i in range(n + 1): p2x, p2y = self.field_boundary[i % n] @@ -116,7 +116,7 @@ def is_point_in_field(self, x, y): if p1x == p2x or x <= xinters: inside = not inside p1x, p1y = p2x, p2y - + return inside def get_closest_boundary_point(self, x, y): @@ -124,41 +124,41 @@ def get_closest_boundary_point(self, x, y): point = np.array([x, y]) min_dist = float('inf') closest_point = None - + # Check each line segment of the boundary for i in range(len(self.field_boundary)): p1 = self.field_boundary[i] p2 = self.field_boundary[(i + 1) % len(self.field_boundary)] - + # Vector from p1 to p2 line_vec = p2 - p1 # Vector from p1 to point point_vec = point - p1 - + # Project point_vec onto line_vec line_len = np.linalg.norm(line_vec) line_unitvec = line_vec / line_len projection = np.dot(point_vec, line_unitvec) - + # Clamp projection to line segment projection = max(0, min(line_len, projection)) - + # Calculate closest point on line segment closest = p1 + projection * line_unitvec - + # Calculate distance to closest point dist = np.linalg.norm(point - closest) - + if dist < min_dist: min_dist = dist closest_point = closest - + return closest_point class Simulation: def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None): """Initialize simulation with field boundary - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels @@ -168,10 +168,10 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st """ self.screen_width = screen_width self.screen_height = screen_height - + # Initialize coordinate transformer self.coord_transformer = CoordinateTransformer(field_boundary, screen_width, screen_height) - + # Create the sheepdog if sheepdog_state is None: # Place sheepdog at center of field in real-world coordinates @@ -187,12 +187,12 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st yaw=0, # Initial yaw coord_transformer=self.coord_transformer ) - + # Create a list of sheep self.num_sheep = 50 if sheep_states is None else len(sheep_states) self.sheep_list = [] self._initialize_sheep(sheep_states) - + # Flocking parameters self.alignment_weight = 1.0 self.cohesion_weight = 0.3 @@ -206,9 +206,11 @@ def _initialize_sheep(self, sheep_states=None): for _ in range(self.num_sheep): while True: # Generate random position in real-world coordinates - x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) - y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) - + # x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) + # y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) + x = -35 + y = 40 + # Check if position is inside field boundary if self.coord_transformer.is_point_in_field(x, y): position = [x, y] @@ -234,7 +236,7 @@ def _initialize_sheep(self, sheep_states=None): def update(self, dt=0.02, sheepdog_state=None): """Update the simulation state - + Args: dt: Time step in seconds sheepdog_state: Optional dictionary with 'position' and 'velocity' keys. @@ -245,12 +247,12 @@ def update(self, dt=0.02, sheepdog_state=None): if sheepdog_state is not None: if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") - + self.sheepdog.set_position(sheepdog_state['position'][0], sheepdog_state['position'][1]) - + if 'velocity' in sheepdog_state: self.sheepdog.velocity = sheepdog_state['velocity'] - + # Update each sheep for sheep in self.sheep_list: @@ -267,18 +269,18 @@ def draw(self, screen): """Draw the simulation state""" # Fill the screen with grass color screen.fill(GREEN) - + # Draw field boundary screen_points = [] for point in self.coord_transformer.field_boundary: screen_x, screen_y = self.coord_transformer.world_to_screen(point[0], point[1]) screen_points.append((screen_x, screen_y)) pygame.draw.polygon(screen, BLACK, screen_points, 2) - + # Draw each sheep for sheep in self.sheep_list: sheep.draw(screen) - + # Draw the sheepdog self.sheepdog.draw(screen) @@ -292,32 +294,32 @@ def __init__(self, width, height): pygame.display.set_caption("Sheep Simulation") self.clock = pygame.time.Clock() self.running = True - + # Colors self.BLACK = (0, 0, 0) self.GREEN = (34, 139, 34) # Grass color self.WHITE = (255, 255, 255) self.GRAY = (128, 128, 128) 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) - + # Initialize simulation self.simulation = Simulation(field_boundary, width, height) - + # Initialize sheepdog controller self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) - + # Initialize sliders self.sliders = { 'alignment': Slider(10, height - 100, 200, 20, 0, 10, 3.0, "Alignment"), 'cohesion': Slider(10, height - 70, 200, 20, 0, 10, 5.0, "Cohesion"), 'separation': Slider(10, height - 40, 200, 20, 0, 10, 2.5, "Separation") } - + # Instructions self.font = pygame.font.Font(None, 24) self.instructions = [ @@ -333,7 +335,7 @@ def handle_events(self): elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: return False - + # Handle slider events for slider in self.sliders.values(): slider.handle_event(event) @@ -343,18 +345,18 @@ def update(self): """Update game state""" # Get time step in seconds dt = self.clock.get_time() / 1000.0 # Convert milliseconds to seconds - + # Update simulation self.simulation.update(dt) - + # Update sheepdog controller keys = pygame.key.get_pressed() self.sheepdog_controller.update(keys, dt) - + # Update sliders for slider in self.sliders.values(): slider.handle_event(pygame.event.Event(pygame.MOUSEMOTION, {'pos': pygame.mouse.get_pos()})) - + # Update simulation parameters self.simulation.alignment_weight = self.sliders['alignment'].value self.simulation.cohesion_weight = self.sliders['cohesion'].value @@ -405,7 +407,7 @@ def load_map_config(map_file): """Load map configuration from YAML file and convert coordinates to meters""" with open(map_file, 'r') as f: config = yaml.safe_load(f) - + # Convert lat/lon to meters (approximate conversion) # Using a simple conversion where 1 degree of latitude ≈ 111,320 meters # and 1 degree of longitude ≈ 111,320 * cos(latitude) meters @@ -424,10 +426,10 @@ def load_map_config(map_file): x = (lon - ref_lon) * 111320 * np.cos(np.radians(ref_lat)) y = (lat - ref_lat) * 111320 boundary_points.append([x, y]) - + return boundary_points if __name__ == "__main__": # Create and run the game game = Game(800, 600) - game.run() \ No newline at end of file + game.run() From ebe59262440ecdfb8ce82f92b7d20ae90e071023 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:13:36 +0100 Subject: [PATCH 27/59] Added clustering to sheep tracking --- .../tmpDogSim/dog_control_lib.py | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index a34e0e4..a0ba6ab 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -61,15 +61,29 @@ def cost(x, y, xd, yd, xc, yc, simulation): return angle(xvel, yvel, xveldesired, yveldesired) # penalise distance to closest sheep, reject if within 2m of sheep - +from sklearn.cluster import DBSCAN 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))): """Return optimal dog (x_d*, y_d*) given current flock and goal.""" - (xmean, ymean), radius_sheep = circle_around_points(np.stack([x, y], axis=1)) + + points = np.stack([x, y], axis=1) + + db = DBSCAN(eps=10, min_samples=1).fit(points) + labels = db.labels_ + furthest_distance, furthest_cluster = -1, -1 + for cluster in np.unique(labels): + centre_of_cluster = np.mean(points[labels==cluster],axis=0) + distance_to_goal = np.linalg.norm(centre_of_cluster-np.array([xc,yc])) + if distance_to_goal > furthest_distance: + furthest_distance = distance_to_goal + furthest_cluster = cluster + points = points[labels==furthest_cluster] + + (xmean, ymean), radius_sheep = circle_around_points(points) #radius_sheep += .5 d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) - if np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xc, yc])) < 1: + if np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xc, yc])) < 10: d = np.linalg.norm(np.asarray([-10, 10]) - np.asarray([xd, yd])) closest_point = (xd + (-10 - xd) * radius_d / d, yd + (10 - yd) * radius_d / d) points = np.array([closest_point]) From 62cd6279af39d9eb35d29892880b25fc44a4f342 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:14:44 +0100 Subject: [PATCH 28/59] Revert "Merge branch 'main' of https://github.com/LCAS/auto_shepherd_simulation" This reverts commit 70eb1bc13647af39d0d6d5de8373e923845630ec, reversing changes made to ebe59262440ecdfb8ce82f92b7d20ae90e071023. --- configs/rviz/default.rviz | 27 +-- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 178 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 9114 -> 9138 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3823 -> 3823 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11481 -> 11562 bytes .../sheep_simulation/sheep.py | 213 +++++++++--------- .../sheep_simulation/simulation.py | 98 ++++---- 8 files changed, 157 insertions(+), 181 deletions(-) diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index c2fe930..e24b2e0 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -11,7 +11,6 @@ Panels: - /Path1/Topic1 - /Path1/Offset1 - /Pose1 - - /Pose2 Splitter Ratio: 0.5 Tree Height: 1204 - Class: rviz_common/Selection @@ -116,26 +115,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /dog/command Value: true - - Alpha: 1 - Axes Length: 2.5 - Axes Radius: 0.5 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 2 - Name: Pose - Shaft Length: 5 - Shaft Radius: 0.5 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sheep/goal_pose - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -182,7 +161,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 135.89871215820312 + Distance: 173.1657257080078 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -208,7 +187,7 @@ Window Geometry: Height: 1495 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008220000003efc0100000002fb0000000800540069006d00650100000000000008220000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006c60000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007270000003efc0100000002fb0000000800540069006d00650100000000000007270000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005cb0000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -217,6 +196,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2082 + Width: 1831 X: 0 Y: 32 diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc index f8a6096bd727c9e77a09c95064a7f89f9e9107ee..782a655adf4fc320f8c8a338ce30d5adabda0d17 100644 GIT binary patch delta 21 bcmdnQxQUTBpO=@50SK}Z12dQ=@-77cGBO0v delta 21 bcmdnQxQUTBpO=@50SMN|1ZFT!7%Q6yBNj&-yn`;@J6XVkdE^X`+hI0;L5K3i(kBMJg&k60#ic#M#8&HRE-f zlwDHca^Vzd#ZN0C#Rm>Of^SGjT#-0&0qvnDB-A6~0OEjnZyaSEw_VHM&b;^LeQ)Ny zd3$f=yOp>Ri-je4zWn?8=KAut@dqT>Z%Kuu8gu?4cSeU7|G0M-pPycNm;^KK19*rl z+MiRK#9}Dx2e>csnf9b_lFUz|2<3U1M{pcH77#v#hQX`64-HcYg3ZULoVDQDNSAk~8T@tPX|Tvs z!R-2BVTqjxU|0Ew%zAO2Mt^w(;a==-9H#dJ;C>`Ln>dTQE{^uK@TX+vJW8MIQNh&p zZB!WBLB*x^KM_LDcmy8lDbx!pL5h6&5b({!=W-X&OQ>2b>zdh68GjLN9|@unzfG<_ z(e*`i2phnqFQdOeiRmWed=Vu}2uO2cTT6*`y6=VP`JO<_lZTU!c)mLUE}{`ScB#4x z|2;wADs_`Hy=-psSJ9w%8)zS{vs#fCP0i4McBTgT{xFp!m8hf)^=3(168s6JHnn9K z(L>pymfWHTq^_7gl{Sg(Q)Sz4`pp39w}MU5B(F&ytlLL(D=;G2!H&)fsJ^u1S*9e= zDW-B5vXmAP@9kiV3`=%MOn|CPj}#qw*i^Q{hY>5(A~v8E8I$ZN*ha0e6>U*FX2w)v z#;uqYY0@Un(K)QOqE;*^r6oJw@qltiPo{^OR-f&&;Xnw+4kK;A{+So%5217u4<7>V=x2>1ILG3mY1%sBj%B zM$vTraI?zHl`Bkl{v5i{RiZs<9_XZ5lwilZ^M1;__{3Z$iaUm;7S@f1t`@n*>ipP$!tq429_c9Uv2On56J0r(hgyUrNWx4Q~%#0cK%6&j958om(xdM%fLR zMgiWPLS99au1gA2H`ECRY!=#w!@rOUHU@qf;dO*Hgb{>MgfWD32pb3`1TO~fvYtVO z=vi|x$}8SYfNI}ltcF2db{>pAlj;cwPrtOi+|G^tM#vA&f8*0i=IC*7CfBeJz@0QjFW_aI`Rg^tRN>0?>+EJ> zxAWTL@8Z&)$ODaT7V?fOPn#ii#+a}&t%6FNFzh}bq-5J zE=~-R=Bamk@etNR)~w z8j(ScCb%0nS~sDum8#6#G*o^Y<)0vUI~SuIq$xN(xAKCLRC3A!O@#>!#0iH!h+Vva bOT?MX!wAyUsMhxQTPQ_~N4z<=rZfKoBp9GS delta 2047 zcmZ`)O>7%Q6rNfCt?hXI6UT9y#Ic(nFin9H5Cl@1K#^1R2O**GBO%N2P8=t;*BOt~ zgzjQc%B8nzB~W`n$pMKT!N36|?g+$z3uq5q5m(@f!h2)zCLws`Z)e`T`QA74-pqbh zxL?qWNF+qy_hsrN*)2S!X`qYUb=sMoD z8e`iy@Lc!LFjdaE0okN zrG6F(VF5VxBJ%mmtRWcZm(a3^fMJd;>tbwG>9`VI?s4=yzB&1@%iR;$Cw3Hl2mZS< zfTOkqXN`(j=S#?Pav5NALM_%T@sg+-jqmNuAg6x&?%+sAD7vyoEwApee8MBVCfV?9 z`1j}@y-eO(wfw!l=^r6h;NQ0H9VV`4iolpE0{g0|9#HuX9MCkeg0iico{DnJ9uk3# z;C{#q9#E?jHiKq(lvo4c95zGdz=2{##DGpk#Eh8XJ!KF3NDidtfEh`WVPa`LilYHc zElq$hYDP`(CgHctsO2>^%eOUR`gBj5@6 zh{4oD`}gD<{}|I+m{@T$4nu>RUjCOYhBk8j>_OevjFNDCaD*z%@zhvD9$Gt*nt$BQ zZ03~ACVH8bRoCOQw^L&1)b!bhsWYL=K%y#|NA=m$!`UHKNhk@L@rGy$A5V&AJ)!QK z!x~-b%j2$uEJ~OF$oqOH)}tF0$1jW`yn97jK$jy25V}sVv~5&%^qK2^lm3x*@X+%Y z5Ks?aMo1%!Ae=#%L(maQ2(AYAAU=Wv@;*8Tt-R`1IaKmGV@(u=QQ$xs>x?rn%j&hG zqnXqRjhKV-W}*Ay=+BgXVULYZ+J*6tS2r-m1q2ya3U{^0;CTHVx$RI3PazB=xZ8;L zuoJJgicO>15XEYvxK?E~-CoR0T*0#s=TOM=&l&c7ttMl-BzAlacC)zCd+qWSJHx|V(9?;U;M7a*RlaO5dPWF8A zcnsbK|BvkNviIitJCIi#|28WNgY#ETQ59^!m!MRuzRf)@G;9_tmQPs`%P|{p@n5FOSQ-Pb1X~!UY6bE_jjh1%wl|B}+&alg#H9y5MS@ zftG-~tXyYe-Ozal?e8JDE0?V)LhOmj*~+|{RHxM^lvIc+evNVkeBtI6I7RNsEVLj^ j?Rs;U-#{yRoWe#Nb)jzP?HYTYf=3HeqY{=JfQ?*fBAuU=JtLU<1ChIia+0DPR zwW+fN43ySHN_t{zEQJKBmO?LaA`~Mc`XY#k;EVHAU+shFiy#y{=Z~xxoPlq?`OZ1t zf6kft@yOMejcQw4T*B+Z?}uL>*|%o6I(rD3`XRmNuM0|@KLyufr($dthCBDP^q|z4 zQD$#rsn0CdkocG)#0WblRRZbMMXOr5g1*Sn@UU5g%(3i+Xef?b%t@C+E4g?vg*F+-uEAzcpQkf*dd{?fuX#4=#;LEN)mN-LBZxhVJFWrZ@;j_e~tkd`812e&ir2WEC%81)=Z*F1=IyrE#x0yeZlAXkL1WjQkG$=cb(;-r|)OB z0Ezwr3*f>2C)obrc)!lZFH_D31Ro+CG*kQe z+(*z$a0R|9^hLxs*?>ER-YudF(SV8ZDah^$4qm6s&j`e3D6qLV)pI7MIBhCaryGtJ zD)pIqsM;qS?;qo|W(pPFaL+)_e5Cz9Zr{OTK9jwKM%(G9Ng~lD{9kwLhQ2MC=a+(I zv&=H5Q8yF(6FQJhhLMS?JL#19*N_<;)o!A?1g{L5dx(X4u~;cNZn0=)MTh)bRQV|i zX}Fx)3?C!m2Us7RAKm~_3oc=6*W2l delta 1095 zcmaKqTWl0%6vyYx_1jC=V!P=sUAA|(LrX#o!N^T&s3s;{0z6O>v)MS?bv2!KzfQT- z*^tmsZW4r}tqm`&5)uTIIzq(oATPcs3Ha7YO!U>n;2RHE>p9az5+9sNe&7E)=X~d! z@63F0cxfm#+RzYJ=(!a?`0m!7iz!#Y2CR#{SZn$%xD%U>!BO1Sv>~W9WwaCDQ=ktH zOek`br#R{qfZ~C#yhm!6@^*J!jVX>^*Bqm+lU1A7ea+VwRJoDYidr`u(=ll^$!pA; zp5+@pYt6APXbY6hg<4~+!GQx*hcXuGmM476i%qF=zw7t%!heh z*CC||;8&$r2eGUg>Yr+({zNpRmVK;F{)J1~98Ih>$gJ&z#A!Z#hH(}LTlX3lN&bOz zt-r!0oXq7x;9Tw%(vNd#ahZJALFVy+-DL2_2+VbHfN5Nl-%l}Yw#l4CKmQtB!29|3 z1m}|+D{Xw#--3VV(^j|0BrWOTvY87ux1CbWIrh)ugN|>Y9glSW4(G7cRe(i2+x0rU z75vmCVE1Pn^Eu-xVUOJ`Z_pZwO4%!V)seFNihWVJG^@MpO|W-NmdD4-j=at0MaH+- z(KDF*h$Hm(%g4v%y{LCSd?+~Aa~U>8$&PXCL4vJGWZlMSXMDowX7n&VX5hZU(Eb@# zvyA5{-r#N26i-Kf%80Ifi6~2(%#bepCb?XL*?WZO4J65 zGM*$M6#A#ui}zSlRu-?e}2d6AHn-R#(G9HtruAhFgyk~ kDY=5&P6+i%iN?kkcnMy{r~QSLaw4UQqhf7bO@hAs55F=K_W%F@ diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py index e4e474b..823d193 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py @@ -13,43 +13,43 @@ def __init__(self, position, velocity=None, coord_transformer=None): # State self.x = position[0] # x position in meters self.y = position[1] # y position in meters - + # Velocity if velocity is None: velocity = [0, 0] self.velocity = velocity - + # Physical properties self.size = 0.8 # Size in meters self.max_speed = 5.5 # Maximum speed in meters per second self.color = (255, 255, 255) # White color for sheep - + # Coordinate transformer self.coord_transformer = coord_transformer - + # Movement state self.current_state = self.GRAZING # Start in grazing state self.walking_threshold = 1.0 # Threshold to start walking was 1.0 self.moving_threshold = 2.0 # Threshold to start moving quickly - + # Speed multipliers for different states self.grazing_speed_multiplier = 0.1 # Slower when grazing self.walking_speed_multiplier = 0.15 # Moderate speed when walking self.moving_speed_multiplier = 1.2 # Faster when moving quickly - + # Flocking parameters self.alignment_weight = 3.0 self.cohesion_weight = 5.0 - self.separation_weight = 8.0 - + self.separation_weight = 6.0 + # Physical properties - self.max_force = 0.2 - + self.max_force = 0.2 + # Perception parameters self.perception_radius = 20 # How far the sheep can see other sheep in meters self.protected_radius = 2 # Minimum distance sheep try to maintain from each other in meters - self.boundary_margin = 2 # Distance from edge where sheep start to turn in meters - self.dog_repulsion_weight = 4.0 # Weight for avoiding the dog + self.boundary_margin = 10 # Distance from edge where sheep start to turn in meters + self.dog_repulsion_weight = 6.0 # Weight for avoiding the dog self.dog_repulsion_radius = 20 # How far the sheep can sense the dog in meters def get_state(self): @@ -63,7 +63,7 @@ def update_state(self): """Update the sheep's movement state based on external forces""" # Calculate the magnitude of external forces force_magnitude = math.sqrt(self.acceleration[0]**2 + self.acceleration[1]**2) - + # State transitions based purely on force magnitude if force_magnitude > self.moving_threshold: self.current_state = self.MOVING @@ -96,22 +96,22 @@ def seek(self, target): """Seek behavior - move towards a target""" desired = [target[0] - self.x, target[1] - self.y] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -119,22 +119,22 @@ def flee(self, target): """Flee behavior - move away from a target""" desired = [self.x - target[0], self.y - target[1]] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -142,10 +142,10 @@ def align(self, sheep_list): """Alignment behavior - match velocity with neighbors""" if not sheep_list: return [0, 0] - + avg_velocity = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -153,29 +153,29 @@ def align(self, sheep_list): avg_velocity[0] += sheep.velocity[0] avg_velocity[1] += sheep.velocity[1] count += 1 - + if count > 0: avg_velocity[0] /= count avg_velocity[1] /= count - + # Normalize and scale magnitude = math.sqrt(avg_velocity[0]**2 + avg_velocity[1]**2) if magnitude > 0: avg_velocity[0] = (avg_velocity[0] / magnitude) * self.max_speed avg_velocity[1] = (avg_velocity[1] / magnitude) * self.max_speed - + # Calculate steering force steer = [ avg_velocity[0] - self.velocity[0], avg_velocity[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -183,10 +183,10 @@ def cohesion(self, sheep_list): """Cohesion behavior - move towards center of neighbors""" if not sheep_list: return [0, 0] - + center = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -194,7 +194,7 @@ def cohesion(self, sheep_list): center[0] += sheep.x center[1] += sheep.y count += 1 - + if count > 0: center[0] /= count center[1] /= count @@ -205,16 +205,16 @@ def separation(self, sheep_list): """Separation behavior - avoid crowding neighbors""" if not sheep_list: return [0, 0] - + steer = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: dx = self.x - sheep.x dy = self.y - sheep.y distance = math.sqrt(dx**2 + dy**2) - + if distance > 0 and distance < self.protected_radius: # Calculate repulsion force dx = dx / distance @@ -224,99 +224,98 @@ def separation(self, sheep_list): steer[0] += dx * strength steer[1] += dy * strength count += 1 - + if count > 0: steer[0] /= count steer[1] /= count - + # Normalize and scale magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if magnitude > 0: steer[0] = (steer[0] / magnitude) * self.max_speed steer[1] = (steer[1] / magnitude) * self.max_speed - + # Calculate steering force steer[0] -= self.velocity[0] steer[1] -= self.velocity[1] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer def avoid_boundaries(self): """Avoid field boundaries""" steer = [0, 0] - + # Get current position in real-world coordinates x, y = self.x, self.y - + # Check if we're too close to the boundary - #if not self.coord_transformer.is_point_in_field(x, y): - # Find closest point on boundary - min_dist = float('inf') - closest_point = None - - for i in range(len(self.coord_transformer.field_boundary)): - p1 = self.coord_transformer.field_boundary[i] - p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] - - # Calculate closest point on line segment - line_vec = np.array(p2) - np.array(p1) - point_vec = np.array([x, y]) - np.array(p1) - line_len = np.linalg.norm(line_vec) - line_unitvec = line_vec / line_len - point_proj_len = np.dot(point_vec, line_unitvec) - point_proj_len = max(0, min(point_proj_len, line_len)) - - closest = np.array(p1) + line_unitvec * point_proj_len - dist = np.linalg.norm(np.array([x, y]) - closest) - - if dist < min_dist: - min_dist = dist - closest_point = closest - - if closest_point is not None: - # Calculate steering force away from boundary - steer[0] = self.x - closest_point[0] - steer[1] = self.y - closest_point[1] - - - # Normalize and scale - magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if magnitude < self.boundary_margin: - steer[0] = (steer[0] / magnitude) * self.max_speed - steer[1] = (steer[1] / magnitude) * self.max_speed - - # Calculate steering force - steer[0] -= self.velocity[0] - steer[1] -= self.velocity[1] - - # Limit force - force_magnitude = -math.sqrt(steer[0]**2 + steer[1]**2) - if force_magnitude > self.max_force: - steer[0] = (steer[0] / force_magnitude) * self.max_force - steer[1] = (steer[1] / force_magnitude) * self.max_force - - return steer + if not self.coord_transformer.is_point_in_field(x, y): + # Find closest point on boundary + min_dist = float('inf') + closest_point = None + + for i in range(len(self.coord_transformer.field_boundary)): + p1 = self.coord_transformer.field_boundary[i] + p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] + + # Calculate closest point on line segment + line_vec = np.array(p2) - np.array(p1) + point_vec = np.array([x, y]) - np.array(p1) + line_len = np.linalg.norm(line_vec) + line_unitvec = line_vec / line_len + point_proj_len = np.dot(point_vec, line_unitvec) + point_proj_len = max(0, min(point_proj_len, line_len)) + + closest = np.array(p1) + line_unitvec * point_proj_len + dist = np.linalg.norm(np.array([x, y]) - closest) + + if dist < min_dist: + min_dist = dist + closest_point = closest + + if closest_point is not None: + # Calculate steering force away from boundary + steer[0] = self.x - closest_point[0] + steer[1] = self.y - closest_point[1] + + # Normalize and scale + magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if magnitude > 0: + steer[0] = (steer[0] / magnitude) * self.max_speed + steer[1] = (steer[1] / magnitude) * self.max_speed + + # Calculate steering force + steer[0] -= self.velocity[0] + steer[1] -= self.velocity[1] + + # Limit force + force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if force_magnitude > self.max_force: + steer[0] = (steer[0] / force_magnitude) * self.max_force + steer[1] = (steer[1] / force_magnitude) * self.max_force + + return steer return [0, 0] def avoid_dog(self, dog): """Avoid the sheepdog""" to_dog = [self.x - dog.x, self.y - dog.y] distance = math.sqrt(to_dog[0]**2 + to_dog[1]**2) - + if distance < self.dog_repulsion_radius: # Stronger repulsion when closer to the dog strength = 1.0 - (distance / self.dog_repulsion_radius) # Normalize the direction if distance > 0: to_dog[0] = to_dog[0] / distance - to_dog[1] = to_dog[1] / distance - + to_dog[1] = to_dog[1] / distance + # Apply repulsion force return [ to_dog[0] * strength * self.dog_repulsion_weight, @@ -328,12 +327,12 @@ def flock(self, sheep_list, sheepdog): """Apply flocking behaviors""" # Reset acceleration self.acceleration = [0, 0] - + # Get forces from each behavior align = self.align(sheep_list) cohere = self.cohesion(sheep_list) separate = self.separation(sheep_list) - + # Apply weights align[0] *= self.alignment_weight align[1] *= self.alignment_weight @@ -341,20 +340,20 @@ def flock(self, sheep_list, sheepdog): cohere[1] *= self.cohesion_weight separate[0] *= self.separation_weight separate[1] *= self.separation_weight - + # Apply forces self.apply_force(align) self.apply_force(cohere) self.apply_force(separate) - + # Apply sheepdog repulsion dog_force = self.avoid_dog(sheepdog) self.apply_force(dog_force) - + # Apply boundary avoidance boundary_force = self.avoid_boundaries() self.apply_force(boundary_force) - + # Update state based on movement if math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) > self.max_speed * 0.8: self.current_state = self.MOVING @@ -363,7 +362,7 @@ def flock(self, sheep_list, sheepdog): def update(self, dt, sheep_list, sheepdog): """Update sheep position and velocity - + Args: dt: Time step in seconds sheep_list: List of other sheep @@ -374,11 +373,11 @@ def update(self, dt, sheep_list, sheepdog): # Update state based on external forces self.update_state() - + # Update velocity based on acceleration self.velocity[0] += self.acceleration[0] * dt self.velocity[1] += self.acceleration[1] * dt - + # Limit speed based on state speed = math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) if self.current_state == self.GRAZING: @@ -387,15 +386,15 @@ def update(self, dt, sheep_list, sheepdog): max_speed = self.max_speed * self.walking_speed_multiplier else: # MOVING state max_speed = self.max_speed * self.moving_speed_multiplier - + if speed > max_speed: self.velocity[0] = (self.velocity[0] / speed) * max_speed self.velocity[1] = (self.velocity[1] / speed) * max_speed - + # Update position new_x = self.x + self.velocity[0] * dt new_y = self.y + self.velocity[1] * dt - + # Reset acceleration self.acceleration = [0, 0] @@ -415,13 +414,13 @@ def draw(self, screen): """Draw the sheep on the screen""" # Convert real-world coordinates to screen coordinates screen_x, screen_y = self.coord_transformer.world_to_screen(self.x, self.y) - + # Convert size from meters to screen pixels screen_size = self.size * self.coord_transformer.scale - + # Draw the sheep body pygame.draw.circle(screen, self.color, (int(screen_x), int(screen_y)), int(screen_size)) - + # Draw state indicator if self.current_state == self.GRAZING: # Draw a small green dot when grazing @@ -445,4 +444,4 @@ def limit(vector, max_value): norm = math.sqrt(vector[0]**2 + vector[1]**2) if norm > max_value: return [vector[0] / norm * max_value, vector[1] / norm * max_value] - return vector + return vector \ No newline at end of file diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 0ba8fa3..1505454 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -58,53 +58,53 @@ def handle_event(self, event): class CoordinateTransformer: def __init__(self, field_boundary, screen_width, screen_height): """Initialize coordinate transformer with field boundary and screen dimensions - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels screen_height: Height of the screen in pixels """ self.field_boundary = np.array(field_boundary) - + # Calculate field bounds in real-world coordinates self.field_min_x = np.min(self.field_boundary[:, 0]) self.field_max_x = np.max(self.field_boundary[:, 0]) self.field_min_y = np.min(self.field_boundary[:, 1]) self.field_max_y = np.max(self.field_boundary[:, 1]) - + # Screen dimensions self.screen_width = screen_width self.screen_height = screen_height - + # Calculate scaling factors self.scale_x = screen_width / (self.field_max_x - self.field_min_x) self.scale_y = screen_height / (self.field_max_y - self.field_min_y) - + # Use the smaller scale to maintain aspect ratio self.scale = min(self.scale_x, self.scale_y) - + # Calculate offsets to center the field self.offset_x = (screen_width - (self.field_max_x - self.field_min_x) * self.scale) / 2 self.offset_y = (screen_height - (self.field_max_y - self.field_min_y) * self.scale) / 2 - + def world_to_screen(self, x, y): """Convert real-world coordinates to screen coordinates""" screen_x = (x - self.field_min_x) * self.scale + self.offset_x screen_y = (y - self.field_min_y) * self.scale + self.offset_y return screen_x, screen_y - + def screen_to_world(self, screen_x, screen_y): """Convert screen coordinates to real-world coordinates""" x = (screen_x - self.offset_x) / self.scale + self.field_min_x y = (screen_y - self.offset_y) / self.scale + self.field_min_y return x, y - + def is_point_in_field(self, x, y): """Check if a point is inside the field boundary using ray casting algorithm""" point = np.array([x, y]) n = len(self.field_boundary) inside = False - + p1x, p1y = self.field_boundary[0] for i in range(n + 1): p2x, p2y = self.field_boundary[i % n] @@ -116,7 +116,7 @@ def is_point_in_field(self, x, y): if p1x == p2x or x <= xinters: inside = not inside p1x, p1y = p2x, p2y - + return inside def get_closest_boundary_point(self, x, y): @@ -124,41 +124,41 @@ def get_closest_boundary_point(self, x, y): point = np.array([x, y]) min_dist = float('inf') closest_point = None - + # Check each line segment of the boundary for i in range(len(self.field_boundary)): p1 = self.field_boundary[i] p2 = self.field_boundary[(i + 1) % len(self.field_boundary)] - + # Vector from p1 to p2 line_vec = p2 - p1 # Vector from p1 to point point_vec = point - p1 - + # Project point_vec onto line_vec line_len = np.linalg.norm(line_vec) line_unitvec = line_vec / line_len projection = np.dot(point_vec, line_unitvec) - + # Clamp projection to line segment projection = max(0, min(line_len, projection)) - + # Calculate closest point on line segment closest = p1 + projection * line_unitvec - + # Calculate distance to closest point dist = np.linalg.norm(point - closest) - + if dist < min_dist: min_dist = dist closest_point = closest - + return closest_point class Simulation: def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None): """Initialize simulation with field boundary - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels @@ -168,10 +168,10 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st """ self.screen_width = screen_width self.screen_height = screen_height - + # Initialize coordinate transformer self.coord_transformer = CoordinateTransformer(field_boundary, screen_width, screen_height) - + # Create the sheepdog if sheepdog_state is None: # Place sheepdog at center of field in real-world coordinates @@ -187,12 +187,12 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st yaw=0, # Initial yaw coord_transformer=self.coord_transformer ) - + # Create a list of sheep self.num_sheep = 50 if sheep_states is None else len(sheep_states) self.sheep_list = [] self._initialize_sheep(sheep_states) - + # Flocking parameters self.alignment_weight = 1.0 self.cohesion_weight = 0.3 @@ -206,11 +206,9 @@ def _initialize_sheep(self, sheep_states=None): for _ in range(self.num_sheep): while True: # Generate random position in real-world coordinates - # x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) - # y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) - x = -35 - y = 40 - + x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) + y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) + # Check if position is inside field boundary if self.coord_transformer.is_point_in_field(x, y): position = [x, y] @@ -236,7 +234,7 @@ def _initialize_sheep(self, sheep_states=None): def update(self, dt=0.02, sheepdog_state=None): """Update the simulation state - + Args: dt: Time step in seconds sheepdog_state: Optional dictionary with 'position' and 'velocity' keys. @@ -247,12 +245,12 @@ def update(self, dt=0.02, sheepdog_state=None): if sheepdog_state is not None: if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") - + self.sheepdog.set_position(sheepdog_state['position'][0], sheepdog_state['position'][1]) - + if 'velocity' in sheepdog_state: self.sheepdog.velocity = sheepdog_state['velocity'] - + # Update each sheep for sheep in self.sheep_list: @@ -269,18 +267,18 @@ def draw(self, screen): """Draw the simulation state""" # Fill the screen with grass color screen.fill(GREEN) - + # Draw field boundary screen_points = [] for point in self.coord_transformer.field_boundary: screen_x, screen_y = self.coord_transformer.world_to_screen(point[0], point[1]) screen_points.append((screen_x, screen_y)) pygame.draw.polygon(screen, BLACK, screen_points, 2) - + # Draw each sheep for sheep in self.sheep_list: sheep.draw(screen) - + # Draw the sheepdog self.sheepdog.draw(screen) @@ -294,32 +292,32 @@ def __init__(self, width, height): pygame.display.set_caption("Sheep Simulation") self.clock = pygame.time.Clock() self.running = True - + # Colors self.BLACK = (0, 0, 0) self.GREEN = (34, 139, 34) # Grass color self.WHITE = (255, 255, 255) self.GRAY = (128, 128, 128) 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) - + # Initialize simulation self.simulation = Simulation(field_boundary, width, height) - + # Initialize sheepdog controller self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) - + # Initialize sliders self.sliders = { 'alignment': Slider(10, height - 100, 200, 20, 0, 10, 3.0, "Alignment"), 'cohesion': Slider(10, height - 70, 200, 20, 0, 10, 5.0, "Cohesion"), 'separation': Slider(10, height - 40, 200, 20, 0, 10, 2.5, "Separation") } - + # Instructions self.font = pygame.font.Font(None, 24) self.instructions = [ @@ -335,7 +333,7 @@ def handle_events(self): elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: return False - + # Handle slider events for slider in self.sliders.values(): slider.handle_event(event) @@ -345,18 +343,18 @@ def update(self): """Update game state""" # Get time step in seconds dt = self.clock.get_time() / 1000.0 # Convert milliseconds to seconds - + # Update simulation self.simulation.update(dt) - + # Update sheepdog controller keys = pygame.key.get_pressed() self.sheepdog_controller.update(keys, dt) - + # Update sliders for slider in self.sliders.values(): slider.handle_event(pygame.event.Event(pygame.MOUSEMOTION, {'pos': pygame.mouse.get_pos()})) - + # Update simulation parameters self.simulation.alignment_weight = self.sliders['alignment'].value self.simulation.cohesion_weight = self.sliders['cohesion'].value @@ -407,7 +405,7 @@ def load_map_config(map_file): """Load map configuration from YAML file and convert coordinates to meters""" with open(map_file, 'r') as f: config = yaml.safe_load(f) - + # Convert lat/lon to meters (approximate conversion) # Using a simple conversion where 1 degree of latitude ≈ 111,320 meters # and 1 degree of longitude ≈ 111,320 * cos(latitude) meters @@ -426,10 +424,10 @@ def load_map_config(map_file): x = (lon - ref_lon) * 111320 * np.cos(np.radians(ref_lat)) y = (lat - ref_lat) * 111320 boundary_points.append([x, y]) - + return boundary_points if __name__ == "__main__": # Create and run the game game = Game(800, 600) - game.run() + game.run() \ No newline at end of file From e33ab03045d04739368d984665cd6c75c0c57017 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:17:48 +0100 Subject: [PATCH 29/59] Restructuring --- .../tmpDogSim/dog_control_lib.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index a0ba6ab..1f4e64d 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -4,6 +4,7 @@ import matplotlib.pyplot as plt from matplotlib.path import Path import numpy as np +from sklearn.cluster import DBSCAN from auto_shepherd_simulation.sheep_simulation.simulation import Simulation from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter @@ -61,23 +62,23 @@ def cost(x, y, xd, yd, xc, yc, simulation): return angle(xvel, yvel, xveldesired, yveldesired) # penalise distance to closest sheep, reject if within 2m of sheep -from sklearn.cluster import DBSCAN 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))): """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]) db = DBSCAN(eps=10, min_samples=1).fit(points) labels = db.labels_ furthest_distance, furthest_cluster = -1, -1 + print(f"{len(np.unique(labels))} clusters found") for cluster in np.unique(labels): centre_of_cluster = np.mean(points[labels==cluster],axis=0) - distance_to_goal = np.linalg.norm(centre_of_cluster-np.array([xc,yc])) + distance_to_goal = np.linalg.norm(centre_of_cluster-goal_point) if distance_to_goal > furthest_distance: - furthest_distance = distance_to_goal - furthest_cluster = cluster + furthest_distance, furthest_cluster = distance_to_goal, cluster points = points[labels==furthest_cluster] (xmean, ymean), radius_sheep = circle_around_points(points) From c41b8f1b16ab90bee3813e9534d3aed20d076755 Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:51:40 +0100 Subject: [PATCH 30/59] sim changes --- configs/rviz/default.rviz | 33 ++- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 178 bytes .../auto_shepherd_simulation/dog_control.py | 17 +- .../auto_shepherd_simulation/ros_interface.py | 2 +- .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 195 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 9138 -> 9133 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3823 -> 3823 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11562 -> 11596 bytes .../sheep_simulation/sheep.py | 210 +++++++++--------- .../sheep_simulation/simulation.py | 95 ++++---- .../tmpDogSim/dog_control_lib.py | 2 +- 11 files changed, 192 insertions(+), 167 deletions(-) diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index e24b2e0..5c3769a 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -11,6 +11,7 @@ Panels: - /Path1/Topic1 - /Path1/Offset1 - /Pose1 + - /Pose2 Splitter Ratio: 0.5 Tree Height: 1204 - Class: rviz_common/Selection @@ -115,6 +116,26 @@ Visualization Manager: Reliability Policy: Reliable Value: /dog/command Value: true + - Alpha: 1 + Axes Length: 3 + Axes Radius: 0.5 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/goal_pose + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -161,25 +182,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 173.1657257080078 + Distance: 140.7987060546875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -44.59016036987305 - Y: 47.47283935546875 - Z: 7.680665493011475 + X: -55.264869689941406 + Y: 46.4901237487793 + Z: 9.337041854858398 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.3597965240478516 + Pitch: 1.0947966575622559 Target Frame: Value: Orbit (rviz) - Yaw: 4.8685808181762695 + Yaw: 5.108584880828857 Saved: ~ Window Geometry: Displays: diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc index 782a655adf4fc320f8c8a338ce30d5adabda0d17..f8a6096bd727c9e77a09c95064a7f89f9e9107ee 100644 GIT binary patch delta 21 bcmdnQxQUTBpO=@50SMN|1ZFT!7%Q6rR~1d;RY?{>z`^G-;?w3X~tIfu>EM$U$ui2vp#JEH^uGoY>xEJWdms z#X$iHacZ>^QY(<+1BV`<0>h0vLgHR&4@igu;##=Fys^kSX~34hoq0dsdo%mq^S8yX zicu>P2_o^?OTD>fE`AgJ1^eS1m6GOw^C$i&mtXk(?j892bm@tFvPk{*J1(gCl4IK(Foq+Y^Axw2E`e%3v*j?Z9tJhz-R-_5g z4uZu0*5cJGAR3T*T5gzD-SxK`)m4kuX%q}o05Y0JC!Cl4Plo!~owol$t{^N5ij3^) z!f}NhNq`ye<7qF?$AIq)fTWk_8xh-kK0%)fPRE}BU0;mumEgyC>RFIJcT6R=uC`hu zWE&JObpHxrWyTBO1sw&w3?+j?yh1?4%SUHVp9hveyMPNC83?XxaO|n_0WmdHsKRdZWisR1h!J#%&Ol2S2nyK14Q<=umy1wq) z$NTs)de^jly*>90A>04&+SD@0vrHqQGnoIt;6?|_r@w=<$PUQ0!qtj$L>^@R^}s=p z2Rhhn>ltK+cz}mSkR28dAs*!6j$%hx*u*TtBRsUP?86=c2x1=Qkt7mc(H>2I5FX8< z4z^=F#+uZ)zRttsJ2U@d9YXd19}xC~8=A0J9G05He&_4dnWrmD_kXzi@WFc*Ww#fS zUJHxnsaa*_>Y@)T#8qpgAv@nmr01UU$|H9|c_ez}AxpPcQqFdorB18?=kN6J{AEF6 zC{fkWHJnt!coZkqERJIXhXp2-VQeTG*08b{R1!)8=hQGx!&gn?eE02492ZY2-8|$r zE})?I=7DNj71zhCl6dx7pa>@Y?R`J{D{jLKp)UeJW^@T43oryQ4Dbwq2~Y;`F2w8W z$3Y>-+l*Kh46kWKW2_Of30XiC(Vx~Dp+tyfGdQu4T>o6_o*(%QXE{GH z8Ut!o#P8jf%w1lYcx}$7`LpppUI^22e$0O{b$H@};`+A93bQDE{a6;xClhyOIS6e5 z;veb*@qqEVQ`#=qTI5lOm5HndSr_sxZGlNk zfE#WyG38e3gsoX7ZG-$n0Pk#aHHDBfKKasX40Q5{v*4{aTJ_CcdJUw?$!^FsF4Qfv URU;R%uxP?98^{n%WQZ2}2R1F0%K!iX delta 2053 zcmaJ?O>7%g5Z?E8*WUO)iQ_o`&Tng)sDKJmXbUNUCQ%DT3gYJgmgC(xPV8OtyiOB% ziz7^rNRd`hwGt?i;7}wE;4>F)NZgV3fP^@pN?Z^J5EqzvjtDxyBEfDh<&t{T{eyfsm|pnx{yluYf9_!t)R+SB5O=gb zrVfclk=G6IKw?v^F=dR*O(F~VS($~AjvO-xn?QQ%7^YiW>L+Bm4;WtHrCYD9yu8GE zQHs92;Op0J-auB5kh8q0TNN)@H%sdltFSmKrVxZTg-y6C!HeMz>uxjnWxdA61;>`hdMG) z^g!m6%ZGHI)HGYsX_GjLE<38N+5wbr2AiZwUXb2ebB@+LFd#X>wu}dKrC;(jQxeED zTRZ6D+946&M(~iNC8tX)z*UhR3EKRSt!;)5!o2H{IDkAnBsme#jqnhU98xE0M|EPy zc$9~mw25mp4tsfoN0U;&jVS^|4<9_LDWOJYqPcN89T)OW_Ys>^ko@XVa_BRAlyAGhyn4FL`s*og&kYSRP(sHYV z*j5g{?e1AdmCniVdHQEk#__`z5ne=CK^Q<7L>NN2gs_fKK=5159|;#wASUB1P+86I zH|QydyNXdPy1x%T(K-KGb3?xn@~zuFGO1;b9{1A77q6Dl+>;2xg^1C2EBxTh?HIA> zmh~f~5&RTkJ+K=9?&Z<3t2kny-EA>uSP@rht!fy$Fy*PHaXYuu{_L_haBFA6?vC!0 zaZzCL+ZHS9IY@7EY&9?hp=F_R; zGiIq5*ft8b#n|epDBR}wy;%pD?*qV}SOY#dfp>D-`EuPjyJ2}^wQ(E(6IV5Z`_CbW zEQ=e)Da@`SoGJ%#>*D^!y)ab4zYH%@vtbYxDu%sb>1+@AA0YT=6Vx;$e{gc~@jA-H Z5$Aw{EY~a5UG@rcVTPXF=6*Dp`4_rsnIr%J diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc index 3815cf9de3f3f0e04ea407fc027d29029cdf2e7c..6073e43978fb529bc61ef1ee198f6e8fcf4fe872 100644 GIT binary patch delta 23 dcmaDa`(BnWpO=@50SMY21ZPxBY~*{$2LMy72OIzZ delta 23 dcmaDa`(BnWpO=@50SKml56%#j+{pKk4**iG2HOAt diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc index 1a0e98d760de0a6aa40a714ab894ea944c321524..1bd3182468a3109a8575fa4dad4008e6313c0ee3 100644 GIT binary patch delta 768 zcmYL{O-vI(6vy|quuPln^3fCuElVk2S=v%Rq8`AYk;H=sP1I-`KSC@)JhXILAQmbD z9t@($i|as!p%c99`s;Oa@2#~^q`3IR@6!6mp5v`%5rQ}dR!HE zECWcX^Km#O>1xj~b~?jG%DD~|pI(WqzWF?@l`h?h5!XUFKPQ|;d2k@!lj+rM_}X<0 zF5-r`y=O0xm#EKf3|9x442ng8l5Hba{SIIhBkp~W!^`dtn8YRbp3>_gw1Ds3+m`H4 z{7~x^LwgK&ogWU}7B_sFaECA=P*l>rnM(u7bbqElnczd@&k-gB^b)R7j3u7oH!zb#6WSCQ!g}9DyLpAtIbK1goKXWT(JcC=Z*ErB{4J#lOMhM7)^ z6PRj!A!dr`ZMFZa|7%gS^1;M3a=pt&PtPd@--K$td{J0F5d9TlFo^+cV=8kg8ecTN w>V5RgXM<6g#kF8J+`@0cg}<*e7xKd*J`Z)kUi=;ksM45fWy7q_PQPmFH~VVF!~g&Q delta 745 zcmYL`O-NKx6vyYx$Lr2x9JO)2XP&Q_M(2&ABoQqvBrIwX%tA(RC}KcKD?hmN2656% ziP8#WJf>+gt5&&*cr9|#HVTRmqJ?dAC4CU?T1nmW(z6)x(~4Uj zMrPb$Z%}P;;FX5YR;?b-c`H_p;jZ^Q&cr;J@i`!cSzn7)24mLu3E(DP4jhLGEC)JZ z8b1Wu9h0KNJG2FVYk50ZEZNb%yxy|CMH+NbjwTn&dDWf6YI_3#!Tu0|A)a6NKTK{H)IvvJDj zCBmY>GvEqtYaRy$Hlryj1I%K(qDX&zm{rMZ9IRj88XKw%(i5_;;P#O-P{MeOgMv%3Z}1SO_P{wc~>%T?LDZ*`m{ayJ2-b*Ovo;}7|;wi)*k)x5I5|#-^2?@fIz$lJ3_ezf` zT)~gcQST~AbCBnx_E&{uPON$<*;H%RFw<%2>sU^{7BgkERPcZGe=Rm`yf{3E3tFes zd self.moving_threshold: self.current_state = self.MOVING @@ -96,22 +96,22 @@ def seek(self, target): """Seek behavior - move towards a target""" desired = [target[0] - self.x, target[1] - self.y] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -119,22 +119,22 @@ def flee(self, target): """Flee behavior - move away from a target""" desired = [self.x - target[0], self.y - target[1]] distance = math.sqrt(desired[0]**2 + desired[1]**2) - + if distance > 0: desired[0] = (desired[0] / distance) * self.max_speed desired[1] = (desired[1] / distance) * self.max_speed - + steer = [ desired[0] - self.velocity[0], desired[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -142,10 +142,10 @@ def align(self, sheep_list): """Alignment behavior - match velocity with neighbors""" if not sheep_list: return [0, 0] - + avg_velocity = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -153,29 +153,29 @@ def align(self, sheep_list): avg_velocity[0] += sheep.velocity[0] avg_velocity[1] += sheep.velocity[1] count += 1 - + if count > 0: avg_velocity[0] /= count avg_velocity[1] /= count - + # Normalize and scale magnitude = math.sqrt(avg_velocity[0]**2 + avg_velocity[1]**2) if magnitude > 0: avg_velocity[0] = (avg_velocity[0] / magnitude) * self.max_speed avg_velocity[1] = (avg_velocity[1] / magnitude) * self.max_speed - + # Calculate steering force steer = [ avg_velocity[0] - self.velocity[0], avg_velocity[1] - self.velocity[1] ] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer return [0, 0] @@ -183,10 +183,10 @@ def cohesion(self, sheep_list): """Cohesion behavior - move towards center of neighbors""" if not sheep_list: return [0, 0] - + center = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: distance = math.sqrt((self.x - sheep.x)**2 + (self.y - sheep.y)**2) @@ -194,7 +194,7 @@ def cohesion(self, sheep_list): center[0] += sheep.x center[1] += sheep.y count += 1 - + if count > 0: center[0] /= count center[1] /= count @@ -205,16 +205,16 @@ def separation(self, sheep_list): """Separation behavior - avoid crowding neighbors""" if not sheep_list: return [0, 0] - + steer = [0, 0] count = 0 - + for sheep in sheep_list: if sheep != self: dx = self.x - sheep.x dy = self.y - sheep.y distance = math.sqrt(dx**2 + dy**2) - + if distance > 0 and distance < self.protected_radius: # Calculate repulsion force dx = dx / distance @@ -224,98 +224,98 @@ def separation(self, sheep_list): steer[0] += dx * strength steer[1] += dy * strength count += 1 - + if count > 0: steer[0] /= count steer[1] /= count - + # Normalize and scale magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if magnitude > 0: steer[0] = (steer[0] / magnitude) * self.max_speed steer[1] = (steer[1] / magnitude) * self.max_speed - + # Calculate steering force steer[0] -= self.velocity[0] steer[1] -= self.velocity[1] - + # Limit force force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) if force_magnitude > self.max_force: steer[0] = (steer[0] / force_magnitude) * self.max_force steer[1] = (steer[1] / force_magnitude) * self.max_force - + return steer def avoid_boundaries(self): """Avoid field boundaries""" steer = [0, 0] - + # Get current position in real-world coordinates x, y = self.x, self.y - + # Check if we're too close to the boundary - if not self.coord_transformer.is_point_in_field(x, y): + #if not self.coord_transformer.is_point_in_field(x, y): # Find closest point on boundary - min_dist = float('inf') - closest_point = None - - for i in range(len(self.coord_transformer.field_boundary)): - p1 = self.coord_transformer.field_boundary[i] - p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] - - # Calculate closest point on line segment - line_vec = np.array(p2) - np.array(p1) - point_vec = np.array([x, y]) - np.array(p1) - line_len = np.linalg.norm(line_vec) - line_unitvec = line_vec / line_len - point_proj_len = np.dot(point_vec, line_unitvec) - point_proj_len = max(0, min(point_proj_len, line_len)) - - closest = np.array(p1) + line_unitvec * point_proj_len - dist = np.linalg.norm(np.array([x, y]) - closest) - - if dist < min_dist: - min_dist = dist - closest_point = closest - - if closest_point is not None: - # Calculate steering force away from boundary - steer[0] = self.x - closest_point[0] - steer[1] = self.y - closest_point[1] - - # Normalize and scale - magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if magnitude > 0: - steer[0] = (steer[0] / magnitude) * self.max_speed - steer[1] = (steer[1] / magnitude) * self.max_speed - - # Calculate steering force - steer[0] -= self.velocity[0] - steer[1] -= self.velocity[1] - - # Limit force - force_magnitude = math.sqrt(steer[0]**2 + steer[1]**2) - if force_magnitude > self.max_force: - steer[0] = (steer[0] / force_magnitude) * self.max_force - steer[1] = (steer[1] / force_magnitude) * self.max_force - - return steer + min_dist = float('inf') + closest_point = None + + for i in range(len(self.coord_transformer.field_boundary)): + p1 = self.coord_transformer.field_boundary[i] + p2 = self.coord_transformer.field_boundary[(i + 1) % len(self.coord_transformer.field_boundary)] + + # Calculate closest point on line segment + line_vec = np.array(p2) - np.array(p1) + point_vec = np.array([x, y]) - np.array(p1) + line_len = np.linalg.norm(line_vec)+ 0.0000001 + line_unitvec = line_vec / line_len + point_proj_len = np.dot(point_vec, line_unitvec) + point_proj_len = max(0, min(point_proj_len, line_len)) + + closest = np.array(p1) + line_unitvec * point_proj_len + dist = np.linalg.norm(np.array([x, y]) - closest) + + if dist < min_dist: + min_dist = dist + closest_point = closest + + if closest_point is not None: + # Calculate steering force away from boundary + steer[0] = self.x - closest_point[0] + steer[1] = self.y - closest_point[1] + + # Normalize and scale + magnitude = math.sqrt(steer[0]**2 + steer[1]**2) + if magnitude < self.boundary_margin: + steer[0] = (steer[0] / magnitude) * self.max_speed + steer[1] = (steer[1] / magnitude) * self.max_speed + + # Calculate steering force + steer[0] -= self.velocity[0] + steer[1] -= self.velocity[1] + + # Limit force + force_magnitude = -math.sqrt(steer[0]**2 + steer[1]**2) + if force_magnitude > self.max_force: + steer[0] = (steer[0] / force_magnitude) * self.max_force + steer[1] = (steer[1] / force_magnitude) * self.max_force + + return steer return [0, 0] def avoid_dog(self, dog): """Avoid the sheepdog""" to_dog = [self.x - dog.x, self.y - dog.y] distance = math.sqrt(to_dog[0]**2 + to_dog[1]**2) - + if distance < self.dog_repulsion_radius: # Stronger repulsion when closer to the dog strength = 1.0 - (distance / self.dog_repulsion_radius) # Normalize the direction if distance > 0: to_dog[0] = to_dog[0] / distance - to_dog[1] = to_dog[1] / distance - + to_dog[1] = to_dog[1] / distance + # Apply repulsion force return [ to_dog[0] * strength * self.dog_repulsion_weight, @@ -327,12 +327,12 @@ def flock(self, sheep_list, sheepdog): """Apply flocking behaviors""" # Reset acceleration self.acceleration = [0, 0] - + # Get forces from each behavior align = self.align(sheep_list) cohere = self.cohesion(sheep_list) separate = self.separation(sheep_list) - + # Apply weights align[0] *= self.alignment_weight align[1] *= self.alignment_weight @@ -340,20 +340,20 @@ def flock(self, sheep_list, sheepdog): cohere[1] *= self.cohesion_weight separate[0] *= self.separation_weight separate[1] *= self.separation_weight - + # Apply forces self.apply_force(align) self.apply_force(cohere) self.apply_force(separate) - + # Apply sheepdog repulsion dog_force = self.avoid_dog(sheepdog) self.apply_force(dog_force) - + # Apply boundary avoidance boundary_force = self.avoid_boundaries() self.apply_force(boundary_force) - + # Update state based on movement if math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) > self.max_speed * 0.8: self.current_state = self.MOVING @@ -362,7 +362,7 @@ def flock(self, sheep_list, sheepdog): def update(self, dt, sheep_list, sheepdog): """Update sheep position and velocity - + Args: dt: Time step in seconds sheep_list: List of other sheep @@ -373,11 +373,11 @@ def update(self, dt, sheep_list, sheepdog): # Update state based on external forces self.update_state() - + # Update velocity based on acceleration self.velocity[0] += self.acceleration[0] * dt self.velocity[1] += self.acceleration[1] * dt - + # Limit speed based on state speed = math.sqrt(self.velocity[0]**2 + self.velocity[1]**2) if self.current_state == self.GRAZING: @@ -386,15 +386,15 @@ def update(self, dt, sheep_list, sheepdog): max_speed = self.max_speed * self.walking_speed_multiplier else: # MOVING state max_speed = self.max_speed * self.moving_speed_multiplier - + if speed > max_speed: self.velocity[0] = (self.velocity[0] / speed) * max_speed self.velocity[1] = (self.velocity[1] / speed) * max_speed - + # Update position new_x = self.x + self.velocity[0] * dt new_y = self.y + self.velocity[1] * dt - + # Reset acceleration self.acceleration = [0, 0] @@ -414,13 +414,13 @@ def draw(self, screen): """Draw the sheep on the screen""" # Convert real-world coordinates to screen coordinates screen_x, screen_y = self.coord_transformer.world_to_screen(self.x, self.y) - + # Convert size from meters to screen pixels screen_size = self.size * self.coord_transformer.scale - + # Draw the sheep body pygame.draw.circle(screen, self.color, (int(screen_x), int(screen_y)), int(screen_size)) - + # Draw state indicator if self.current_state == self.GRAZING: # Draw a small green dot when grazing @@ -444,4 +444,4 @@ def limit(vector, max_value): norm = math.sqrt(vector[0]**2 + vector[1]**2) if norm > max_value: return [vector[0] / norm * max_value, vector[1] / norm * max_value] - return vector \ No newline at end of file + return vector diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 1505454..0d34ff4 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -58,53 +58,53 @@ def handle_event(self, event): class CoordinateTransformer: def __init__(self, field_boundary, screen_width, screen_height): """Initialize coordinate transformer with field boundary and screen dimensions - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels screen_height: Height of the screen in pixels """ self.field_boundary = np.array(field_boundary) - + # Calculate field bounds in real-world coordinates self.field_min_x = np.min(self.field_boundary[:, 0]) self.field_max_x = np.max(self.field_boundary[:, 0]) self.field_min_y = np.min(self.field_boundary[:, 1]) self.field_max_y = np.max(self.field_boundary[:, 1]) - + # Screen dimensions self.screen_width = screen_width self.screen_height = screen_height - + # Calculate scaling factors self.scale_x = screen_width / (self.field_max_x - self.field_min_x) self.scale_y = screen_height / (self.field_max_y - self.field_min_y) - + # Use the smaller scale to maintain aspect ratio self.scale = min(self.scale_x, self.scale_y) - + # Calculate offsets to center the field self.offset_x = (screen_width - (self.field_max_x - self.field_min_x) * self.scale) / 2 self.offset_y = (screen_height - (self.field_max_y - self.field_min_y) * self.scale) / 2 - + def world_to_screen(self, x, y): """Convert real-world coordinates to screen coordinates""" screen_x = (x - self.field_min_x) * self.scale + self.offset_x screen_y = (y - self.field_min_y) * self.scale + self.offset_y return screen_x, screen_y - + def screen_to_world(self, screen_x, screen_y): """Convert screen coordinates to real-world coordinates""" x = (screen_x - self.offset_x) / self.scale + self.field_min_x y = (screen_y - self.offset_y) / self.scale + self.field_min_y return x, y - + def is_point_in_field(self, x, y): """Check if a point is inside the field boundary using ray casting algorithm""" point = np.array([x, y]) n = len(self.field_boundary) inside = False - + p1x, p1y = self.field_boundary[0] for i in range(n + 1): p2x, p2y = self.field_boundary[i % n] @@ -116,7 +116,7 @@ def is_point_in_field(self, x, y): if p1x == p2x or x <= xinters: inside = not inside p1x, p1y = p2x, p2y - + return inside def get_closest_boundary_point(self, x, y): @@ -124,41 +124,41 @@ def get_closest_boundary_point(self, x, y): point = np.array([x, y]) min_dist = float('inf') closest_point = None - + # Check each line segment of the boundary for i in range(len(self.field_boundary)): p1 = self.field_boundary[i] p2 = self.field_boundary[(i + 1) % len(self.field_boundary)] - + # Vector from p1 to p2 line_vec = p2 - p1 # Vector from p1 to point point_vec = point - p1 - + # Project point_vec onto line_vec line_len = np.linalg.norm(line_vec) line_unitvec = line_vec / line_len projection = np.dot(point_vec, line_unitvec) - + # Clamp projection to line segment projection = max(0, min(line_len, projection)) - + # Calculate closest point on line segment closest = p1 + projection * line_unitvec - + # Calculate distance to closest point dist = np.linalg.norm(point - closest) - + if dist < min_dist: min_dist = dist closest_point = closest - + return closest_point class Simulation: def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None): """Initialize simulation with field boundary - + Args: field_boundary: List of [x, y] points in meters defining the field boundary screen_width: Width of the screen in pixels @@ -168,10 +168,10 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st """ self.screen_width = screen_width self.screen_height = screen_height - + # Initialize coordinate transformer self.coord_transformer = CoordinateTransformer(field_boundary, screen_width, screen_height) - + # Create the sheepdog if sheepdog_state is None: # Place sheepdog at center of field in real-world coordinates @@ -187,17 +187,20 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st yaw=0, # Initial yaw coord_transformer=self.coord_transformer ) - + # Create a list of sheep self.num_sheep = 50 if sheep_states is None else len(sheep_states) self.sheep_list = [] self._initialize_sheep(sheep_states) - + # Flocking parameters self.alignment_weight = 1.0 self.cohesion_weight = 0.3 self.separation_weight = 6.0 + for i in range(200): + self.update(0.05) + def _initialize_sheep(self, sheep_states=None): """Initialize sheep with given states or random positions""" if sheep_states is None: @@ -208,7 +211,7 @@ def _initialize_sheep(self, sheep_states=None): # Generate random position in real-world coordinates x = random.uniform(self.coord_transformer.field_min_x, self.coord_transformer.field_max_x) y = random.uniform(self.coord_transformer.field_min_y, self.coord_transformer.field_max_y) - + # Check if position is inside field boundary if self.coord_transformer.is_point_in_field(x, y): position = [x, y] @@ -234,7 +237,7 @@ def _initialize_sheep(self, sheep_states=None): def update(self, dt=0.02, sheepdog_state=None): """Update the simulation state - + Args: dt: Time step in seconds sheepdog_state: Optional dictionary with 'position' and 'velocity' keys. @@ -245,12 +248,12 @@ def update(self, dt=0.02, sheepdog_state=None): if sheepdog_state is not None: if not isinstance(sheepdog_state, dict) or 'position' not in sheepdog_state: raise ValueError("Sheepdog state must be provided as a dictionary with 'position' key") - + self.sheepdog.set_position(sheepdog_state['position'][0], sheepdog_state['position'][1]) - + if 'velocity' in sheepdog_state: self.sheepdog.velocity = sheepdog_state['velocity'] - + # Update each sheep for sheep in self.sheep_list: @@ -267,18 +270,18 @@ def draw(self, screen): """Draw the simulation state""" # Fill the screen with grass color screen.fill(GREEN) - + # Draw field boundary screen_points = [] for point in self.coord_transformer.field_boundary: screen_x, screen_y = self.coord_transformer.world_to_screen(point[0], point[1]) screen_points.append((screen_x, screen_y)) pygame.draw.polygon(screen, BLACK, screen_points, 2) - + # Draw each sheep for sheep in self.sheep_list: sheep.draw(screen) - + # Draw the sheepdog self.sheepdog.draw(screen) @@ -292,32 +295,32 @@ def __init__(self, width, height): pygame.display.set_caption("Sheep Simulation") self.clock = pygame.time.Clock() self.running = True - + # Colors self.BLACK = (0, 0, 0) self.GREEN = (34, 139, 34) # Grass color self.WHITE = (255, 255, 255) self.GRAY = (128, 128, 128) 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) - + # Initialize simulation self.simulation = Simulation(field_boundary, width, height) - + # Initialize sheepdog controller self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) - + # Initialize sliders self.sliders = { 'alignment': Slider(10, height - 100, 200, 20, 0, 10, 3.0, "Alignment"), 'cohesion': Slider(10, height - 70, 200, 20, 0, 10, 5.0, "Cohesion"), 'separation': Slider(10, height - 40, 200, 20, 0, 10, 2.5, "Separation") } - + # Instructions self.font = pygame.font.Font(None, 24) self.instructions = [ @@ -333,7 +336,7 @@ def handle_events(self): elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: return False - + # Handle slider events for slider in self.sliders.values(): slider.handle_event(event) @@ -343,18 +346,18 @@ def update(self): """Update game state""" # Get time step in seconds dt = self.clock.get_time() / 1000.0 # Convert milliseconds to seconds - + # Update simulation self.simulation.update(dt) - + # Update sheepdog controller keys = pygame.key.get_pressed() self.sheepdog_controller.update(keys, dt) - + # Update sliders for slider in self.sliders.values(): slider.handle_event(pygame.event.Event(pygame.MOUSEMOTION, {'pos': pygame.mouse.get_pos()})) - + # Update simulation parameters self.simulation.alignment_weight = self.sliders['alignment'].value self.simulation.cohesion_weight = self.sliders['cohesion'].value @@ -405,7 +408,7 @@ def load_map_config(map_file): """Load map configuration from YAML file and convert coordinates to meters""" with open(map_file, 'r') as f: config = yaml.safe_load(f) - + # Convert lat/lon to meters (approximate conversion) # Using a simple conversion where 1 degree of latitude ≈ 111,320 meters # and 1 degree of longitude ≈ 111,320 * cos(latitude) meters @@ -424,10 +427,10 @@ def load_map_config(map_file): x = (lon - ref_lon) * 111320 * np.cos(np.radians(ref_lat)) y = (lat - ref_lat) * 111320 boundary_points.append([x, y]) - + return boundary_points if __name__ == "__main__": # Create and run the game game = Game(800, 600) - game.run() \ No newline at end of file + game.run() diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 1f4e64d..ad484c8 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -84,7 +84,7 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d (xmean, ymean), radius_sheep = circle_around_points(points) #radius_sheep += .5 d = np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xd, yd])) - if np.linalg.norm(np.asarray([xmean, ymean]) - np.asarray([xc, yc])) < 10: + 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])) closest_point = (xd + (-10 - xd) * radius_d / d, yd + (10 - yd) * radius_d / d) points = np.array([closest_point]) From a62d3921debac40f550def316714221e36f69b39 Mon Sep 17 00:00:00 2001 From: VioletMayne <132717700+VioletMayne@users.noreply.github.com> Date: Thu, 19 Jun 2025 14:53:15 +0100 Subject: [PATCH 31/59] Added distance metric to cost function --- .../auto_shepherd_simulation/tmpDogSim/dog_control_lib.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index ad484c8..67602c1 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -41,7 +41,7 @@ def normalise_velocities(*values): def angle(xbase, ybase, xnew, ynew): return maths.acos((xbase*xnew + ybase*ynew)/(maths.sqrt(xbase**2+ybase**2)*maths.sqrt(xnew**2+ynew**2))) -def cost(x, y, xd, yd, xc, yc, simulation): +def cost(x, y, xd, yd, xc, yc, simulation, distance_weight = 1): # get the new direction of the flock given the current sheep positions and future dog position # xvel, yvel = get_direction(x, y, xd, yd) # position and velocity, @@ -59,7 +59,8 @@ def cost(x, y, xd, yd, xc, yc, simulation): xmean, ymean = np.mean(x), np.mean(y) xveldesired, yveldesired = xc - xmean, yc - ymean xveldesired, yveldesired = normalise_velocities(xveldesired, yveldesired) - return angle(xvel, yvel, xveldesired, yveldesired) # penalise distance to closest sheep, reject if within 2m of sheep + return angle(xvel, yvel, xveldesired, yveldesired) - distance_weight * np.linalg.norm(np.array([xd,yd] - np.array([xc,yc]))) + #return angle(xvel, yvel, xveldesired, yveldesired) # penalise distance to closest sheep, reject if within 2m of sheep def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, dog, goal @@ -82,7 +83,8 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d points = points[labels==furthest_cluster] (xmean, ymean), radius_sheep = circle_around_points(points) - #radius_sheep += .5 + radius_sheep += .05 # ensure single sheep clusters have a radius + 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])) From 75ebdd577875c3dd3e5f5e61bc4c894adcd4f4b0 Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Thu, 19 Jun 2025 17:47:07 +0100 Subject: [PATCH 32/59] changes on omars computer while away --- configs/rviz/default.rviz | 27 ++++++++--------- .../__pycache__/sheep.cpython-310.pyc | Bin 9133 -> 9133 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11596 -> 11537 bytes .../sheep_simulation/sheep.py | 2 +- .../sheep_simulation/simulation.py | 4 +-- .../tmpDogSim/dog_control_lib.py | 28 ++++++++++-------- 6 files changed, 31 insertions(+), 30 deletions(-) diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 5c3769a..365223d 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -10,7 +10,6 @@ Panels: - /Path1 - /Path1/Topic1 - /Path1/Offset1 - - /Pose1 - /Pose2 Splitter Ratio: 0.5 Tree Height: 1204 @@ -46,8 +45,8 @@ Visualization Manager: Name: Grid Normal Cell Count: 0 Offset: - X: 60 - Y: -60 + X: -60 + Y: 60 Z: 0 Plane: XY Plane Cell Count: 14 @@ -101,7 +100,7 @@ Visualization Manager: Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/Pose Color: 0; 25; 255 - Enabled: true + Enabled: false Head Length: 1 Head Radius: 1 Name: Pose @@ -115,7 +114,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dog/command - Value: true + Value: false - Alpha: 1 Axes Length: 3 Axes Radius: 0.5 @@ -138,7 +137,7 @@ Visualization Manager: Value: true Enabled: true Global Options: - Background Color: 48; 48; 48 + Background Color: 255; 255; 255 Fixed Frame: map Frame Rate: 30 Name: root @@ -182,25 +181,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 140.7987060546875 + Distance: 86.55028533935547 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -55.264869689941406 - Y: 46.4901237487793 - Z: 9.337041854858398 + X: -37.34757614135742 + Y: 33.51708221435547 + Z: -0.8786823153495789 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0947966575622559 + Pitch: 0.7197969555854797 Target Frame: Value: Orbit (rviz) - Yaw: 5.108584880828857 + Yaw: 5.033583164215088 Saved: ~ Window Geometry: Displays: @@ -208,7 +207,7 @@ Window Geometry: Height: 1495 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007270000003efc0100000002fb0000000800540069006d00650100000000000007270000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005cb0000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000008a40000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -217,6 +216,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1831 + Width: 2560 X: 0 Y: 32 diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc index 0dd1d41db756514fb96f969d489fed5a7dfb9946..a22fc791a76e18699e6fc3394eb11c530959d25b 100644 GIT binary patch delta 26 gcmZ4MzSf;9pO=@50SFo;H*%#gF-mOCXR=cS09Bg>-2eap delta 26 gcmZ4MzSf;9pO=@50SFpIH*%#gF^X)?XR=cS098f>)Bpeg diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc index 1bd3182468a3109a8575fa4dad4008e6313c0ee3..ff59bb7bc0da3a4f87dc0f54e223f76d3447eeb8 100644 GIT binary patch delta 529 zcmXw$-%FEG7{~X#*~{D5=8v&=uT9V9Y`HUE1nJTnr6TCYi(0~PLNKX|q&h0m{83I& zQ$gMyb#1vWldeKef`5Wg5`kBBC3F*l_#fo+Og(Tud=JmB?>Xm3&dUX#2ZMeet&;ul zY5z`eq$On0$)tUo_V_n^-J;C`Jn+eYz8^jYLpI~x$d(~3JdNDLQZkH#_EpGW*6ud^ zpqK1JfGNy7w_qAQrw``v!0Ck<{offen%+@Qwai#y6mcvTZJj3h+njt+cswyG8BN>Kuooo*QEMcz@=6McgMGrNg zD3-;YhBZpZwPvSjhKs$+4oc#w8`UDd1IE97{VU-I^CJfZ`YMwxMgkz#D(v#z}Pfn^1McC|;UEbCLh zvbyjO=^XhVPTzzw&ZocA^g)>+U6e!D>i#7q^9bm{;AOU;%A+>+*VgZW3zv$9;JQx&4Vt delta 595 zcmXw$O=uHQ5XX07iQBLnW8x-DHhoEI)Alt*tzHBRMJ!dsTm2$5DZ)wx585??Hci#k zgZN3Arfp(~%|+=+%R77UB7(UHJ=Kecf{J&qIuq-I_uK!@%%3;&-cqieGjEt?K%=!` zoxe7^X%@o4Zu;}nK1KUh8}EjdDDHQSMHtued&~N~wjOrP7JxQ8<-(*UgX(DH7leL( zqWYP7jn~GQj9}7g?DH;DaNB?TrhBD$Rd(Zh>ogSbi--jJDe;O=ID$RVtnLTHXF%5< zbvC*MFpo+57?g3+J^_pP!XAMI^}#OqLQlxyJ!L#&l<=?(HmlsHhub|MZ{^*s>;c#OY$g#TX>khQyY%)sA=vK5tH~qype?aJ-JM(v z0b#dc5Aj?^Kt)~8yn*BjXOoN+V~x?r$S_uMWFUXM#Ow_>xAm VO?ZS~vyK1LmWO+xiR;6&e*xzXn3Mnj diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py index ccd5bce..722ce8a 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py @@ -39,7 +39,7 @@ def __init__(self, position, velocity=None, coord_transformer=None): # Flocking parameters self.alignment_weight = 3.0 - self.cohesion_weight = 5.0 + self.cohesion_weight = 5.5 self.separation_weight = 8.0 # Physical properties diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py index 0d34ff4..9236036 100644 --- a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py @@ -198,8 +198,8 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st self.cohesion_weight = 0.3 self.separation_weight = 6.0 - for i in range(200): - self.update(0.05) + # for i in range(200): + # self.update(0.05) def _initialize_sheep(self, sheep_states=None): """Initialize sheep with given states or random positions""" diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py index 67602c1..cc1a726 100644 --- a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py @@ -73,24 +73,27 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d db = DBSCAN(eps=10, min_samples=1).fit(points) labels = db.labels_ - furthest_distance, furthest_cluster = -1, -1 + cluster_distances = [] print(f"{len(np.unique(labels))} clusters found") - for cluster in np.unique(labels): - centre_of_cluster = np.mean(points[labels==cluster],axis=0) - distance_to_goal = np.linalg.norm(centre_of_cluster-goal_point) - if distance_to_goal > furthest_distance: - furthest_distance, furthest_cluster = distance_to_goal, cluster - points = points[labels==furthest_cluster] + if len(np.unique(labels)) != 1: + for cluster in np.unique(labels): + centre_of_cluster = np.mean(points[labels==cluster],axis=0) + distance_to_goal = np.linalg.norm(centre_of_cluster-goal_point) + cluster_distances.append([cluster, distance_to_goal]) + cluster_distances.sort(key=lambda x: x[1]) + points = points[labels==cluster_distances[-1][0]] (xmean, ymean), radius_sheep = circle_around_points(points) - radius_sheep += .05 # ensure single sheep clusters have a radius - + radius_sheep = max(radius_sheep, 1) # ensure single sheep clusters have a radius + 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])) - 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 + 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 elif d > radius_d + radius_sheep: print("Moving towards sheep") closest_point = (xd + (xmean - xd) * radius_d / d, yd + (ymean - yd) * radius_d / d) @@ -128,7 +131,6 @@ def find_best_dog_position(x, y, xd, yd, xc, yc, field_boundary, # ← flock, d last_update += 1 print(f"Best Cost: {optimal_cost}") if i-last_update > early_exit_threshold: break - if not map_polygon.contains_point((optimal_xd, optimal_yd)): optimal_xd, optimal_yd = xd, yd return optimal_xd, optimal_yd def pure_pursuit(dog_xy, target_xy, lookahead=2.0, step=0.5): From e0ed5b2285bd0235d5f60b53342717f819d8a6a4 Mon Sep 17 00:00:00 2001 From: Omro <49950899+omroali@users.noreply.github.com> Date: Fri, 20 Jun 2025 11:49:36 +0100 Subject: [PATCH 33/59] goal pose ability minor visual changes --- bash_scripts/container/launcher.yaml | 10 +- configs/rviz/default.rviz | 26 ++--- .../auto_shepherd_simulation/dog_control.py | 2 +- .../auto_shepherd_simulation/mapper/mapper.py | 93 ++++++++++++++---- .../auto_shepherd_simulation/ros_interface.py | 18 +++- .../__pycache__/sheep.cpython-310.pyc | Bin 9133 -> 9124 bytes .../tmpDogSim/dog_control_lib.py | 2 + 7 files changed, 109 insertions(+), 42 deletions(-) diff --git a/bash_scripts/container/launcher.yaml b/bash_scripts/container/launcher.yaml index 0496361..a5a87e1 100644 --- a/bash_scripts/container/launcher.yaml +++ b/bash_scripts/container/launcher.yaml @@ -17,14 +17,14 @@ init_cmd: | } windows: + - name: mapper + tags: ["viz", "mapper"] + panes: + - "ros2 run auto_shepherd_simulation mapper.py" + # - "rviz2" - name: simulator tags: ["ros_interface", "dog_control_simulator", "dog_control"] panes: - ros2 run auto_shepherd_simulation ros_interface.py - ros2 run auto_shepherd_simulation dog_control_simulator.py - ros2 run auto_shepherd_simulation dog_control.py - - name: rviz - tags: ["viz", "mapper"] - panes: - - "ros2 run auto_shepherd_simulation mapper.py" - - "rviz2" diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 365223d..48d7b68 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -38,7 +38,7 @@ Visualization Manager: Cell Size: 10 Class: rviz_default_plugins/Grid Color: 200; 200; 200 - Enabled: true + Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -51,17 +51,17 @@ Visualization Manager: Plane: XY Plane Cell Count: 14 Reference Frame: - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path - Color: 25; 255; 0 + Color: 179; 119; 97 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 + Line Style: Billboards + Line Width: 1 Name: Path Offset: X: 0 @@ -133,11 +133,11 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sheep/goal_pose + Value: /goal_pose Value: true Enabled: true Global Options: - Background Color: 255; 255; 255 + Background Color: 48; 80; 48 Fixed Frame: map Frame Rate: 30 Name: root @@ -181,25 +181,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 86.55028533935547 + Distance: 168.8179931640625 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -37.34757614135742 - Y: 33.51708221435547 - Z: -0.8786823153495789 + X: -51.37569046020508 + Y: 64.75483703613281 + Z: -8.768682479858398 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7197969555854797 + Pitch: -1.4047964811325073 Target Frame: Value: Orbit (rviz) - Yaw: 5.033583164215088 + Yaw: 6.21675968170166 Saved: ~ Window Geometry: Displays: diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/ros2_package/auto_shepherd_simulation/dog_control.py index 52d3d0a..52c8001 100644 --- a/ros2_package/auto_shepherd_simulation/dog_control.py +++ b/ros2_package/auto_shepherd_simulation/dog_control.py @@ -21,7 +21,7 @@ def __init__(self): self._dog_cb, 10) # :contentReference[oaicite:3]{index=3} self.create_subscription(Path, '/sheep/poses_sim', self._sheep_cb, 10) # :contentReference[oaicite:4]{index=4} - self.create_subscription(PoseStamped, '/sheep/goal_pose', + self.create_subscription(PoseStamped, '/goal_pose', self._goal_cb, 10) # state caches ---------------------------------------------------- diff --git a/ros2_package/auto_shepherd_simulation/mapper/mapper.py b/ros2_package/auto_shepherd_simulation/mapper/mapper.py index 406740e..4c63da8 100644 --- a/ros2_package/auto_shepherd_simulation/mapper/mapper.py +++ b/ros2_package/auto_shepherd_simulation/mapper/mapper.py @@ -1,7 +1,8 @@ import rclpy from rclpy.node import Node from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped, Point, Quaternion +from geometry_msgs.msg import PolygonStamped # Import for PolygonStamped +from geometry_msgs.msg import PoseStamped, Point, Quaternion, Point32 # Import Point32 for PolygonStamped import yaml import os import tf2_ros @@ -15,6 +16,7 @@ class MapperNode(Node): def __init__(self): super().__init__('mapper_node') + self.declare_parameter('map_file_path', '/home/ros/map/map1.yaml') # Define the QoS profile for a latched topic @@ -24,13 +26,20 @@ def __init__(self): durability=DurabilityPolicy.TRANSIENT_LOCAL ) - # Publisher for the Path message - self.publisher_ = self.create_publisher( + # # Publisher for the Path message + self.publisher_path_ = self.create_publisher( # Renamed for clarity Path, 'FieldBoundaryPath', qos_profile ) + # Publisher for the PolygonStamped message + self.publisher_polygon_ = self.create_publisher( + PolygonStamped, + 'field', # Topic name for the PolygonStamped message + qos_profile + ) + # Static Transform Broadcaster for the 'field_frame' self.static_broadcaster = tf2_ros.StaticTransformBroadcaster(self) @@ -61,8 +70,8 @@ def __init__(self): self.origin_utm_x = map_data['origin_utm_x'] self.origin_utm_y = map_data['origin_utm_y'] - # Get the relative X, Y meters for the path - self.path_xy_meters = map_data['map_coords_xy_meters'] + # Get the relative X, Y meters for the path and polygon + self.relative_xy_meters = map_data['map_coords_xy_meters'] self.get_logger().info(f"Map converted. Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") @@ -71,22 +80,32 @@ def __init__(self): rclpy.shutdown() return - # 4. Create PoseStamped messages for the Path from the relative meter coordinates - self.path_poses = self._create_path_poses(self.path_xy_meters) + # 4. Create PoseStamped messages for the Path + self.path_poses = self._create_path_poses(self.relative_xy_meters) if not self.path_poses: self.get_logger().error("Failed to create Path poses from converted data. Shutting down.") rclpy.shutdown() return + # 5. Create Point32 messages for the PolygonStamped + self.polygon_points = self._create_polygon_points(self.relative_xy_meters) + if not self.polygon_points: + self.get_logger().error("Failed to create PolygonStamped points from converted data. Shutting down.") + rclpy.shutdown() + return + # --- Publish Data --- - # Publish the Path message immediately after loading and converting + # Publish the Path message self.publish_path_once() + # Publish the PolygonStamped message + self.publish_polygon_once() + # Publish the static transform for 'field_frame' self.publish_static_transform() - self.get_logger().info("Map published once as Path. Static transform published. Node will now spin indefinitely.") + self.get_logger().info("Map published once as Path and Polygon. Static transform published. Node will now spin indefinitely.") def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseStamped]: """ @@ -94,25 +113,22 @@ def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseS These poses are in the 'field_frame', which will be offset by the map's UTM origin. """ poses = [] - # Create an identity quaternion for the poses (no rotation) q = quaternion_from_euler(0, 0, 0) identity_orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - - current_stamp = self.get_clock().now().to_msg() # Use a single stamp for all poses in this path + current_stamp = self.get_clock().now().to_msg() for x_m, y_m in xy_meters: pose_stamped = PoseStamped() pose_stamped.header.stamp = current_stamp - pose_stamped.header.frame_id = 'field_frame' # Poses are relative to 'field_frame' + pose_stamped.header.frame_id = 'field_frame' pose_stamped.pose.position.x = x_m pose_stamped.pose.position.y = y_m - pose_stamped.pose.position.z = 0.0 # Assuming 2D path + pose_stamped.pose.position.z = 0.0 pose_stamped.pose.orientation = identity_orientation poses.append(pose_stamped) - # Close the loop for the path visualization in RViz by adding the first point again + # Close the loop by adding the first point again if poses: - # Create a new PoseStamped to avoid modifying the original first pose object closed_loop_pose = PoseStamped() closed_loop_pose.header.stamp = current_stamp closed_loop_pose.header.frame_id = 'field_frame' @@ -121,9 +137,30 @@ def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseS closed_loop_pose.pose.position.z = poses[0].pose.position.z closed_loop_pose.pose.orientation = poses[0].pose.orientation poses.append(closed_loop_pose) - return poses + 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.z = 0.0 # Assuming 2D polygon + points.append(p32) + + # PolygonStamped is implicitly closed if first and last points are the same. + # Adding the first point at the end ensures closure for clear visualization. + if points: + first_point = Point32() + first_point.x = points[0].x + first_point.y = points[0].y + first_point.z = points[0].z + points.append(first_point) + return points + def publish_path_once(self): """Publishes the Path message containing the field boundary.""" if not self.path_poses: @@ -132,13 +169,29 @@ def publish_path_once(self): 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 + path_msg.header.frame_id = 'map' # The Path itself is defined in the 'map' frame (absolute) path_msg.poses = self.path_poses - self.publisher_.publish(path_msg) + 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'.""" t = TransformStamped() @@ -147,7 +200,7 @@ def publish_static_transform(self): t.child_frame_id = 'field_frame' # Set the translation to the absolute UTM coordinates of the bounding box origin. - # This places 'field_frame' (and thus the relative Path) at its real-world location in 'map'. + # This places 'field_frame' (and thus the relative Path/Polygon) at its real-world location in 'map'. t.transform.translation.x = self.origin_utm_x t.transform.translation.y = self.origin_utm_y t.transform.translation.z = 0.0 # Assuming 2D environment diff --git a/ros2_package/auto_shepherd_simulation/ros_interface.py b/ros2_package/auto_shepherd_simulation/ros_interface.py index 599c6a9..e880b54 100644 --- a/ros2_package/auto_shepherd_simulation/ros_interface.py +++ b/ros2_package/auto_shepherd_simulation/ros_interface.py @@ -1,3 +1,6 @@ +# from sheep_simulation import sheep +# from random import shuffle +# from os import sep import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped @@ -12,10 +15,18 @@ def __init__(self): self.drone_publisher = self.create_publisher(PoseStamped, '/drone/pose', 10) self.dog_publisher = self.create_publisher(PoseStamped, '/dog/pose', 10) self.sheep_publisher = self.create_publisher(Path, '/sheep/poses', 10) - self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/sheep/goal_pose', 10) + self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/goal_pose', 10) + self.sheep_goal_subscriber = self.create_subscription(PoseStamped, '/goal_pose', self.sheep_goal_callback, 10) + self.dog_command_subscription = self.create_subscription(PoseStamped, '/dog/command', self.dog_command_callback, 10) self.timer = self.create_timer(0.1, self.timer_callback) + # self.sheep_goal_publisher.publish(sheep_goal_pose) + self.sheep_goal_pose = self.create_pose_stamped(-38.0, 90.0, 0.0) + + def sheep_goal_callback(self, msg): + self.sheep_goal_pose = msg + def create_header(self): header = Header() header.stamp = self.get_clock().now().to_msg() @@ -50,8 +61,9 @@ def timer_callback(self): self.sheep_publisher.publish(sheep_path) # Publish sheep goal pose - sheep_goal_pose = self.create_pose_stamped(-38.0, 90.0, 0.0) - self.sheep_goal_publisher.publish(sheep_goal_pose) + # sheep_goal_pose = self.create_pose_stamped(-11.0, 13.0, 0.0) + # sheep_goal_pose = self.create_pose_stamped(-110.0, 45.0, 0.0) + self.sheep_goal_publisher.publish(self.sheep_goal_pose) def dog_command_callback(self, msg): target_x = msg.pose.position.x diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc index a22fc791a76e18699e6fc3394eb11c530959d25b..abaae7da3d58eb11358d492f4b40f9b73547b52d 100644 GIT binary patch delta 83 zcmZ4MzQmn7pO=@50SLD7hHm6O%xKC|!w}D(!UH4)Qh0%+U Date: Sun, 22 Jun 2025 19:35:00 +0100 Subject: [PATCH 34/59] pkg rename --- .../CMakeLists.txt | 0 .../README.md | 0 .../auto_shepherd_simulation/README.md | 0 .../auto_shepherd_simulation/__init__.py | 0 .../boid_training_simulator.py | 0 .../auto_shepherd_simulation/dog_control.py | 0 .../dog_control_simulator.py | 0 .../auto_shepherd_simulation/mapper/__init__.py | 0 .../auto_shepherd_simulation/mapper/mapper.py | 0 .../auto_shepherd_simulation/requirements.txt | 0 .../auto_shepherd_simulation/ros_interface.py | 0 .../sheep_simulation/__init__.py | 0 .../sheep_simulation/sheep.py | 0 .../sheep_simulation/sheepdog.py | 0 .../sheep_simulation/simulation.py | 0 .../auto_shepherd_simulation/sheepdog_teleop.py | 0 .../tmpDogSim/__init__.py | 0 .../tmpDogSim/dog_control_lib.py | 0 .../auto_shepherd_simulation/tmpDogSim/main.py | 0 .../utils/geo_converter.py | 0 .../package.xml | 0 .../resource/auto_shepherd_simulation | 0 .../setup.cfg | 0 .../setup.py | 0 bash_scripts/container/bashrc_extensions.sh | 2 +- docker/docker-compose.yml | 2 +- .../__pycache__/__init__.cpython-310.pyc | Bin 178 -> 0 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 195 -> 0 bytes .../__pycache__/sheep.cpython-310.pyc | Bin 9124 -> 0 bytes .../__pycache__/sheepdog.cpython-310.pyc | Bin 3823 -> 0 bytes .../__pycache__/simulation.cpython-310.pyc | Bin 11537 -> 0 bytes 31 files changed, 2 insertions(+), 2 deletions(-) rename {ros2_package => auto_shepherd_simulation}/CMakeLists.txt (100%) rename {ros2_package => auto_shepherd_simulation}/README.md (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/README.md (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/__init__.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/boid_training_simulator.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/dog_control.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/dog_control_simulator.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/mapper/__init__.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/mapper/mapper.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/requirements.txt (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/ros_interface.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/sheep_simulation/__init__.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/sheep_simulation/sheep.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/sheep_simulation/sheepdog.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/sheep_simulation/simulation.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/sheepdog_teleop.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/tmpDogSim/__init__.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/tmpDogSim/main.py (100%) rename {ros2_package => auto_shepherd_simulation}/auto_shepherd_simulation/utils/geo_converter.py (100%) rename {ros2_package => auto_shepherd_simulation}/package.xml (100%) rename {ros2_package => auto_shepherd_simulation}/resource/auto_shepherd_simulation (100%) rename {ros2_package => auto_shepherd_simulation}/setup.cfg (100%) rename {ros2_package => auto_shepherd_simulation}/setup.py (100%) delete mode 100644 ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/__init__.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheep.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/sheepdog.cpython-310.pyc delete mode 100644 ros2_package/auto_shepherd_simulation/sheep_simulation/__pycache__/simulation.cpython-310.pyc diff --git a/ros2_package/CMakeLists.txt b/auto_shepherd_simulation/CMakeLists.txt similarity index 100% rename from ros2_package/CMakeLists.txt rename to auto_shepherd_simulation/CMakeLists.txt diff --git a/ros2_package/README.md b/auto_shepherd_simulation/README.md similarity index 100% rename from ros2_package/README.md rename to auto_shepherd_simulation/README.md diff --git a/ros2_package/auto_shepherd_simulation/README.md b/auto_shepherd_simulation/auto_shepherd_simulation/README.md similarity index 100% rename from ros2_package/auto_shepherd_simulation/README.md rename to auto_shepherd_simulation/auto_shepherd_simulation/README.md diff --git a/ros2_package/auto_shepherd_simulation/__init__.py b/auto_shepherd_simulation/auto_shepherd_simulation/__init__.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/__init__.py rename to auto_shepherd_simulation/auto_shepherd_simulation/__init__.py diff --git a/ros2_package/auto_shepherd_simulation/boid_training_simulator.py b/auto_shepherd_simulation/auto_shepherd_simulation/boid_training_simulator.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/boid_training_simulator.py rename to auto_shepherd_simulation/auto_shepherd_simulation/boid_training_simulator.py diff --git a/ros2_package/auto_shepherd_simulation/dog_control.py b/auto_shepherd_simulation/auto_shepherd_simulation/dog_control.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/dog_control.py rename to auto_shepherd_simulation/auto_shepherd_simulation/dog_control.py diff --git a/ros2_package/auto_shepherd_simulation/dog_control_simulator.py b/auto_shepherd_simulation/auto_shepherd_simulation/dog_control_simulator.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/dog_control_simulator.py rename to auto_shepherd_simulation/auto_shepherd_simulation/dog_control_simulator.py diff --git a/ros2_package/auto_shepherd_simulation/mapper/__init__.py b/auto_shepherd_simulation/auto_shepherd_simulation/mapper/__init__.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/mapper/__init__.py rename to auto_shepherd_simulation/auto_shepherd_simulation/mapper/__init__.py diff --git a/ros2_package/auto_shepherd_simulation/mapper/mapper.py b/auto_shepherd_simulation/auto_shepherd_simulation/mapper/mapper.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/mapper/mapper.py rename to auto_shepherd_simulation/auto_shepherd_simulation/mapper/mapper.py diff --git a/ros2_package/auto_shepherd_simulation/requirements.txt b/auto_shepherd_simulation/auto_shepherd_simulation/requirements.txt similarity index 100% rename from ros2_package/auto_shepherd_simulation/requirements.txt rename to auto_shepherd_simulation/auto_shepherd_simulation/requirements.txt diff --git a/ros2_package/auto_shepherd_simulation/ros_interface.py b/auto_shepherd_simulation/auto_shepherd_simulation/ros_interface.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/ros_interface.py rename to auto_shepherd_simulation/auto_shepherd_simulation/ros_interface.py diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/__init__.py b/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/__init__.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/sheep_simulation/__init__.py rename to auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/__init__.py diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py b/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheep.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/sheep_simulation/sheep.py rename to auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheep.py diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py b/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheepdog.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/sheep_simulation/sheepdog.py rename to auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheepdog.py diff --git a/ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py b/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/simulation.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/sheep_simulation/simulation.py rename to auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/simulation.py diff --git a/ros2_package/auto_shepherd_simulation/sheepdog_teleop.py b/auto_shepherd_simulation/auto_shepherd_simulation/sheepdog_teleop.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/sheepdog_teleop.py rename to auto_shepherd_simulation/auto_shepherd_simulation/sheepdog_teleop.py diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/__init__.py b/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/__init__.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/tmpDogSim/__init__.py rename to auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/__init__.py diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py rename to auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py diff --git a/ros2_package/auto_shepherd_simulation/tmpDogSim/main.py b/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/main.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/tmpDogSim/main.py rename to auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/main.py diff --git a/ros2_package/auto_shepherd_simulation/utils/geo_converter.py b/auto_shepherd_simulation/auto_shepherd_simulation/utils/geo_converter.py similarity index 100% rename from ros2_package/auto_shepherd_simulation/utils/geo_converter.py rename to auto_shepherd_simulation/auto_shepherd_simulation/utils/geo_converter.py diff --git a/ros2_package/package.xml b/auto_shepherd_simulation/package.xml similarity index 100% rename from ros2_package/package.xml rename to auto_shepherd_simulation/package.xml diff --git a/ros2_package/resource/auto_shepherd_simulation b/auto_shepherd_simulation/resource/auto_shepherd_simulation similarity index 100% rename from ros2_package/resource/auto_shepherd_simulation rename to auto_shepherd_simulation/resource/auto_shepherd_simulation diff --git a/ros2_package/setup.cfg b/auto_shepherd_simulation/setup.cfg similarity index 100% rename from ros2_package/setup.cfg rename to auto_shepherd_simulation/setup.cfg diff --git a/ros2_package/setup.py b/auto_shepherd_simulation/setup.py similarity index 100% rename from ros2_package/setup.py rename to auto_shepherd_simulation/setup.py diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index f84081d..1048947 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -46,4 +46,4 @@ echo "ROS 2 workspace setup and sourced. Happy robot wrangling!" # Mark that the setup has been run in this shell session export _ROS_WORKSPACE_SETUP_RUN=true -cd ~/base_ws/src/ros2_package/auto_shepherd_simulation +cd ~/base_ws/src/auto_shepherd_simulation/auto_shepherd_simulation diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 927c397..aea6764 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -15,7 +15,7 @@ services: # — Bind‑mount each git sub‑module into the workspace ———————————— volumes: - - ../ros2_package:/home/ros/base_ws/src/ros2_package:rw + - ../auto_shepherd_simulation:/home/ros/base_ws/src/auto_shepherd_simulation:rw # Bash Scripts - ../bash_scripts/container:/home/ros/bash_scripts:rw diff --git a/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc b/ros2_package/auto_shepherd_simulation/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index f8a6096bd727c9e77a09c95064a7f89f9e9107ee..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 178 zcmd1j<>g`kg7qVkRKL!Tg`kg7qmgL75XQUQnq!y*b7iZ>{<|LM6=I7y-04h!`Kvo$apP83g c5+AQuP#@D#A7G$>pja&q zXEq!<`mIu=`k z^X8Vxv9)F2F{CN2$A+{H%`HcYO`~N$MLT1oOX2J<@An6w`Udrde#Dj~%`ID6X#J9O zr2ANGxw0U=$HrDc7G(*sC(E*exG0xo6>&+{WF2u?F3T0f6?smsB3_c`Wdm_lUXW{u zYx1JJgt#s*%XP%d@;P}0@rwMMY$87Augd4;)yKx6*gB8&3-U!yH>8bqyqq|j+rB?i z0`q;!fr->c*PdAwrHT4JzK|9c3n?xdvC*cK$te+IyT*oEktpk#Q!_SEj%`C|NgJUp z9fVFy6gM_@3Wz<4RJ$lCU6LNcvMeI3$P&UOSw>jp)-_o{x-ORxF3T#y6A=1 zNBTU|8`8kSmN$4wiVoywpL~ygPjB{+!LMt zO6^`S9?Fh7Xzz7YuRlyyWzcIYe>5Jo5c$`y8xB_(QHj7TpQt} z+wK0aAGO;weE2&EN@huTmM2`{TKEupX{>W;sA-}kCRG;8YgtEK63y&KY2s^^M`Acg z6Mc|7gn*mBG9dxx~>2UDz^#2`+$T$*u;unFm z=V&Pa4xPSW9E+)lsodJN)jDDV6Gu9m03YOBP+vhz5TdEZ4grZ16_1L?1_6<@cMbJ> zQcTTZ4OBqOPPt5@R8}bj$WZDSpp~|V_vpKs&~<4*hFyE zDp?h6~v!sBmu3+>QYS#0u$6NX0vvYNk{u zCryQdH(Elqkb#Jh6eJFB%;KWU9qPHwWm~8g z(%Ci4OJcID*ELfa zKpe)pMr>WC&9K4bXn~F4+H+KssjSO)VAx!w5p^vCdGyVr+B~YAk8012>cr6NEQVps zbz+CdbuljT7=i20QE6P5A6j7eU|!nv{oUq0f4lRrAE@SUvz*|3_5o2L&MSg8@S_u* z&N}uKZj(z&I!r zf47Z-U#ufoH?M$u8=@iVR$bV?y5g3QZU`IvJSpjOrUsvJzy#OKvDj|Fwl<>vpC9lm z9}ax~)B$()I|t4CD%d;gh}9c(D8Kuxlev!$A5y{F2(sNL;@s{RQAi^8KECkJA=I%5 z-Hh=vW0i0|79SXY_}0{pp)`SA0l9#;9X!QA*JW znk=V6r=?U&T5lRt>p?XY*?-PkThD0}%i5)Ro{biK(vb7|(T%Yp2>xU01!(Bz0+OkH;-Q@R7#|Tz`&A;}+%w z|N8CBtej(xPSo9QW(IR}zaMQkhh%l$3)G@+Y8Kl2>j2F&+>Uw2B%EN+FwW z1IRxV=)+z+8;A_?@h}1)@TKT2v>`J+)qv_t)aA2K!eQI^FHjs(8-fGRv`Nv4m!a|q zB0ORKiD%YDU0i`3$Pi+mf{=2mDrigQxR^LN=i;9r^Qm0?M$X0W{U5pby_^rh^`|%( zJj)t(k$dmvtRefI9zXMY3R z)Y9Ouejn+?)%~fP$f#Qs&|xhizL(uxtB0aKUNG1N08(xs4*s6a9YS-cy=65b(1x(mfzxJ+$b zj_J~E#|73oHt1e{YEJB}X2_J`jypqJJntlLdB%Cqm?@{xsu&k}rk_wRK#d}!hBITP z5Dv{!Q?z0ng{eWU-kL{AqN0_ie(LC@nCyQ-EnWpiD&?p(iH zk5z6RW_zk;1vZ`y9ut=A)QVtf9*Qa4VpAuwBBiKRI$ z?F+_9%?3>WsTUQFpo)$K$Ax1Oks_!0Vm3!FMTH%@02YshbS@ava$Jnd5QPfrm%$hn zTmqM(iWJdOycCxY%|psl|L8Za#7nCXwrN%OGCM|GU89R?Ew05RdegWDF|JOX2Nz?g z)3NwVp=f;ua$sJz>q$Y9P&?92DfT-?R6VL`(@?q)Z$V|0ViB*$@N(_ccMa~14am68 z%)1QXK^{GFi#aNKrLt6b;GfIHgzDy0jFSq_r_7*iOqb(j%;DUwjX9XqF9c|r=WuRe z4zpCA!wmQrCXL%HGWYxbKsHmKPd~l(CRTrVe?#AUzx=m9{^aNXF?pBm{nqM<{NNfk z2s%*JxRv(WZRcJw4w?z5fwkrHz_N}P^TorA<3^l`)PCH4^4-o%nYlvr@WA?Och z^vD(P?xH-dXnAA{Ah$6R4-MONjevV@5QKghwMRi8288G*=I9Ift|uM_^>iBH&DyT4nfgr zY_yiyRixe{3W=ZkG6icCT%_O<1z)5YsRW9QhH&100|Bt(07yurZE zCmALOV zU8Qd{KSD^iwE%Fcnp|?aE|?SD(hEe2x6R<*4&H$BnhQ?8jn<0VA?E)A1uP-tjh&Gi z787!9&v>#)Y7_yhxzs!z?6|T$bIU= z;TJLtDJ+ zIul@-ISR<#EO1QcV}n3OLoZqIR4W#j0U?{l6FYrBr&kQN#GjZa64x>l7uISNDD_P$ zU*Pp3nb+`$P5i&3wUA=OQ;)F0+?hd>*O=pFI>KCYd~L?Eb&K(^z-|}&S306a}myHt@WU=iVWB zq}@(RZM=`k@qp5mcKgA&Gtedc@}?M+ShK%Gvw4{UMm07F$X})2paUU8MA>J)$3PPzk2oY zUcL7wX*O#H%1=Jp?f+=qFg~WnV&Q0Pp{B2b2t%;Q*ekwlkLk9#$KN!BDfm-E@KbZo zLdz01S~gm?V7o@!xxj2=Cm_05X3}q?R~jU7&lGjRh4qx}abXMRsj+8?ig59@MOD=B zb+pVz?(X)b9Be1O9mOy|uopxVZ=&8uP2T{SFu^vAiQyAxYK~1f+UCR*#?Vk#1rR;+ z1a0(kL1UJ%K)+OpF8A5pKMi zK@+F!$BcT;%n6&A=cZc67x%BVV3DTvs9lVn@Ch`dwMU$#IgM)DW7i^p5TOG-1CmYtRKc+H4 z`=S%@bMr&IZ`hD=fERsW9M~=6#LAqr$^a>h*iLGdC0}^S)ibjEUNwTY3)oH6XAM0^i>C(*ld=xVq zmu5ESW5rCdbmHpSO|0u?OPB%ar)f11zd&REQ~57PFQ2VI1FPrkq=t3Y&aU7bHRz{4sUNi9CwSKst3PEYjR`xuI;l?@ z=O#2m-g<54g3LodR9GJcg^OPi`7KEA>$?1Pv-gXiQ@Plj_IzA2f7(>!xjrezf4$l3 z>hkB!-XDHXNKz;pwM{>$G<0Q>g7?sUV+>4|? z-DO@cb|rjB&0P{rTl+6psJ@I-Qzns8n7L*yu^aprx8VO8_?a)P7nV)Fkbcg;Yj(+5 zl^VMSPiZ}`@#0Ik%RLRCSNIAaH)k%0ZoJKld|(F=AHgz)c6>jt`Tk)dhSS5>DPX|&8u!xZb+nh#~X|vG@0*OuPEGBOV zxzJkDMuF_6z?Ok?6AR_`Q7B;MPG5$-{$~t@)DWVklvE*+nf+>P{JB!W^Ba13Vnh0& zs;}w;3N{FlPYmSU+%oVVX4-{L@oR)%FY(Kl#{@?J zSs2f*oV75IA6VxUNGaH0^n|1TYS~|>KwFMbXl*Rc!VL7(jl~%)Lnjs7zeY07t`j|2 zpYZF1$;)cIHerc_-F481SJ`z@yv2C57~xS=GQfET4u~W)l2WrS~U7Zj?%l{ zb{J4vjHJs;y52y^G?hY+T>^{juJ>+uNLh^>P==GrAVJzV1NHv(?9)GfZ~xKej*b-V z8|sU&h}?PTzy08icXR&G?cx) zWL6|-`tDYsK=GvTy}PLwOU=QC~a3_WsEIZCgCECEm?PLuN-R?#S6|bp6;2Q(d^9Z z>0U`YnI$G0AV5;7Kp;RI*sTHzKM){}0UoG+g5rVN7b=jqU|W3O-#xeW zitx-#_21`yy3hG9-}(RU&dlUA{64$5yn6n;ru`c;lRp!g^SHvl1@SdsZ)+>bU0=~9 zZ>$({H&-mV+bg!*ofQXnqn%lES6rQa%yxDyw~|}SujG-pRtmnoGUGcdML)AL>$@u@ zKf5yL=T_#gYJT1?d`|NV+s4Wvh;d9!` zk>IF*_>Q4!!J=>ZHfoLq$99hIYM52ODOY)M5AUNbmG#)Zxf%q$vJtzIxYTXL^TX7| zZYNURb~{je9#&%SlZK|fZ7LmI$K-8u?=ckY{VGedASigibYVzP@Fvs6Awj`Mklx#2 zK?$UWYwCxIl6C(O5`o6G#yac1f%h|g)33uQs__|yKc+Ld zF5(L7Ad$AC5A=aCFbCGIHP9-?zy>u34yZNAfZ78W)bVxDOxiv(u#akJ1Nw+IFc-8V z*jtnsq&~*-%_EwO=lU9+##*|vXJfiOvR649JH5?DbuEakSA$v0 zHgyan&Nn;Fs99}uT=fV_-qsY?^82f2R=aD#8PyHXyipB<%01jSnr;6~btCFl!quR+ z8YsUKHrF=V)u`F+JiG#pgWhDzai`P0&DgC}_z;x}rw`{r=5=TEKdBe>+=KpXvT3J> zZEM{5;m<WPI@XuCZkdj2-M|EBiI< z7w$RV{F?Q3@K0pznp@^T-^qg$FjlJ|bjHUH*Nt6+5{rFwmiD-&edC=$<+v$*ZqkcrCVXU%S41S$#lC>X4q;x_KkKkQXfQ8 zc_3v=on)~S)>II5)RW9uQE)$!T}#M^QK-rwOoZgDd1Kxv8Ygv6@8{En zE6O5%6N!P|(jvWOKt(LzrOh1+CHAha=7r^sznHdC9(fWBK}xrPzcVf;2m_rOD&NVMm^34BvYtoNhsqJL|BHRN2Uv15La_`H&@6hmez1h{f2t?dmlyd z57xWB3;pZ(SwDw+#?ND`3i08K-LCSRooW;;t7<2#chwryGKG2gV|v7s-$f=-v%6He z2GSyuQ1|*TK*83HW17@hsDZL>(GX16IQ#mTqdrvNlYPcDE;+K2rTABJ!ksVyE$|;X zw34U=s*X%rQV^w+M+wX_TFgxLLFvqb22)^3@1iJ`wD-QcSsLN8U405>^m?MCoBhD6 zjTX&|#;fMtYeuVHy&1H9?~U$8$FHhQ57xj-^n~X(*Md$6Egj}^Ui!PB8sWK7j)l^{ zGVFiOd#M>lUbpW3^!+Ej&DTAVk)ytN<-ROtCZ9h$q-@S@dV-{e!{CJ#{&8+6XQ zO;f~S*(-fMjM;1658B~D{Su>f&bubY>j(B+5)H6N^sz@iEtg`W(~Iq@Qq|44IQ^39 z$I&9rCxtY$_tlS0mcZiPpRC>-Wl*yj7bagVo=J1b1IL-LR)r+qA0}Xir6hK{^?DdY zs2QdrwOq*gX3N-dOH1V?oPT&nj!3JiERP?4vCYWt~s zT?IxQvufx?-QiYGl$LFE6;rAkB(IQMC;2!Cs%YrOmM?bG#l@nfsm*=M6VJ-L72*>y zk1H7AdjtlH#-i@({SP0!F#CGd{?F?pHtK!rBc%^rpXAB*ug$@$hDDH7k@b<4;?l?> z4cF&Y7ThEmVTAXsO1fNF7J1X3W$RlY4_l9Y?Wfj*V11;Kzl{v^rU2!Jv8!*Hk?_g3 zzGeCP7WJv=Lo5G`vE^)KA}fKv+xjK#^%u6N^-Mr$Yf+OH*)3?-$Og2|Z5wwi^-1KM zG^Zn{Bj*gVsACST?wIOj@Fn!_AUkk~5(YWuu>KCXGtbs{jPMpXIxl_ff`(_x4swP( z&-x<+8@y}vuU}jZYInS5-K!=#&TCT3g@6SIXf|&nq{@RX@oLpDV!GOHbQKivS_(d1 zz`B$(>Jo11GDvK;fz>G5KmpZGBM_}4il$uH&*ydmMf6VWB+md;au#@_cXl(@o47y2 z%rl#D<~|?`)bRfOo@<5`83VAoB8sh!nL~=LlaHelWJ|$OI1nes7}$JaB!Bmed|N$FwaRsF>pMXxN5) z0WNX5r(3kYyypPz0ow;za9bXGnSVsvDquzzdtU&Mp8?9WfHL!g1XIp{Q)UJQBFL>` z+SeQaQ=Pw1zAjYVipI`>N-tJZ-^+{IVCK1MP+CW0>1`c(AL9yv~BjFF>lyYew zlc?9wFLr6KeAwuCCVk9g-_@sBYxK_IdM0);a8S7$)Z%Qy6-*|D;L5ll)f)g9EYI~+ zw}s;)kV2eE2gUIPbL{7`;|&L)S2dXD6I5>BUlJEE#q%1gD2yu0|8pJUJ$SW=xKwcA? zWQ#Ia+sv5*+t)WVb()H_MZq## zHvJ6{2@00kN@yl6%RnJ!8V0-~CaCom1=7VBHV$L(kQ}3b0=@jq7wPZFLRoY47kszF zW1F>2(4B7(^x_P>GXuYfj?-@z`J$f#o%I2FD(mbt z;lLA~^KSGgW!1LVPEod0d!bZM!(jSP{BENnbrU_M2P%?IqdrTD%#$b{`0H?{g^LFQ z?cNRA-C8r+R2!_n3j)|VO2_7A^@g~JX`Co20Ydb3+BS0S5@MVN{gT>1V~=tLyUkmfP3PUlGToj!$(S%ICg88 zBv1tCX=*>#6ovD97&CkV#K}2&(R9rPV?lQeIIrF_EV{4^s5tkXMc2V;o*+G^;|xDi z0b@jNV2t4}Iee!I;9pQWz$q4jcqxKtZ$soFB6Or+K|i`>WVMK%8CXvdce|B=6*Na> zL|Jw#i!y1mlOtx|HPm_D8&CnzV!TsTlE-gwr(T%vA@Vzx3Q2=KTvdx$CKnd21Um>~91f zY~Ed9J>RS1NZS!h<_SG!*wSQ7iVBf>B&fD z75*1V5dIfQ5Zg46KGj|i4nM{1k8@4nO1x{Q2G1@ZBhGz-Zr;mEbyaM_8V*_c=hrV-jW%}>j>iJN;g4HI>ku2E%W`O+YoVN`B3D^d_15ZU5)VdvipQ|F(Y46j~BZk_1 zA3M=Mb84uQg}cJjBPC9QG5xz<_m;Yy;K}3#()mt>UTs6cZ5$6%MM&XA|}LDzs&TnkVtTc_zwye_;4sX(oiKlzahIY zbYvTTq&~~`wutb4#{Dv?{sWhU1>78&@}vPEspqT(_(%J56AYOQT;tOXe@xTn2$-T@ zHwg`aJ+w<20S&_hy8u3K(TidApLjKhHdF%grwiGLnJLrU@KgQXqs{QY4z*quLmSF|ED=^0uDXL_x$tL`$&B;bfFoY6j=M{=z{M zPQ?u}?1f9#nDg+RBb~ORp@~o5XqgbsMF=PC76jY$bqKC_26hej1}s<%1Kw9_U?Zf0 ze(U+jX=QMx)1xfJ+}yQv3VIHsWMEBr{*&?734Qa!oNr+d?f$V#h-@4vgCHziN+S3u#9T;d6L5$W!=fKm1fFI~9!;&@4M(iepBG+PZ_ zfO6Cd<(#I*KAHn|S_Yrts?*V)GcB_Kj~)-`IiyM^B3~n(JI$aCBxihdKxB3aTm2<; zeb}Lhx?fR_Wb2aGKk875s-Fwvb#KU z>JbS|9bf8~67(_N&t*m7QAB)-E2U;gWh|rhC_)Q)T0dZ1jqJ8sJc6`4aWm+8MV=2{6T$&GEf42M41@te7v5-KK0{Q6_D6Mg$QMtGEx&$o4XY=G1k@J9-V zh2(EpKx^i_dDJ>8LBLYR0Cvm0*Po%ljgJuIf#J_Yg3>3F_bDX6n_}n$z{4oDMD7rp zEhXQ2q$M^(xJXk5C7heqrcx;CmyvP=aYapkXx;W{IL-dTHGr8mptO9x;7K$yys`|x zmnhJ4cn0+a5}qYv>*KesFUOe|FTch>Y-|El#O{j~u*!v3E~`Ih6GuLA2qjpV22vGA zLF8LDVZXhcnT&~sFLG5WDMdg9IyXs+(5J=&_?zZs<)%7JS#$D27MCpFZz2KJA*##Q z;aVYTFc5i66qMDncbwhC&Sg*v2Ms|g+KA%9lFbV!%|(`+udEi}F4D!F*mWkp%xGQhokQ=hesX;Oh5D{($5U zL6*u3>W^`cU1A3cl=?H)iFJ^T`b*a4S=&=V7zV!jCL3@|)L)T2OClR#UuIZa{U{1z zd#$?>2609@B7}aNt$vQ=GbC?<3gqxisi(Eot!9Wl&7h;@%*SJo4|Ky9|X_kB>YaC`h#X{mfv z?1K6&7K^qQ?v=-)r5l4a^*hXpRrnL8#_)H<$#FJ-<0*izC{W*GnjTY4b2Ehnl+K=Dk%Fzna{ntksN`9i`w08Z;xWXRL-ey;I96LM05sUBhe@@&$T8W^{g^pM91_Dn)A<4ypoAeDgr(?(fR=s-Zg{=#$}6ME!$9r6NB9Rm z$5@k$rcqx9557Bi&~{;878v(JI;lf^)Z9T#bC6@K6{jR`z^H=Tzxoo;jR&ZK zs4~&bM&bbV>I%-X*Dk#L628rVZKk^c4J1l0PG0%+CV_17Jx8por6d^T@{_)3!w=$Y zyW43^sOOQ7jX4AwbH2lM3n;=8{CWGofI{n=A73r|AAw?yHAH0Y?7B0}G5pCbA^3 zWWnH{-~kGG#n${tijI*=rTj>uDE=O$>K9pMLlg8ilMvlMvB1C^JoJ0U(*h7DzGqAd z5-;8}5BogwA-#Yz6{T1JD8384erfqy>|DD{?0OKaRie*m93@sIKvsBJUQ(iBu|>I6 z50i=5eKz?->^W+X@EH&Zfyv@Yd``D)#O6%K=lCz$MX%^ Date: Sun, 22 Jun 2025 20:59:40 +0100 Subject: [PATCH 35/59] pkg rename complete --- auto_shepherd_simulation/setup.cfg | 4 ---- .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../auto_shepherd_simulation_ros2}/README.md | 0 .../__init__.py | 0 .../boid_training_simulator.py | 0 .../dog_control.py | 4 ++-- .../dog_control_simulator.py | 4 ++-- .../mapper/__init__.py | 0 .../mapper/mapper.py | 2 +- .../requirements.txt | 0 .../ros_interface.py | 0 .../sheep_simulation/__init__.py | 0 .../sheep_simulation/sheep.py | 0 .../sheep_simulation/sheepdog.py | 0 .../sheep_simulation/simulation.py | 4 ++-- .../sheepdog_teleop.py | 0 .../tmpDogSim/__init__.py | 0 .../tmpDogSim/dog_control_lib.py | 4 ++-- .../tmpDogSim/main.py | 0 .../utils/geo_converter.py | 0 .../package.xml | 4 ++-- .../resource/auto_shepherd_simulation_ros2 | 0 auto_shepherd_simulation_ros2/setup.cfg | 4 ++++ .../setup.py | 2 +- bash_scripts/container/bashrc_extensions.sh | 2 +- bash_scripts/container/launcher.yaml | 22 ++++++------------- docker/docker-compose.yml | 2 +- 28 files changed, 27 insertions(+), 35 deletions(-) delete mode 100644 auto_shepherd_simulation/setup.cfg rename {auto_shepherd_simulation => auto_shepherd_simulation_ros2}/CMakeLists.txt (80%) rename {auto_shepherd_simulation => auto_shepherd_simulation_ros2}/README.md (83%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/README.md (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/__init__.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/boid_training_simulator.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/dog_control.py (96%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/dog_control_simulator.py (98%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/mapper/__init__.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/mapper/mapper.py (98%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/requirements.txt (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/ros_interface.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/sheep_simulation/__init__.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/sheep_simulation/sheep.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/sheep_simulation/sheepdog.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/sheep_simulation/simulation.py (98%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/sheepdog_teleop.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/tmpDogSim/__init__.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/tmpDogSim/dog_control_lib.py (97%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/tmpDogSim/main.py (100%) rename {auto_shepherd_simulation/auto_shepherd_simulation => auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2}/utils/geo_converter.py (100%) rename {auto_shepherd_simulation => auto_shepherd_simulation_ros2}/package.xml (82%) rename auto_shepherd_simulation/resource/auto_shepherd_simulation => auto_shepherd_simulation_ros2/resource/auto_shepherd_simulation_ros2 (100%) create mode 100644 auto_shepherd_simulation_ros2/setup.cfg rename {auto_shepherd_simulation => auto_shepherd_simulation_ros2}/setup.py (95%) diff --git a/auto_shepherd_simulation/setup.cfg b/auto_shepherd_simulation/setup.cfg deleted file mode 100644 index 60dbc19..0000000 --- a/auto_shepherd_simulation/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/auto_shepherd_simulation -[install] -install_scripts=$base/lib/auto_shepherd_simulation diff --git a/auto_shepherd_simulation/CMakeLists.txt b/auto_shepherd_simulation_ros2/CMakeLists.txt similarity index 80% rename from auto_shepherd_simulation/CMakeLists.txt rename to auto_shepherd_simulation_ros2/CMakeLists.txt index b4dd6ad..ba69dc0 100644 --- a/auto_shepherd_simulation/CMakeLists.txt +++ b/auto_shepherd_simulation_ros2/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(auto_shepherd_simulation) +project(auto_shepherd_simulation_ros2) # Export information to downstream packages ament_export_dependencies(rosidl_default_runtime) diff --git a/auto_shepherd_simulation/README.md b/auto_shepherd_simulation_ros2/README.md similarity index 83% rename from auto_shepherd_simulation/README.md rename to auto_shepherd_simulation_ros2/README.md index 1019694..9746549 100644 --- a/auto_shepherd_simulation/README.md +++ b/auto_shepherd_simulation_ros2/README.md @@ -1,4 +1,4 @@ -# auto_shepherd_simulation +# auto_shepherd_simulation_ros2 1. create repo from template 2. git clone the new repo 3. cd into the bash folder diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/README.md b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/README.md similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/README.md rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/README.md diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/__init__.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/__init__.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/__init__.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/boid_training_simulator.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/boid_training_simulator.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/boid_training_simulator.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/boid_training_simulator.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/dog_control.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py similarity index 96% rename from auto_shepherd_simulation/auto_shepherd_simulation/dog_control.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py index 52c8001..68bada3 100644 --- a/auto_shepherd_simulation/auto_shepherd_simulation/dog_control.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control.py @@ -6,8 +6,8 @@ import numpy as np import math # from shapely.geometry import Point, LineString -from auto_shepherd_simulation.tmpDogSim.dog_control_lib import find_best_dog_position, pure_pursuit, plot_current_state -from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter +from auto_shepherd_simulation_ros2.tmpDogSim.dog_control_lib import find_best_dog_position, pure_pursuit, plot_current_state +from auto_shepherd_simulation_ros2.utils.geo_converter import load_coords_from_yaml, MapConverter class DogController(Node): def __init__(self): diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/dog_control_simulator.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py similarity index 98% rename from auto_shepherd_simulation/auto_shepherd_simulation/dog_control_simulator.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py index 92c6131..e307f84 100644 --- a/auto_shepherd_simulation/auto_shepherd_simulation/dog_control_simulator.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py @@ -9,8 +9,8 @@ from std_msgs.msg import ColorRGBA from rclpy.callback_groups import ReentrantCallbackGroup as RCG -from auto_shepherd_simulation.sheep_simulation.simulation import Simulation -from auto_shepherd_simulation.utils.geo_converter import MapConverter, load_coords_from_yaml +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): def __init__(self): diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/mapper/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/__init__.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/mapper/__init__.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/__init__.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/mapper/mapper.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py similarity index 98% rename from auto_shepherd_simulation/auto_shepherd_simulation/mapper/mapper.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py index 4c63da8..959f503 100644 --- a/auto_shepherd_simulation/auto_shepherd_simulation/mapper/mapper.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py @@ -11,7 +11,7 @@ from tf_transformations import quaternion_from_euler from typing import List, Tuple -from auto_shepherd_simulation.utils.geo_converter import MapConverter, load_coords_from_yaml +from auto_shepherd_simulation_ros2.utils.geo_converter import MapConverter, load_coords_from_yaml class MapperNode(Node): def __init__(self): diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/requirements.txt b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/requirements.txt similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/requirements.txt rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/requirements.txt diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/ros_interface.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/ros_interface.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/ros_interface.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/ros_interface.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/__init__.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/__init__.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/__init__.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheep.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/sheep.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheep.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/sheep.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheepdog.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/sheepdog.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/sheepdog.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/sheepdog.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/simulation.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py similarity index 98% rename from auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/simulation.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py index 9236036..bbdae80 100644 --- a/auto_shepherd_simulation/auto_shepherd_simulation/sheep_simulation/simulation.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheep_simulation/simulation.py @@ -4,8 +4,8 @@ import numpy as np import yaml import os -from auto_shepherd_simulation.sheep_simulation.sheep import Sheep -from auto_shepherd_simulation.sheep_simulation.sheepdog import SheepDog, SheepDogController +from auto_shepherd_simulation_ros2.sheep_simulation.sheep import Sheep +from auto_shepherd_simulation_ros2.sheep_simulation.sheepdog import SheepDog, SheepDogController diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/sheepdog_teleop.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheepdog_teleop.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/sheepdog_teleop.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sheepdog_teleop.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/__init__.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/__init__.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/__init__.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/__init__.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py similarity index 97% rename from auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py index b732ef8..2ef82f1 100644 --- a/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/dog_control_lib.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py @@ -5,8 +5,8 @@ from matplotlib.path import Path import numpy as np from sklearn.cluster import DBSCAN -from auto_shepherd_simulation.sheep_simulation.simulation import Simulation -from auto_shepherd_simulation.utils.geo_converter import load_coords_from_yaml, MapConverter +from auto_shepherd_simulation_ros2.sheep_simulation.simulation import Simulation +from auto_shepherd_simulation_ros2.utils.geo_converter import load_coords_from_yaml, MapConverter try: mc = MapConverter(load_coords_from_yaml("/home/ros/map/map1.yaml")) diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/main.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/main.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/tmpDogSim/main.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/main.py diff --git a/auto_shepherd_simulation/auto_shepherd_simulation/utils/geo_converter.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py similarity index 100% rename from auto_shepherd_simulation/auto_shepherd_simulation/utils/geo_converter.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/utils/geo_converter.py diff --git a/auto_shepherd_simulation/package.xml b/auto_shepherd_simulation_ros2/package.xml similarity index 82% rename from auto_shepherd_simulation/package.xml rename to auto_shepherd_simulation_ros2/package.xml index f33d022..d6fb6d5 100644 --- a/auto_shepherd_simulation/package.xml +++ b/auto_shepherd_simulation_ros2/package.xml @@ -1,9 +1,9 @@ - auto_shepherd_simulation + auto_shepherd_simulation_ros2 0.0.1 - The auto_shepherd_simulation package + The auto_shepherd_simulation_ros2 package james TODO: License declaration diff --git a/auto_shepherd_simulation/resource/auto_shepherd_simulation b/auto_shepherd_simulation_ros2/resource/auto_shepherd_simulation_ros2 similarity index 100% rename from auto_shepherd_simulation/resource/auto_shepherd_simulation rename to auto_shepherd_simulation_ros2/resource/auto_shepherd_simulation_ros2 diff --git a/auto_shepherd_simulation_ros2/setup.cfg b/auto_shepherd_simulation_ros2/setup.cfg new file mode 100644 index 0000000..2d3d815 --- /dev/null +++ b/auto_shepherd_simulation_ros2/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/auto_shepherd_simulation_ros2 +[install] +install_scripts=$base/lib/auto_shepherd_simulation_ros2 diff --git a/auto_shepherd_simulation/setup.py b/auto_shepherd_simulation_ros2/setup.py similarity index 95% rename from auto_shepherd_simulation/setup.py rename to auto_shepherd_simulation_ros2/setup.py index 47fa0ac..91a73cd 100644 --- a/auto_shepherd_simulation/setup.py +++ b/auto_shepherd_simulation_ros2/setup.py @@ -3,7 +3,7 @@ from glob import glob import os -package_name = 'auto_shepherd_simulation' +package_name = 'auto_shepherd_simulation_ros2' pkg = package_name setup( diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index 1048947..d8d18bb 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -46,4 +46,4 @@ echo "ROS 2 workspace setup and sourced. Happy robot wrangling!" # Mark that the setup has been run in this shell session export _ROS_WORKSPACE_SETUP_RUN=true -cd ~/base_ws/src/auto_shepherd_simulation/auto_shepherd_simulation +cd ~/base_ws/src/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2 diff --git a/bash_scripts/container/launcher.yaml b/bash_scripts/container/launcher.yaml index a5a87e1..51d774a 100644 --- a/bash_scripts/container/launcher.yaml +++ b/bash_scripts/container/launcher.yaml @@ -3,28 +3,20 @@ init_cmd: | echo " " echo " " echo " " - echo "Running Initialisation" | awk '{ gsub("Running Initialisation", "\033[1;21m\033[1;30m&\033[0m"); print }' ; + echo "Running Initialisation" #exec /bin/bash cd @TMULE_CONFIG_DIR@ || true set -o pipefail - function export_default () { - var_name="$1" - var_default="$2" - eval $var_name="${!var_name:-$var_default}" - export $var_name - echo " $0 -> $var_name=${!var_name}" - } windows: - name: mapper - tags: ["viz", "mapper"] panes: - - "ros2 run auto_shepherd_simulation mapper.py" - # - "rviz2" + - ros2 run auto_shepherd_simulation_ros2 mapper.py + - rviz2 + - name: simulator - tags: ["ros_interface", "dog_control_simulator", "dog_control"] panes: - - ros2 run auto_shepherd_simulation ros_interface.py - - ros2 run auto_shepherd_simulation dog_control_simulator.py - - ros2 run auto_shepherd_simulation dog_control.py + - ros2 run auto_shepherd_simulation_ros2 ros_interface.py + - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py + - ros2 run auto_shepherd_simulation_ros2 dog_control.py diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index aea6764..a43fd3f 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -15,7 +15,7 @@ services: # — Bind‑mount each git sub‑module into the workspace ———————————— volumes: - - ../auto_shepherd_simulation:/home/ros/base_ws/src/auto_shepherd_simulation:rw + - ../auto_shepherd_simulation_ros2:/home/ros/base_ws/src/auto_shepherd_simulation_ros2:rw # Bash Scripts - ../bash_scripts/container:/home/ros/bash_scripts:rw From 0e7b1a5c32751f2ea50d80e8d714bfc84705c56f Mon Sep 17 00:00:00 2001 From: "James R. Heselden" Date: Sun, 22 Jun 2025 22:25:00 +0100 Subject: [PATCH 36/59] tmule moved --- .../tmule/launcher.tmule.yaml | 22 ++++++++++++++ bash_scripts/container/bashrc_extensions.sh | 7 ++++- bash_scripts/container/launcher.yaml | 30 ------------------- docker/Dockerfile | 9 ------ 4 files changed, 28 insertions(+), 40 deletions(-) create mode 100644 auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml delete mode 100644 bash_scripts/container/launcher.yaml diff --git a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml new file mode 100644 index 0000000..51d774a --- /dev/null +++ b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml @@ -0,0 +1,22 @@ +session: simulator +init_cmd: | + echo " " + echo " " + echo " " + echo "Running Initialisation" + + #exec /bin/bash + cd @TMULE_CONFIG_DIR@ || true + set -o pipefail + +windows: + - name: mapper + panes: + - ros2 run auto_shepherd_simulation_ros2 mapper.py + - rviz2 + + - name: simulator + panes: + - ros2 run auto_shepherd_simulation_ros2 ros_interface.py + - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py + - ros2 run auto_shepherd_simulation_ros2 dog_control.py diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index f84081d..253bf82 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -46,4 +46,9 @@ echo "ROS 2 workspace setup and sourced. Happy robot wrangling!" # Mark that the setup has been run in this shell session export _ROS_WORKSPACE_SETUP_RUN=true -cd ~/base_ws/src/ros2_package/auto_shepherd_simulation + +# Define custom functions to control the tmule +export TMULE_FILE=${BASE_WS}/src/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml +function s(){ tmule -c $TMULE_FILE -W 3 launch ; } +function t(){ tmule -c $TMULE_FILE terminate ; } +function r(){ tmule -c $TMULE_FILE -W 3 relaunch ; } diff --git a/bash_scripts/container/launcher.yaml b/bash_scripts/container/launcher.yaml deleted file mode 100644 index a5a87e1..0000000 --- a/bash_scripts/container/launcher.yaml +++ /dev/null @@ -1,30 +0,0 @@ -session: simulator -init_cmd: | - echo " " - echo " " - echo " " - echo "Running Initialisation" | awk '{ gsub("Running Initialisation", "\033[1;21m\033[1;30m&\033[0m"); print }' ; - - #exec /bin/bash - cd @TMULE_CONFIG_DIR@ || true - set -o pipefail - function export_default () { - var_name="$1" - var_default="$2" - eval $var_name="${!var_name:-$var_default}" - export $var_name - echo " $0 -> $var_name=${!var_name}" - } - -windows: - - name: mapper - tags: ["viz", "mapper"] - panes: - - "ros2 run auto_shepherd_simulation mapper.py" - # - "rviz2" - - name: simulator - tags: ["ros_interface", "dog_control_simulator", "dog_control"] - panes: - - ros2 run auto_shepherd_simulation ros_interface.py - - ros2 run auto_shepherd_simulation dog_control_simulator.py - - ros2 run auto_shepherd_simulation dog_control.py diff --git a/docker/Dockerfile b/docker/Dockerfile index e057f4d..1eaf5fe 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -146,15 +146,6 @@ RUN echo '[ -f $HOME/bash_scripts/bashrc_extensions.sh ] && \ source $HOME/bash_scripts/bashrc_extensions.sh' \ >> /home/${USERNAME}/.bashrc -RUN echo "function s(){ tmule -c ~/bash_scripts/launcher.yaml -W 3 launch ; }" \ - >> /home/ros/.bashrc - -RUN echo "function t(){ tmule -c ~/bash_scripts/launcher.yaml terminate ; }" \ - >> /home/ros/.bashrc - -RUN echo "function r(){ tmule -c ~/bash_scripts/launcher.yaml -W 3 relaunch ; }" \ - >> /home/ros/.bashrc - # ────────────────────────────── # 9. Default shell # ────────────────────────────── From 3dc6fc9bd409bc11f79ad699c5ef5c993cb291c3 Mon Sep 17 00:00:00 2001 From: "James R. Heselden" Date: Mon, 23 Jun 2025 07:23:22 +0100 Subject: [PATCH 37/59] tmule window rename --- auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml index 51d774a..abd1111 100644 --- a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml @@ -10,9 +10,12 @@ init_cmd: | set -o pipefail windows: - - name: mapper + - name: data_loader panes: - ros2 run auto_shepherd_simulation_ros2 mapper.py + + - name: visuals + panes: - rviz2 - name: simulator From 37d4e39d3784bf6d65ed10805495c39eb0327cba Mon Sep 17 00:00:00 2001 From: "James R. Heselden" Date: Tue, 24 Jun 2025 21:13:12 +0100 Subject: [PATCH 38/59] setup for launching from auto_shepherd --- .../{ros_interface.py => sim_data_loader.py} | 28 ++++--- auto_shepherd_simulation_ros2/setup.py | 2 +- .../tmule/internal.tmule.yaml | 25 ++++++ .../tmule/launcher.tmule.yaml | 13 ++- configs/rviz/default.rviz | 79 ++++++++++++++----- docker/Dockerfile | 3 +- 6 files changed, 111 insertions(+), 39 deletions(-) rename auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/{ros_interface.py => sim_data_loader.py} (85%) create mode 100644 auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/ros_interface.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py similarity index 85% rename from auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/ros_interface.py rename to auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py index e880b54..abdd138 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/ros_interface.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py @@ -9,21 +9,29 @@ import numpy as np from builtin_interfaces.msg import Duration -class RosInterface(Node): +class SimDataLoader(Node): def __init__(self): - super().__init__('ros_interface') + super().__init__('sim_data_loader') + + # Connect to drone topics self.drone_publisher = self.create_publisher(PoseStamped, '/drone/pose', 10) + + # Connect to dog topics self.dog_publisher = self.create_publisher(PoseStamped, '/dog/pose', 10) + self.dog_command_subscription = self.create_subscription(PoseStamped, '/dog/command', self.dog_command_callback, 10) + + # Connect to sheep topics self.sheep_publisher = self.create_publisher(Path, '/sheep/poses', 10) + + # Connect to target topics self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/goal_pose', 10) self.sheep_goal_subscriber = self.create_subscription(PoseStamped, '/goal_pose', self.sheep_goal_callback, 10) - - self.dog_command_subscription = self.create_subscription(PoseStamped, '/dog/command', self.dog_command_callback, 10) - self.timer = self.create_timer(0.1, self.timer_callback) - - # self.sheep_goal_publisher.publish(sheep_goal_pose) self.sheep_goal_pose = self.create_pose_stamped(-38.0, 90.0, 0.0) + # Define publisher to periodically republish the simulated data + #self.timer = self.create_timer(0.1, self.timer_callback) + self.timer_callback() + def sheep_goal_callback(self, msg): self.sheep_goal_pose = msg @@ -75,9 +83,9 @@ def dog_command_callback(self, msg): def main(args=None): rclpy.init(args=args) - ros_interface = RosInterface() - rclpy.spin(ros_interface) - ros_interface.destroy_node() + sim_data_loader = SimDataLoader() + rclpy.spin(sim_data_loader) + sim_data_loader.destroy_node() rclpy.shutdown() if __name__ == '__main__': diff --git a/auto_shepherd_simulation_ros2/setup.py b/auto_shepherd_simulation_ros2/setup.py index 91a73cd..7b6f10f 100644 --- a/auto_shepherd_simulation_ros2/setup.py +++ b/auto_shepherd_simulation_ros2/setup.py @@ -22,7 +22,7 @@ license='TODO: License declaration', entry_points={ 'console_scripts': [ - f'ros_interface.py = {pkg}.ros_interface:main', + 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', diff --git a/auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml new file mode 100644 index 0000000..cbd6020 --- /dev/null +++ b/auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml @@ -0,0 +1,25 @@ +session: simulator +init_cmd: | + echo " " + echo " " + echo " " + echo "Running Initialisation" + + #exec /bin/bash + cd @TMULE_CONFIG_DIR@ || true + set -o pipefail + +windows: + - name: data_loader + panes: + - ros2 run auto_shepherd_simulation_ros2 mapper.py + - ros2 run auto_shepherd_simulation_ros2 sim_data_loader.py + + - name: visuals + panes: + - rviz2 + + - name: simulator + panes: + - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py + - ros2 run auto_shepherd_simulation_ros2 dog_control.py diff --git a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml index abd1111..dc4229a 100644 --- a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml @@ -10,16 +10,15 @@ init_cmd: | set -o pipefail windows: - - name: data_loader - panes: - - ros2 run auto_shepherd_simulation_ros2 mapper.py +# - name: data_loader +# panes: +# - ros2 run auto_shepherd_simulation_ros2 mapper.py - - name: visuals - panes: - - rviz2 +# - name: visuals +# panes: +# - rviz2 - name: simulator panes: - - ros2 run auto_shepherd_simulation_ros2 ros_interface.py - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py - ros2 run auto_shepherd_simulation_ros2 dog_control.py diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 48d7b68..43477bb 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -1,18 +1,13 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Status1 - - /Grid1 - - /Path1 - - /Path1/Topic1 - - /Path1/Offset1 - - /Pose2 + - /Path2 Splitter Ratio: 0.5 - Tree Height: 1204 + Tree Height: 458 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -100,7 +95,7 @@ Visualization Manager: Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/Pose Color: 0; 25; 255 - Enabled: false + Enabled: true Head Length: 1 Head Radius: 1 Name: Pose @@ -114,7 +109,21 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dog/command - Value: false + Value: true + - 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: /drone_feed + Value: true - Alpha: 1 Axes Length: 3 Axes Radius: 0.5 @@ -135,6 +144,34 @@ Visualization Manager: Reliability Policy: Reliable Value: /goal_pose Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: 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 + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/poses + Value: true Enabled: true Global Options: Background Color: 48; 80; 48 @@ -181,33 +218,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 168.8179931640625 + Distance: 155.27615356445312 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -51.37569046020508 - Y: 64.75483703613281 - Z: -8.768682479858398 + X: -53.533905029296875 + Y: 65.6354751586914 + Z: -8.464354515075684 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -1.4047964811325073 + Pitch: -1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 6.21675968170166 + Yaw: 6.276760101318359 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1495 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000053dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000053d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000008a40000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000022a0000035efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000205000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000246000001530000002800ffffff000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005500000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -216,6 +255,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2560 + Width: 1920 X: 0 - Y: 32 + Y: 27 diff --git a/docker/Dockerfile b/docker/Dockerfile index 1eaf5fe..745c83e 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -149,4 +149,5 @@ RUN echo '[ -f $HOME/bash_scripts/bashrc_extensions.sh ] && \ # ────────────────────────────── # 9. Default shell # ────────────────────────────── -CMD ["bin/bash"] +#CMD ["/bin/bash"] +CMD ["tail", "-f", "/dev/null"] From 1fcd892ffb497c52c8ddc334f093cd50bfd2c0d9 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 10:54:29 +0100 Subject: [PATCH 39/59] new tmule launching configured (with gps setup reworked) --- .../dog_control.py | 48 ++- .../dog_control_simulator.py | 176 +++++---- .../mapper/mapper.py | 122 ++---- .../sheep_simulation/simulation.py | 17 +- .../sim_data_loader.py | 155 ++++---- .../utils/geo_converter.py | 40 +- ...uncher.tmule.yaml => connected.tmule.yaml} | 22 +- ...nternal.tmule.yaml => injected.tmule.yaml} | 20 +- bash_scripts/container/bashrc_extensions.sh | 21 +- bash_scripts/container/tmux.conf | 23 ++ configs/rviz/default.rviz | 351 +++++++++++------- 11 files changed, 540 insertions(+), 455 deletions(-) rename auto_shepherd_simulation_ros2/tmule/{launcher.tmule.yaml => connected.tmule.yaml} (63%) rename auto_shepherd_simulation_ros2/tmule/{internal.tmule.yaml => injected.tmule.yaml} (72%) create mode 100644 bash_scripts/container/tmux.conf 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 68bada3..6527188 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 @@ -1,12 +1,14 @@ +import numpy as np +import math + import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy + from geometry_msgs.msg import PoseStamped, Point, Quaternion from nav_msgs.msg import Path -from std_msgs.msg import Float64MultiArray -import numpy as np -import math -# from shapely.geometry import Point, LineString -from auto_shepherd_simulation_ros2.tmpDogSim.dog_control_lib import find_best_dog_position, pure_pursuit, plot_current_state + +from auto_shepherd_simulation_ros2.tmpDogSim.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): @@ -14,16 +16,11 @@ def __init__(self): super().__init__('dog_controller') # publishers / subscribers --------------------------------------- - self.cmd_pub = self.create_publisher( - PoseStamped, '/dog/command', 10) # :contentReference[oaicite:2]{index=2} - - self.create_subscription(PoseStamped, '/dog/pose', - self._dog_cb, 10) # :contentReference[oaicite:3]{index=3} - self.create_subscription(Path, '/sheep/poses_sim', - self._sheep_cb, 10) # :contentReference[oaicite:4]{index=4} - self.create_subscription(PoseStamped, '/goal_pose', - self._goal_cb, 10) - + self.cmd_pub = self.create_publisher(PoseStamped, '/dog/command', 10) + self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, self.get_qos()) + self.create_subscription(Path, '/sheep/poses_sim', self._sheep_cb, 10) + self.create_subscription(PoseStamped, '/sheep/goal', self._goal_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 @@ -49,7 +46,6 @@ def __init__(self): 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: @@ -57,6 +53,15 @@ def __init__(self): self.map_converter = None # Ensure map_converter is not set if initialization failed + def get_qos(self): + qos_profile = QoSProfile( + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + return qos_profile + + # ------------ message callbacks ------------------------------------- def _dog_cb(self, msg: PoseStamped): self.dog_xy = (msg.pose.position.x, msg.pose.position.y) @@ -137,16 +142,12 @@ def _control_step(self): xs, ys = self.sheep_xy[:, 0], self.sheep_xy[:, 1] xc, yc = self.goal_xy - # if not self.lap_done: - # xd_opt, yd_opt = self._boundary_follow_step() - # print(f"Boundary follow: ({xd_opt:.2f}, {yd_opt:.2f})") - # else: xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc, self.field_boundary) print(f"Optimised dog position: ({xd_opt:.2f}, {yd_opt:.2f})") ps = PoseStamped() ps.header.stamp = self.get_clock().now().to_msg() - ps.header.frame_id = "map" # or any frame you prefer + ps.header.frame_id = "field_frame" # or any frame you prefer ps.pose.position = Point(x=float(xd_opt), y=float(yd_opt), z=0.0) ps.pose.orientation = Quaternion(w=1.0) # identity; adjust if needed self.cmd_pub.publish(ps) @@ -154,12 +155,7 @@ def _control_step(self): self._planned_dog_xy = (xd_opt, yd_opt) - # plot - # plot_current_state(xs, ys, xd, yd, xc, yc, xd_opt, yd_opt) - -# ---------------------------------------------------------------------- -# entry-point ----------------------------------------------------------- def main(args=None): rclpy.init(args=args) node = DogController() diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py index e307f84..3cb3ac8 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/dog_control_simulator.py @@ -1,13 +1,15 @@ import os +import math import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from rclpy.callback_groups import ReentrantCallbackGroup as RCG + +from std_msgs.msg import ColorRGBA, UInt16 +from geometry_msgs.msg import Point, Vector3, PoseStamped, PoseWithCovarianceStamped from nav_msgs.msg import Path as Path from visualization_msgs.msg import Marker, MarkerArray -from geometry_msgs.msg import Point, Vector3, PoseStamped -from std_msgs.msg import ColorRGBA -from rclpy.callback_groups import ReentrantCallbackGroup as RCG from auto_shepherd_simulation_ros2.sheep_simulation.simulation import Simulation from auto_shepherd_simulation_ros2.utils.geo_converter import MapConverter, load_coords_from_yaml @@ -18,6 +20,7 @@ def __init__(self): # Default the storage points for the animal data self.sheep_poses = {} # {sheep_name: [dict, dict, ...]} + self.sheep_pose_store = {} # self.dog_poses = {} # {timestep: {dog_name: PoseStamped}} self.dog_state = None self.dog_command = None # Store the latest dog command @@ -29,47 +32,49 @@ def __init__(self): history=HistoryPolicy.KEEP_LAST, depth=10 ) + self.frame = 0 - # Subscribe to input data - self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, qos_profile) - self.create_subscription(Path, '/sheep/poses', self._sheep_cb, qos_profile) + # For dog connections + self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, self.get_qos()) self.create_subscription(PoseStamped, '/dog/command', self._dog_command_cb, qos_profile, callback_group=RCG()) - - # Setup output channels self.create_publisher(Path, '/dog_paths', qos_profile) - self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) + + # For sheep connections + self.create_subscription(Path, '/sheep/poses', self._sheep_cb, qos_profile) + self.create_subscription(UInt16, '/sheep/randomise', self._sheep_randomise_cb, self.get_qos()) # For sim data self.sheep_sim_pub = self.create_publisher(Path, '/sheep/poses_sim', qos_profile) + # Setup rviz visuals + self.marker_pub = self.create_publisher(MarkerArray, '/simulation_markers', qos_profile) + # Load the field boundaries 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 + self._load_map(yaml_map_file_path) + #TODO: self.create_subscription(String, '/field/boundaries', self._load_map, qos_profile) + # Start Simulation + self.simulation = Simulation(self.field_boundary, 800, 600, sheep_states=None, sheepdog_state=None, spawn_random=False) + self.dt = 0.05 + self.sim_step_timer = self.create_timer(self.dt, self.run_sim_step, callback_group=RCG()) + print('Dog Control Simulator Initialised') - # Create Map Bounding Box & Convert All Coords - try: - map_converter = MapConverter(field_coords_latlon) - map_data = 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}") - map_converter = None # Ensure map_converter is not set if initialization failed + def get_qos(self): + qos_profile = QoSProfile( + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + return qos_profile - self.simulation = Simulation(self.field_boundary, 800, 600, sheep_states=None, sheepdog_state=None) - self.dt = 0.05 - self.sim_step_timer = self.create_timer(self.dt, self.run_sim_step, callback_group=RCG()) + 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) - print('Dog Control Simulator Initialised') + # Create Map Bounding Box & Convert All Coords + self.map_converter = MapConverter(field_coords_latlon) + map_data = self.map_converter.get_map_data() + self.field_boundary = map_data['map_coords_xy_meters'] @@ -77,66 +82,71 @@ def _dog_command_cb(self, msg): """Callback for dog command messages""" self.dog_command = { - 'position': [msg.pose.position.x, msg.pose.position.y], + 'position': [msg.pose.position.y, msg.pose.position.x], 'orientation': [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w], 'velocity': [0,0] } self.get_logger().info(f'Received new dog command: {self.dog_command}') - def _dog_cb(self, msg): - # timestep = msg.header.secs - # dog_name = msg.header.frame_id or 'dog' - - # Initialise storage - # if timestep not in self.dog_poses: - # self.dog_poses[timestep] = dict() - - # Save dog position at timestep - # self.dog_poses[timestep][dog_name] = msg + def _dog_cb(self, msg): self.dog_state = { 'position': [msg.pose.position.x, msg.pose.position.y], 'velocity': [0.0, 0.0] # you can compute real velocity later } + 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): + self.simulation.num_sheep = msg.data + self.simulation.sheep_list = [] + self.simulation._initialize_sheep(None, spawn_random=True) + self.get_logger().info(f"Initialised {msg.data} sheep.") def _sheep_cb(self, msg): + + self.frame += 1 + #if not str(self.frame).endswith('0'): return + # Get current timestep - timestep = msg.header.stamp.sec + print('_______________') + self.sheep_poses = {} for sheep in msg.poses: - + # Create sheep creator name = sheep.header.frame_id if name not in self.sheep_poses: self.sheep_poses[name] = [] + if name not in self.sheep_pose_store: + self.sheep_pose_store[name] = [] # Get prior pose prior = None - if self.sheep_poses[name]: - prior = self.sheep_poses[name][-1]['position'] + if self.sheep_pose_store[name]: + prior = self.sheep_pose_store[name][-1]['position'] # Save sheep pose data pos = sheep.pose.position - current = {'position': [pos.x, pos.y]} + x, y = self.map_converter.latlon_to_xy(pos.x, pos.y) + print(pos.x, pos.y, x, y, self.map_converter.origin_lat, self.map_converter.origin_lon) + current = {'position': [x, y]} + self.sheep_pose_store[name].append(current) if prior: px, py = prior - current['velocity'] = [pos.x - px, pos.y - py] - - self.sheep_poses[name].append(current) - print("sheep poses ready") - print(self.simulation, self.dog_state) - # ------------- create Simulation once ----------------------------- - if self.simulation is None and self.dog_state is not None: - # flatten current sheep snapshot into list of dicts - sheep_states = [hist[-1] for hist in self.sheep_poses.values()] - self.simulation = Simulation( - self.field_boundary, - 800, 600, - sheep_states=sheep_states, - sheepdog_state=self.dog_state - ) - self.get_logger().info('Simulation initialised with ' - f'{len(sheep_states)} sheep.') + current['velocity'] = [x - px, y - py] + self.sheep_poses[name].append(current) + + total_tracked = len([len(s) for s in self.sheep_poses if s]) + self.get_logger().info(f"Callback detected with {len(msg.poses)} sheep, totalling {total_tracked} tracked target ids.") + + # Flatten current sheep snapshot into list of dicts + sheep_states = [hist[-1] for hist in self.sheep_poses.values() if hist] + self.get_logger().info(f"Sheep positions updated to {len(sheep_states)} sheep.") + + # Update sheep positions in simulation to input + self.simulation._initialize_sheep(sheep_states) + [print(s[-1]) for s in self.sheep_poses.values() if s] + def publish_simulation_state(self, state): """Convert simulation state to MarkerArray and publish for RViz visualization""" @@ -146,7 +156,7 @@ def publish_simulation_state(self, state): for i, sheep_state in enumerate(state['sheep']): # Create sheep marker sheep_marker = Marker() - sheep_marker.header.frame_id = "map" + sheep_marker.header.frame_id = "field_frame" sheep_marker.header.stamp = self.get_clock().now().to_msg() sheep_marker.ns = "sheep" sheep_marker.id = i @@ -154,8 +164,8 @@ def publish_simulation_state(self, state): sheep_marker.action = Marker.ADD # Set sheep position - sheep_marker.pose.position.x = sheep_state['position'][0] - sheep_marker.pose.position.y = sheep_state['position'][1] + sheep_marker.pose.position.x = sheep_state['position'][1] + sheep_marker.pose.position.y = sheep_state['position'][0] sheep_marker.pose.position.z = 0.0 # Set sheep size @@ -174,11 +184,11 @@ def publish_simulation_state(self, state): vel_marker.action = Marker.ADD # Set arrow points - start_point = Point(x=sheep_state['position'][0], - y=sheep_state['position'][1], + start_point = Point(x=sheep_state['position'][1], + y=sheep_state['position'][0], z=0.0) - end_point = Point(x=sheep_state['position'][0] + sheep_state['velocity'][0], - y=sheep_state['position'][1] + sheep_state['velocity'][1], + end_point = Point(x=sheep_state['position'][1] + sheep_state['velocity'][1], + y=sheep_state['position'][0] + sheep_state['velocity'][0], z=0.0) vel_marker.points = [start_point, end_point] @@ -193,7 +203,7 @@ def publish_simulation_state(self, state): # Create marker for sheepdog dog_state = state['sheepdog'] dog_marker = Marker() - dog_marker.header.frame_id = "map" + dog_marker.header.frame_id = "field_frame" dog_marker.header.stamp = self.get_clock().now().to_msg() dog_marker.ns = "sheepdog" dog_marker.id = 0 @@ -201,8 +211,8 @@ def publish_simulation_state(self, state): dog_marker.action = Marker.ADD # Set dog position - dog_marker.pose.position.x = dog_state['position'][0] - dog_marker.pose.position.y = dog_state['position'][1] + dog_marker.pose.position.x = dog_state['position'][1] + dog_marker.pose.position.y = dog_state['position'][0] dog_marker.pose.position.z = 0.0 # Set dog size (slightly larger than sheep) @@ -221,11 +231,11 @@ def publish_simulation_state(self, state): vel_marker.action = Marker.ADD # Set arrow points - start_point = Point(x=dog_state['position'][0], - y=dog_state['position'][1], + start_point = Point(x=dog_state['position'][1], + y=dog_state['position'][0], z=0.0) - end_point = Point(x=dog_state['position'][0] + dog_state['velocity'][0], - y=dog_state['position'][1] + dog_state['velocity'][1], + end_point = Point(x=dog_state['position'][1] + dog_state['velocity'][1], + y=dog_state['position'][0] + dog_state['velocity'][0], z=0.0) vel_marker.points = [start_point, end_point] @@ -240,6 +250,7 @@ def publish_simulation_state(self, state): # Publish the markers self.marker_pub.publish(marker_array) + def publish_sheep_path(self, state): """Convert simulation['sheep'] list → nav_msgs/Path and publish.""" path_msg = Path() @@ -251,8 +262,8 @@ def publish_sheep_path(self, state): ps = PoseStamped() ps.header = path_msg.header # same stamp / frame ps.header.frame_id = f"sheep_{i}" # optional: unique id - ps.pose.position.x = sheep['position'][0] - ps.pose.position.y = sheep['position'][1] + ps.pose.position.x = sheep['position'][1] + ps.pose.position.y = sheep['position'][0] ps.pose.position.z = 0.0 poses.append(ps) @@ -280,5 +291,6 @@ def main(): rclpy.spin(node) rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py index 959f503..c509151 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/mapper/mapper.py @@ -1,15 +1,14 @@ +from typing import List, Tuple + import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + from nav_msgs.msg import Path -from geometry_msgs.msg import PolygonStamped # Import for PolygonStamped -from geometry_msgs.msg import PoseStamped, Point, Quaternion, Point32 # Import Point32 for PolygonStamped -import yaml -import os +from geometry_msgs.msg import PolygonStamped, PoseStamped, Quaternion, Point32, TransformStamped + import tf2_ros -from geometry_msgs.msg import TransformStamped -from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy from tf_transformations import quaternion_from_euler -from typing import List, Tuple from auto_shepherd_simulation_ros2.utils.geo_converter import MapConverter, load_coords_from_yaml @@ -17,96 +16,48 @@ class MapperNode(Node): def __init__(self): super().__init__('mapper_node') - self.declare_parameter('map_file_path', '/home/ros/map/map1.yaml') - - # Define the QoS profile for a latched topic - qos_profile = QoSProfile( - depth=1, - reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL - ) - - # # Publisher for the Path message - self.publisher_path_ = self.create_publisher( # Renamed for clarity - Path, - 'FieldBoundaryPath', - qos_profile - ) - - # Publisher for the PolygonStamped message - self.publisher_polygon_ = self.create_publisher( - PolygonStamped, - 'field', # Topic name for the PolygonStamped message - qos_profile - ) + #TODO: Fences should not be published from here, they should be published from sim_data_loader.py instead - # Static Transform Broadcaster for the 'field_frame' + # 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.static_broadcaster = tf2_ros.StaticTransformBroadcaster(self) - # --- Integration with geo_converter.py --- - - # 1. Get the map file path from parameters - map_file_path = self.get_parameter('map_file_path').get_parameter_value().string_value + # 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) - # 2. Load raw (lat, lon) coordinates from the YAML file using the utility function - try: - raw_latlon_coords = load_coords_from_yaml(map_file_path) - if not raw_latlon_coords: - self.get_logger().error("Loaded map data is empty. Shutting down.") - rclpy.shutdown() - return - except (FileNotFoundError, ValueError) as e: - self.get_logger().error(f"Failed to load map data from YAML: {e}. Shutting down.") - rclpy.shutdown() - return - - # 3. Initialize the MapConverter with the raw LatLon coordinates - try: - self.map_converter = MapConverter(raw_latlon_coords) - map_data = self.map_converter.get_map_data() # Get processed data from the converter - - # Store the map's calculated origin (in UTM) - self.origin_utm_x = map_data['origin_utm_x'] - self.origin_utm_y = map_data['origin_utm_y'] + # Initialize the MapConverter + self.map_converter = MapConverter(raw_latlon_coords) + map_data = self.map_converter.get_map_data() - # Get the relative X, Y meters for the path and polygon - self.relative_xy_meters = map_data['map_coords_xy_meters'] + # Store map origin (in UTM) + self.origin_utm_x = map_data['origin_utm_x'] + self.origin_utm_y = map_data['origin_utm_y'] - self.get_logger().info(f"Map converted. Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") + # Get metric coords path and polygon + self.relative_xy_meters = map_data['map_coords_xy_meters'] + self.get_logger().info(f"Map converted. Origin (UTM X, Y): ({self.origin_utm_x:.3f}, {self.origin_utm_y:.3f})") - except ValueError as e: - self.get_logger().error(f"Error initializing MapConverter: {e}. Shutting down.") - rclpy.shutdown() - return - - # 4. Create PoseStamped messages for the Path + # Create messages for the path self.path_poses = self._create_path_poses(self.relative_xy_meters) - if not self.path_poses: - self.get_logger().error("Failed to create Path poses from converted data. Shutting down.") - rclpy.shutdown() - return - - # 5. Create Point32 messages for the PolygonStamped self.polygon_points = self._create_polygon_points(self.relative_xy_meters) - if not self.polygon_points: - self.get_logger().error("Failed to create PolygonStamped points from converted data. Shutting down.") - rclpy.shutdown() - return - - # --- Publish Data --- - # Publish the Path message + # Publish the Path, PolygonStamped, and static transform messages self.publish_path_once() - - # Publish the PolygonStamped message self.publish_polygon_once() - - # Publish the static transform for 'field_frame' self.publish_static_transform() - self.get_logger().info("Map published once as Path and Polygon. Static transform published. Node will now spin indefinitely.") + def get_qos(self): + qos_profile = QoSProfile( + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + return qos_profile + 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. @@ -121,8 +72,8 @@ def _create_path_poses(self, xy_meters: List[Tuple[float, float]]) -> List[PoseS pose_stamped = PoseStamped() pose_stamped.header.stamp = current_stamp pose_stamped.header.frame_id = 'field_frame' - pose_stamped.pose.position.x = x_m - pose_stamped.pose.position.y = y_m + pose_stamped.pose.position.x = y_m + pose_stamped.pose.position.y = x_m pose_stamped.pose.position.z = 0.0 pose_stamped.pose.orientation = identity_orientation poses.append(pose_stamped) @@ -191,7 +142,6 @@ def publish_polygon_once(self): 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'.""" t = TransformStamped() @@ -201,8 +151,8 @@ def publish_static_transform(self): # Set the translation to the absolute UTM coordinates of the bounding box origin. # This places 'field_frame' (and thus the relative Path/Polygon) at its real-world location in 'map'. - t.transform.translation.x = self.origin_utm_x - t.transform.translation.y = self.origin_utm_y + t.transform.translation.x = 0.0 #self.origin_utm_y + t.transform.translation.y = 0.0 #self.origin_utm_x t.transform.translation.z = 0.0 # Assuming 2D environment q = quaternion_from_euler(0, 0, 0) # Identity rotation for the frame itself 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 bbdae80..a086a73 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 @@ -156,7 +156,7 @@ def get_closest_boundary_point(self, x, y): return closest_point class Simulation: - def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None): + def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_states=None, sheepdog_state=None, spawn_random=False): """Initialize simulation with field boundary Args: @@ -191,7 +191,7 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st # Create a list of sheep self.num_sheep = 50 if sheep_states is None else len(sheep_states) self.sheep_list = [] - self._initialize_sheep(sheep_states) + self._initialize_sheep(sheep_states, spawn_random=spawn_random) # Flocking parameters self.alignment_weight = 1.0 @@ -201,9 +201,9 @@ def __init__(self, field_boundary, screen_width=800, screen_height=600, sheep_st # for i in range(200): # self.update(0.05) - def _initialize_sheep(self, sheep_states=None): + def _initialize_sheep(self, sheep_states=None, spawn_random=False): """Initialize sheep with given states or random positions""" - if sheep_states is None: + if spawn_random: print('random sheep init') # Initialize with random positions within field boundary for _ in range(self.num_sheep): @@ -222,13 +222,12 @@ def _initialize_sheep(self, sheep_states=None): coord_transformer=self.coord_transformer )) break - else: + elif sheep_states is not None: # Initialize with given states + self.sheep_list = [] for state in sheep_states: - if not isinstance(state, dict) or 'position' not in state: - raise ValueError("Sheep states must be provided as dictionaries with 'position' key") position = state['position'] - velocity = state.get('velocity', [0, 0]) # Default to zero velocity if not provided + velocity = state.get('velocity', [0.01, 0.01]) # Default to zero velocity if not provided self.sheep_list.append(Sheep( position=position, velocity=velocity, @@ -309,7 +308,7 @@ def __init__(self, width, height): field_boundary = load_map_config(map_file) # Initialize simulation - self.simulation = Simulation(field_boundary, width, height) + self.simulation = Simulation(field_boundary, width, height, spawn_random=True) # Initialize sheepdog controller self.sheepdog_controller = SheepDogController(self.simulation.sheepdog, width, height) diff --git a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py index abdd138..8259ce7 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py @@ -1,85 +1,104 @@ -# from sheep_simulation import sheep -# from random import shuffle -# from os import sep +import time +import numpy as np +from copy import deepcopy + import rclpy from rclpy.node import Node -from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped +from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy + +from std_msgs.msg import Header, UInt16 +from geometry_msgs.msg import Point, Quaternion, PoseStamped, PoseWithCovarianceStamped from nav_msgs.msg import Path -from std_msgs.msg import Float64MultiArray, Header -import numpy as np -from builtin_interfaces.msg import Duration +from visualization_msgs.msg import Marker, MarkerArray + + class SimDataLoader(Node): def __init__(self): super().__init__('sim_data_loader') - # Connect to drone topics - self.drone_publisher = self.create_publisher(PoseStamped, '/drone/pose', 10) + + #TODO: dog and goal initialisation could come form a config file. + #TODO: fences should be published form here too + # Connect to dog topics - self.dog_publisher = self.create_publisher(PoseStamped, '/dog/pose', 10) - self.dog_command_subscription = self.create_subscription(PoseStamped, '/dog/command', self.dog_command_callback, 10) + self.dog_publisher = self.create_publisher(PoseStamped, '/dog/pose', self.get_qos()) + self.dog_location_sub = self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.dog_initialpose_cb, 10) + + + # Connect to target topics + self.sheep_goal_subscriber = self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_cb, 10) + self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/sheep/goal', self.get_qos()) + self.sheep_goal_marker_pub = self.create_publisher(MarkerArray, '/sheep/goal_marker', 10) + # Connect to sheep topics - self.sheep_publisher = self.create_publisher(Path, '/sheep/poses', 10) + self.sheep_publisher = self.create_publisher(UInt16, '/sheep/randomise', self.get_qos()) + self.start_simulated_sheep() - # Connect to target topics - self.sheep_goal_publisher = self.create_publisher(PoseStamped, '/goal_pose', 10) - self.sheep_goal_subscriber = self.create_subscription(PoseStamped, '/goal_pose', self.sheep_goal_callback, 10) - self.sheep_goal_pose = self.create_pose_stamped(-38.0, 90.0, 0.0) - - # Define publisher to periodically republish the simulated data - #self.timer = self.create_timer(0.1, self.timer_callback) - self.timer_callback() - - def sheep_goal_callback(self, msg): - self.sheep_goal_pose = msg - - def create_header(self): - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = 'map' - return header - - def create_pose_stamped(self, x, y, z, qw=1.0, qx=0.0, qy=0.0, qz=0.0): - pose_stamped = PoseStamped() - pose_stamped.header = self.create_header() - pose_stamped.pose.position = Point(x=x, y=y, z=z) - pose_stamped.pose.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw) - return pose_stamped - - def timer_callback(self): - # Publish drone pose - drone_pose = self.create_pose_stamped(1.0, 2.0, 3.0) - self.drone_publisher.publish(drone_pose) - - # Publish dog pose - dog_pose = self.create_pose_stamped(-117.0, 50.0, 0.0) - self.dog_publisher.publish(dog_pose) - - # Publish sheep poses as a Path - sheep_path = Path() - sheep_path.header = self.create_header() - - # Example sheep poses with individual timestamps - sheep_path.poses = [ - self.create_pose_stamped(6.0, 7.0, 0.0), - self.create_pose_stamped(8.0, 9.0, 0.0) - ] - self.sheep_publisher.publish(sheep_path) - - # Publish sheep goal pose - # sheep_goal_pose = self.create_pose_stamped(-11.0, 13.0, 0.0) - # sheep_goal_pose = self.create_pose_stamped(-110.0, 45.0, 0.0) - self.sheep_goal_publisher.publish(self.sheep_goal_pose) - - def dog_command_callback(self, msg): - target_x = msg.pose.position.x - target_y = msg.pose.position.y - # orientation is already in quaternion form if you need it - self.get_logger().info( - f"Received dog command: Move to ({target_x:.2f}, {target_y:.2f})" + + def get_qos(self): + qos_profile = QoSProfile( + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1 ) + return qos_profile + + ######################### + ## DOG ## + ######################### + + def dog_initialpose_cb(self, msg): + # Sub to redirect /initialpose to /dog/pose + self.get_logger().info(f"Initialising dog to: x={msg.pose.pose.position.x} y={msg.pose.pose.position.y}") + ps = PoseStamped() + ps.pose = msg.pose.pose + ps.header = msg.header + self.dog_publisher.publish(ps) + + + ######################### + ## SHEEP ## + ######################### + + def start_simulated_sheep(self): + # Publish to the initialise random sheep poses + count = 10 + self.get_logger().info(f"Initialising {count} sheep randomly.") + self.sheep_publisher.publish(UInt16(data=count)) + + def goal_pose_cb(self, msg): + # Sub to redirect /goal_pose to /sheep/goal + self.get_logger().info(f"Setting goal pose to: x={msg.pose.position.x} y={msg.pose.position.y}") + self.sheep_goal_publisher.publish(msg) + + # Publish markers + colors = [(1.0, 0.0, 0.0), (1.0, 1.0, 1.0)] # red, white + radii = [2.5, 2.0, 1.5, 1.0, 0.5] # outer to inner + scale = 1.0 + marker_array = MarkerArray() + for i, radius in enumerate(radii): + marker = Marker() + marker.header = msg.header + marker.ns = 'target' + marker.id = i + marker.type = Marker.CYLINDER + marker.action = Marker.ADD + marker.pose = deepcopy(msg.pose) + marker.pose.position.z = msg.pose.position.z + (i*0.02) + marker.scale.x = marker.scale.y = (radius * scale) * 2 + marker.scale.z = 0.02 + color = colors[i % len(colors)] + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = 1.0 + marker.lifetime.sec = 0 + marker_array.markers.append(marker) + self.sheep_goal_marker_pub.publish(marker_array) + def main(args=None): rclpy.init(args=args) 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 7e97ec8..6323567 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 @@ -27,15 +27,19 @@ def __init__(self, map_coords_latlon: List[Tuple[float, float]]): min_lon = min(c[1] for c in map_coords_latlon) max_lon = max(c[1] for c in map_coords_latlon) + mid_lat = (min_lat+max_lat) / 2 + mid_lon = (min_lon+max_lon) / 2 + self.bounding_box_corners_latlon = { 'top_left': (max_lat, min_lon), 'top_right': (max_lat, max_lon), 'bottom_left': (min_lat, min_lon), - 'bottom_right': (min_lat, max_lon) + 'bottom_right': (min_lat, max_lon), + 'middle': (mid_lat, mid_lon) } - self.origin_lat = self.bounding_box_corners_latlon['top_left'][0] - self.origin_lon = self.bounding_box_corners_latlon['top_left'][1] + self.origin_lat = self.bounding_box_corners_latlon['middle'][0] + self.origin_lon = self.bounding_box_corners_latlon['middle'][1] self.origin_utm_y, self.origin_utm_x = _transformer_wgs84_to_utm.transform(self.origin_lat, self.origin_lon) @@ -82,28 +86,14 @@ def load_coords_from_yaml(yaml_file_path: str) -> List[Tuple[float, float]]: - latitude: float longitude: float """ - if not os.path.exists(yaml_file_path): - raise FileNotFoundError(f"YAML file not found: {yaml_file_path}") - - try: - with open(yaml_file_path, 'r') as file: - data = yaml.safe_load(file) - - if 'field_boundary' not in data or not isinstance(data['field_boundary'], list): - raise ValueError("YAML file must contain a 'field_boundary' list.") - - coords = [] - for item in data['field_boundary']: - if 'latitude' in item and 'longitude' in item: - coords.append((float(item['latitude']), float(item['longitude']))) - else: - print(f"Warning: Skipping malformed point in YAML: {item}") - return coords - except yaml.YAMLError as e: - raise ValueError(f"Error parsing YAML file: {e}") - except Exception as e: - raise ValueError(f"An unexpected error occurred while loading YAML: {e}") - + with open(yaml_file_path, 'r') as file: + data = yaml.safe_load(file) + coords = [] + for item in data['field_boundary']: + 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/tmule/launcher.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml similarity index 63% rename from auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml rename to auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml index dc4229a..a5d31b2 100644 --- a/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml @@ -9,16 +9,22 @@ init_cmd: | cd @TMULE_CONFIG_DIR@ || true set -o pipefail -windows: -# - name: data_loader -# panes: -# - ros2 run auto_shepherd_simulation_ros2 mapper.py + # Source the .bashrc + source ~/.bashrc + + # Setup ROS2 DDS Settings + export ROS_DOMAIN_ID=70 + unset ROS_LOCALHOST_ONLY -# - name: visuals -# panes: -# - rviz2 +windows: - - name: simulator + - 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 + + diff --git a/auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml b/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml similarity index 72% rename from auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml rename to auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml index cbd6020..c940c43 100644 --- a/auto_shepherd_simulation_ros2/tmule/internal.tmule.yaml +++ b/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml @@ -9,17 +9,25 @@ init_cmd: | cd @TMULE_CONFIG_DIR@ || true set -o pipefail + # Source the .bashrc + source ~/.bashrc + + # Setup ROS2 DDS Settings + export ROS_DOMAIN_ID=70 + unset ROS_LOCALHOST_ONLY + windows: - - name: data_loader - panes: - - ros2 run auto_shepherd_simulation_ros2 mapper.py - - ros2 run auto_shepherd_simulation_ros2 sim_data_loader.py - - name: visuals + - name: outputs panes: - rviz2 - - name: simulator + - name: system panes: - ros2 run auto_shepherd_simulation_ros2 dog_control_simulator.py - ros2 run auto_shepherd_simulation_ros2 dog_control.py + + - name: inputs + panes: + - ros2 run auto_shepherd_simulation_ros2 mapper.py + - ros2 run auto_shepherd_simulation_ros2 sim_data_loader.py diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index 2d0eb7a..050b7e3 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -46,8 +46,21 @@ echo "ROS 2 workspace setup and sourced. Happy robot wrangling!" # Mark that the setup has been run in this shell session export _ROS_WORKSPACE_SETUP_RUN=true +# Setup ROS2 DDS Settings +export ROS_DOMAIN_ID=70 +unset ROS_LOCALHOST_ONLY + +# Setup .tmux.conf +TMUX_CONF="$HOME/bash_scripts/tmux.conf" +[ ! -f "$HOME/.tmux.conf" ] && cp $TMUX_CONF "$HOME/.tmux.conf" + # Define custom functions to control the tmule -export TMULE_FILE=${BASE_WS}/src/auto_shepherd_simulation_ros2/tmule/launcher.tmule.yaml -function s(){ tmule -c $TMULE_FILE -W 3 launch ; } -function t(){ tmule -c $TMULE_FILE terminate ; } -function r(){ tmule -c $TMULE_FILE -W 3 relaunch ; } +alias t='tmux' + +# TMuLe for connecting with other subsystems +export CONNECTED=${BASE_WS}/src/auto_shepherd_simulation_ros2/tmule/connected.tmule.yaml +function con(){ tmule -c $CONNECTED $1 ; } + +# TMuLe for injecting data to inputs +export INJECTED=${BASE_WS}/src/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml +function inj(){ tmule -c $INJECTED $1 ; } diff --git a/bash_scripts/container/tmux.conf b/bash_scripts/container/tmux.conf new file mode 100644 index 0000000..7d426f7 --- /dev/null +++ b/bash_scripts/container/tmux.conf @@ -0,0 +1,23 @@ +# Use Ctrl-arrow keys without prefix key to switch panes +bind -n M-Left select-pane -L +bind -n M-Right select-pane -R +bind -n M-Up select-pane -U +bind -n M-Down select-pane -D + +# Shift arrow to switch windows +bind -n S-Left previous-window +bind -n S-Right next-window +bind -n S-+ previous-window +bind -n S-- next-window + +# Alt arrow to switch sessions +bind -n C-Up choose-session + +# No delay for escape key press +set -sg escape-time 0 + +# Reload tmux config +bind r source-file ~/.tmux.conf + +# Jump to bell +#set -g bell-action any diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 43477bb..129c485 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -4,16 +4,18 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Path2 + - /Group1 + - /Group1/Dog Command1 + - /MarkerArray2 Splitter Ratio: 0.5 - Tree Height: 458 + Tree Height: 529 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - - /Publish Point1 + - /2D Pose Estimate1 + - /Interact1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -30,50 +32,22 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 10 + Cell Size: 20 Class: rviz_default_plugins/Grid Color: 200; 200; 200 - Enabled: false + Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 - Offset: - X: -60 - Y: 60 - Z: 0 - Plane: XY - Plane Cell Count: 14 - Reference Frame: - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 179; 119; 97 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 1 - Name: 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: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /FieldBoundaryPath + Plane: XY + Plane Cell Count: 7 + Reference Frame: field_frame Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -90,28 +64,8 @@ Visualization Manager: Reliability Policy: Reliable Value: /simulation_markers Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/Pose - Color: 0; 25; 255 - Enabled: true - Head Length: 1 - Head Radius: 1 - Name: Pose - Shaft Length: 5 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /dog/command - Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -122,131 +76,246 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /drone_feed - Value: true - - Alpha: 1 - Axes Length: 3 - Axes Radius: 0.5 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 + Value: /sheep/labelled_img + Value: false + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Sheep Goal + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/goal + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Sheep Poses + 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 + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/poses + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Dog Initial Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Dog Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dog/pose + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 0; 25; 255 + Enabled: true + Head Length: 1 + Head Radius: 1 + Name: Dog Command + Shaft Length: 5 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dog/command + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 179; 119; 97 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 1 + Name: Field Boundary + 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: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /FieldBoundaryPath + Value: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 10 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 25; 255; 0 + Name: Group + - Class: rviz_default_plugins/MarkerArray Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: 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 + Name: MarkerArray + Namespaces: + target: true Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sheep/poses + Value: /sheep/goal_marker Value: true Enabled: true Global Options: Background Color: 48; 80; 48 - Fixed Frame: map + Fixed Frame: field_frame Frame Rate: 30 Name: root Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true + Value: goal_pose + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /clicked_point + Value: initialpose + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz_default_plugins/Orbit - Distance: 155.27615356445312 + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Focal Point: - X: -53.533905029296875 - Y: 65.6354751586914 - Z: -8.464354515075684 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -1.5697963237762451 - Target Frame: - Value: Orbit (rviz) - Yaw: 6.276760101318359 + Scale: 5.497114658355713 + Target Frame: field_frame + Value: TopDownOrtho (rviz_default_plugins) + X: -0.899625837802887 + Y: 3.823430299758911 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false + collapsed: true + Height: 957 + Hide Left Dock: true Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000022a0000035efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000205000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000246000001530000002800ffffff000000010000010f0000053dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000053d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005500000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000024cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000024c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000108000002d50000002800ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000024c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a60000003efc0100000002fb0000000800540069006d00650000000000000003a60000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003f50000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -255,6 +324,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 - Y: 27 + Width: 1013 + X: 1963 + Y: 100 From 06b89dc90e6ad151cef0c223ff9fe54741f6080e Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 15:20:16 +0100 Subject: [PATCH 40/59] improved visuals included for goal and control system --- .../dog_control.py | 6 +- .../sim_data_loader.py | 2 +- .../tmpDogSim/dog_control_lib.py | 61 +++++- configs/rviz/default.rviz | 197 +++++++----------- 4 files changed, 146 insertions(+), 120 deletions(-) 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 6527188..43da730 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 @@ -7,6 +7,7 @@ from geometry_msgs.msg import PoseStamped, Point, Quaternion 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.utils.geo_converter import load_coords_from_yaml, MapConverter @@ -20,7 +21,8 @@ def __init__(self): self.create_subscription(PoseStamped, '/dog/pose', self._dog_cb, self.get_qos()) 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) + # state caches ---------------------------------------------------- self.dog_xy = None # (x, y) self._planned_dog_xy = None # (x, y) of the last planned dog position @@ -142,7 +144,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) + xd_opt, yd_opt = find_best_dog_position(xs, ys, xd_start, yd_start, xc, yc, self.field_boundary, boundary_pub=self.marker_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/sim_data_loader.py b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py index 8259ce7..546cba4 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/sim_data_loader.py @@ -65,7 +65,7 @@ def dog_initialpose_cb(self, msg): def start_simulated_sheep(self): # Publish to the initialise random sheep poses - count = 10 + count = 40 self.get_logger().info(f"Initialising {count} sheep randomly.") self.sheep_publisher.publish(UInt16(data=count)) 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/tmpDogSim/dog_control_lib.py index 2ef82f1..fe31dc4 100644 --- a/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py +++ b/auto_shepherd_simulation_ros2/auto_shepherd_simulation_ros2/tmpDogSim/dog_control_lib.py @@ -8,6 +8,14 @@ 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 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: @@ -63,15 +71,66 @@ def cost(x, y, xd, yd, xc, yc, simulation, distance_weight = 1): #return angle(xvel, yvel, xveldesired, yveldesired) # penalise distance to closest sheep, reject if within 2m of sheep +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): + marker_array = MarkerArray() + labels = db.labels_ + unique_labels = set(labels) + + marker_id = 0 + for label in unique_labels: + if label == -1: + continue # Skip noise + + cluster_points = points[labels == label] + + if len(cluster_points) < 3: + continue # Can't form a hull + + try: + hull = ConvexHull(cluster_points[:, :2]) + delaunay = Delaunay(cluster_points[hull.vertices, :2]) + except: + continue # Skip invalid hull + + marker = Marker() + marker.header = Header(frame_id=frame_id) + marker.ns = "dbscan_hulls" + marker.id = marker_id + marker.type = Marker.TRIANGLE_LIST + marker.action = Marker.ADD + marker.scale.x = marker.scale.y = marker.scale.z = 1.0 + marker.pose.orientation.w = 1.0 + marker.color = color_for_label(label) + marker.lifetime = Duration(sec=1, nanosec=500_000_000) # 1.5 seconds + + for simplex in delaunay.simplices: + for idx in simplex: + pt = cluster_points[hull.vertices[idx]] + p = Point() + p.x, p.y = pt[0], pt[1] + p.z = pt[2] if pt.shape[0] == 3 else z + marker.points.append(p) + + marker_array.markers.append(marker) + marker_id += 1 + + 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))): + default_goto=np.asarray((0,0)), boundary_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]) db = DBSCAN(eps=10, min_samples=1).fit(points) + if boundary_pub: + render_dbscan_convex_hulls(boundary_pub, db, points) labels = db.labels_ cluster_distances = [] print(f"{len(np.unique(labels))} clusters found") diff --git a/configs/rviz/default.rviz b/configs/rviz/default.rviz index 129c485..66e4a8b 100644 --- a/configs/rviz/default.rviz +++ b/configs/rviz/default.rviz @@ -4,18 +4,16 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Group1 - - /Group1/Dog Command1 - - /MarkerArray2 + - /Grid1 + - /Sheep Data1 + - /Dog Data1 + - /Field Data1 Splitter Ratio: 0.5 - Tree Height: 529 + Tree Height: 443 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /2D Pose Estimate1 - - /Interact1 + Expanded: ~ Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -32,7 +30,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 20 + Cell Size: 7 Class: rviz_default_plugins/Grid Color: 200; 200; 200 Enabled: true @@ -46,12 +44,12 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 7 + Plane Cell Count: 20 Reference Frame: field_frame Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: MarkerArray + Name: Sheep and Dog Namespaces: dog_velocity: true sheep: true @@ -64,42 +62,8 @@ Visualization Manager: Reliability Policy: Reliable Value: /simulation_markers Value: true - - Class: rviz_default_plugins/Image - Enabled: false - 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: false - Class: rviz_common/Group Displays: - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Sheep Goal - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sheep/goal - Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -128,47 +92,28 @@ Visualization Manager: Reliability Policy: Reliable Value: /sheep/poses Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 255; 25; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true + - Class: rviz_default_plugins/MarkerArray Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Dog Initial Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow + Name: Sheep Clusters + Namespaces: + dbscan_hulls: true Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /initialpose + Value: /dbscan_hulls Value: true + Enabled: true + Name: Sheep Data + - Class: rviz_common/Group + Displays: - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/Pose Color: 255; 25; 0 - Enabled: true + Enabled: false Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Dog Pose @@ -182,13 +127,13 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dog/pose - Value: true + Value: false - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/Pose Color: 0; 25; 255 - Enabled: true + Enabled: false Head Length: 1 Head Radius: 1 Name: Dog Command @@ -202,7 +147,25 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dog/command - Value: true + Value: false + Enabled: true + Name: Dog Data + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 10 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -213,7 +176,7 @@ Visualization Manager: Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 - Name: Field Boundary + Name: Fences Offset: X: 0 Y: 0 @@ -231,34 +194,38 @@ Visualization Manager: Reliability Policy: Reliable Value: /FieldBoundaryPath Value: true - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 10 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - Enabled: true - Name: Group - - Class: rviz_default_plugins/MarkerArray + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Goal Marker + Namespaces: + target: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sheep/goal_marker + Value: true Enabled: true - Name: MarkerArray - Namespaces: - target: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sheep/goal_marker - Value: 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: false + Name: ShLAMb Input Enabled: true Global Options: Background Color: 48; 80; 48 @@ -283,15 +250,13 @@ 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 Value: true Views: Current: - Angle: 0 + Angle: -1.862645149230957e-08 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -301,21 +266,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 5.497114658355713 + Scale: 6.984107971191406 Target Frame: field_frame Value: TopDownOrtho (rviz_default_plugins) - X: -0.899625837802887 - Y: 3.823430299758911 + X: 0.5686241984367371 + Y: -0.5041577816009521 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 957 + Height: 1043 Hide Left Dock: true Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000024cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000024c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000108000002d50000002800ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000024c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a60000003efc0100000002fb0000000800540069006d00650000000000000003a60000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003f50000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000705fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000705000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000219000001890000002800ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000024c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a60000003efc0100000002fb0000000800540069006d00650000000000000003a60000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -324,6 +289,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1013 - X: 1963 - Y: 100 + Width: 1920 + X: 0 + Y: 285 From bd22f09cc19c46caa7352721c73de86a4b329212 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 15:54:22 +0100 Subject: [PATCH 41/59] workflow updated for docker execution --- .github/workflows/run_tests.yml | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 26047ad..2da7c88 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -1,4 +1,4 @@ -name: Run Tests +name: CI - ROS2 Simulation on: pull_request: @@ -8,24 +8,24 @@ on: branches: [main, develop] jobs: - ros-tests: + ros2-simulation-test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 - with: - fetch-depth: 0 + - name: Checkout Repository + uses: actions/checkout@v3 - - name: Prepare files for Docker build - run: | - mkdir -p docker_build/src - cp ros2_package/auto_shepherd_simulation/ros_interface.py docker_build/src/ros_interface.py - cp -r tests docker_build/tests - cp tests/Dockerfile.test docker_build/Dockerfile + - name: Set up Docker Compose + run: sudo apt-get update && sudo apt-get install -y docker-compose - - name: Build ROS Test Container + - name: Build and Start Simulation + working-directory: docker run: | - cd docker_build - docker build -t ros_test . + chmod +x docker_start.sh + ./docker_start.sh & + sleep 10 # give it some time to start - - name: Run ROS Tests - run: docker run ros_test + - name: Check for /dbscan_hulls Topic Message + run: | + timeout 30 docker exec -i auto_shepherd_simulation_ros2_humble \ + bash -c 'source /home/ros/base_ws/install/setup.bash && \ + ros2 topic echo /dbscan_hulls --once' From f92605ea99158dba8d8923e2bb34fcab8b17f687 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 15:59:24 +0100 Subject: [PATCH 42/59] dockerfile update --- docker/docker-compose.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index a43fd3f..0e2a4cb 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -1,5 +1,6 @@ services: auto_shepherd_simulation_ros2_humble: + container_name: auto_shepherd_simulation_ros2_humble # <- included for .git action identification build: context: . dockerfile: Dockerfile From 9d2dc32f47637c3b9f8d001b549a9f037dda58d0 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 16:04:47 +0100 Subject: [PATCH 43/59] container start dettached --- docker/docker_start.sh | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/docker/docker_start.sh b/docker/docker_start.sh index 6f88f60..0693960 100755 --- a/docker/docker_start.sh +++ b/docker/docker_start.sh @@ -29,12 +29,13 @@ echo "Docker Compose cleanup completed." # --- Run the Docker Compose service interactively --- echo "Starting Docker Compose service '${SERVICE_NAME}' in interactive mode..." -docker compose -f "${COMPOSE_FILE}" run \ - -v "$HOME/.Xauthority:/root/.Xauthority:rw" \ - -e "HOST_DISPLAY_VAR=${HOST_DISPLAY_VAR}" \ - -e QT_X11_NO_MITSHM=1 \ - "${DOCKER_RUN_ARGS[@]}" \ - "${SERVICE_NAME}" \ - bash +docker compose -f "${COMPOSE_FILE}" up -d "${SERVICE_NAME}" +#docker compose -f "${COMPOSE_FILE}" run \ +# -v "$HOME/.Xauthority:/root/.Xauthority:rw" \ +# -e "HOST_DISPLAY_VAR=${HOST_DISPLAY_VAR}" \ +# -e QT_X11_NO_MITSHM=1 \ +# "${DOCKER_RUN_ARGS[@]}" \ +# "${SERVICE_NAME}" \ +# bash echo "Docker container session ended." From aaf2e61e4a5ed4062c931d90674f875fe9cc8c09 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 10 Jul 2025 16:07:24 +0100 Subject: [PATCH 44/59] sleep delay made longer --- .github/workflows/run_tests.yml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 2da7c88..1d14773 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -22,7 +22,14 @@ jobs: run: | chmod +x docker_start.sh ./docker_start.sh & - sleep 10 # give it some time to start + for i in {1..60}; do + if docker ps --filter "name=auto_shepherd_simulation_ros2_humble" --filter "status=running" | grep auto_shepherd_simulation_ros2_humble; then + echo "Container is running." + break + fi + echo "Waiting for container to start..." + sleep 2 + done - name: Check for /dbscan_hulls Topic Message run: | From 80bbe059ffd1e7420d20797d4528a463d5b7f57a Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 10:12:50 +0100 Subject: [PATCH 45/59] workflow update --- .github/workflows/run_tests.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 1d14773..4ac4fb0 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -31,6 +31,14 @@ jobs: sleep 2 done + - name: Print Docker Container Logs if Not Running + if: ${{ failure() || !steps.build-and-start-simulation.outcome == 'success' }} + run: | + echo "Container logs:" + docker logs auto_shepherd_simulation_ros2_humble || echo "No logs found." + echo "Docker ps output:" + docker ps -a + - name: Check for /dbscan_hulls Topic Message run: | timeout 30 docker exec -i auto_shepherd_simulation_ros2_humble \ From 4794b4fab96eddd733d94da12593b01048003aab Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 10:43:43 +0100 Subject: [PATCH 46/59] workflow update --- .github/workflows/run_tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 4ac4fb0..7630140 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -21,7 +21,7 @@ jobs: working-directory: docker run: | chmod +x docker_start.sh - ./docker_start.sh & + ./docker_start.sh | tee docker_start_output.log & for i in {1..60}; do if docker ps --filter "name=auto_shepherd_simulation_ros2_humble" --filter "status=running" | grep auto_shepherd_simulation_ros2_humble; then echo "Container is running." From 254b9644c138261e595d195c1b2a6e7884a7e7bc Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 11:47:44 +0100 Subject: [PATCH 47/59] workflow tests added --- .github/workflows/run_tests.yml | 21 +++++++++++++++++++++ docker/docker_start.sh | 6 ++++++ 2 files changed, 27 insertions(+) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 7630140..99d4f95 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -17,9 +17,15 @@ jobs: - name: Set up Docker Compose run: sudo apt-get update && sudo apt-get install -y docker-compose + - name: Cleanup Docker + run: | + docker container prune -f + docker volume prune -f + - name: Build and Start Simulation working-directory: docker run: | + set -e chmod +x docker_start.sh ./docker_start.sh | tee docker_start_output.log & for i in {1..60}; do @@ -31,6 +37,12 @@ jobs: sleep 2 done + - name: Show Docker Compose Status and Logs + working-directory: docker + run: | + docker compose ps + docker compose logs --tail=50 auto_shepherd_simulation_ros2_humble + - name: Print Docker Container Logs if Not Running if: ${{ failure() || !steps.build-and-start-simulation.outcome == 'success' }} run: | @@ -39,6 +51,15 @@ jobs: echo "Docker ps output:" docker ps -a + - name: Smoke Test Container + run: | + docker exec auto_shepherd_simulation_ros2_humble ros2 --help + + - name: Print ROS2 Nodes and Topics + run: | + docker exec auto_shepherd_simulation_ros2_humble bash -c \ + "source /home/ros/base_ws/install/setup.bash && ros2 node list && ros2 topic list" + - name: Check for /dbscan_hulls Topic Message run: | timeout 30 docker exec -i auto_shepherd_simulation_ros2_humble \ diff --git a/docker/docker_start.sh b/docker/docker_start.sh index 0693960..e3c1fc7 100755 --- a/docker/docker_start.sh +++ b/docker/docker_start.sh @@ -38,4 +38,10 @@ docker compose -f "${COMPOSE_FILE}" up -d "${SERVICE_NAME}" # "${SERVICE_NAME}" \ # bash +# Check if the container started successfully +docker ps --filter "name=${SERVICE_NAME}" --filter "status=running" | grep ${SERVICE_NAME} > /dev/null +if [ $? -ne 0 ]; then + echo "Container failed to start!" + exit 1 +fi echo "Docker container session ended." From 1ee88d2cec585c2cb97676b72e8de4a8c448739f Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 11:54:21 +0100 Subject: [PATCH 48/59] workflow tests added --- .github/workflows/run_tests.yml | 7 +++++++ docker/docker_start.sh | 5 +++++ 2 files changed, 12 insertions(+) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 99d4f95..51a098e 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -37,6 +37,13 @@ jobs: sleep 2 done + if ! docker ps --filter "name=auto_shepherd_simulation_ros2_humble" --filter "status=running" | grep auto_shepherd_simulation_ros2_humble; then + echo "ERROR: Container auto_shepherd_simulation_ros2_humble failed to start." + docker ps -a + docker logs auto_shepherd_simulation_ros2_humble || echo "No logs found." + exit 1 + fi + - name: Show Docker Compose Status and Logs working-directory: docker run: | diff --git a/docker/docker_start.sh b/docker/docker_start.sh index e3c1fc7..60f1683 100755 --- a/docker/docker_start.sh +++ b/docker/docker_start.sh @@ -42,6 +42,11 @@ docker compose -f "${COMPOSE_FILE}" up -d "${SERVICE_NAME}" docker ps --filter "name=${SERVICE_NAME}" --filter "status=running" | grep ${SERVICE_NAME} > /dev/null if [ $? -ne 0 ]; then echo "Container failed to start!" + docker ps -a + docker logs auto_shepherd_simulation_ros2_humble || echo "No logs found for container." exit 1 fi +echo "Container started successfully." +docker ps -a +docker logs auto_shepherd_simulation_ros2_humble || echo "No logs found for container." echo "Docker container session ended." From a4f815fda0c73794dfa452250c0e743877022fb8 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 12:10:01 +0100 Subject: [PATCH 49/59] workflow tests added --- .github/workflows/run_tests.yml | 17 +---------------- docker/docker_start.sh | 3 ++- 2 files changed, 3 insertions(+), 17 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 51a098e..80ce324 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -27,22 +27,7 @@ jobs: run: | set -e chmod +x docker_start.sh - ./docker_start.sh | tee docker_start_output.log & - for i in {1..60}; do - if docker ps --filter "name=auto_shepherd_simulation_ros2_humble" --filter "status=running" | grep auto_shepherd_simulation_ros2_humble; then - echo "Container is running." - break - fi - echo "Waiting for container to start..." - sleep 2 - done - - if ! docker ps --filter "name=auto_shepherd_simulation_ros2_humble" --filter "status=running" | grep auto_shepherd_simulation_ros2_humble; then - echo "ERROR: Container auto_shepherd_simulation_ros2_humble failed to start." - docker ps -a - docker logs auto_shepherd_simulation_ros2_humble || echo "No logs found." - exit 1 - fi + ./docker_start.sh | tee docker_start_output.log - name: Show Docker Compose Status and Logs working-directory: docker diff --git a/docker/docker_start.sh b/docker/docker_start.sh index 60f1683..f94a858 100755 --- a/docker/docker_start.sh +++ b/docker/docker_start.sh @@ -4,7 +4,8 @@ COMPOSE_FILE="docker-compose.yml" SERVICE_NAME="auto_shepherd_simulation_ros2_humble" -xhost +local:docker +command -v xhost >/dev/null && xhost +local:docker +#xhost +local:docker if [[ "$OSTYPE" == "linux-gnu"* ]]; then HOST_DISPLAY_VAR="$DISPLAY" From 45f618eb2eb3211a334df0faffe71828cb24494e Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 12:19:50 +0100 Subject: [PATCH 50/59] workflow tests added --- .github/workflows/run_tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 80ce324..e526775 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -45,7 +45,7 @@ jobs: - name: Smoke Test Container run: | - docker exec auto_shepherd_simulation_ros2_humble ros2 --help + docker exec auto_shepherd_simulation_ros2_humble bash -c "source /opt/ros/humble/setup.bash && ros2 --help" - name: Print ROS2 Nodes and Topics run: | From 4a522be2560cf6e7284728c62f9665109acf4011 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 12:21:21 +0100 Subject: [PATCH 51/59] workflow tests added --- docker/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index 745c83e..676fc9c 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -116,6 +116,7 @@ RUN apt-get update && \ # 5. Python pip installs RUN python3 -m pip install --no-cache-dir --upgrade \ + --ignore-installed \ pip wheel \ setuptools==58.2.0 \ packaging==24.2 \ From 7a9fe9706c19b6fd2172bd4723f88f761d9c1a53 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 12:35:16 +0100 Subject: [PATCH 52/59] workflow tests added --- .github/workflows/run_tests.yml | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index e526775..404ffe5 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -47,10 +47,20 @@ jobs: run: | docker exec auto_shepherd_simulation_ros2_humble bash -c "source /opt/ros/humble/setup.bash && ros2 --help" - - name: Print ROS2 Nodes and Topics + + - name: Build ROS2 Workspace + run: | + docker exec auto_shepherd_simulation_ros2_humble bash -c \ + "source /opt/ros/humble/setup.bash && \ + cd /home/ros/base_ws && \ + colcon build --symlink-install" + + - name: List ROS2 Topics run: | docker exec auto_shepherd_simulation_ros2_humble bash -c \ - "source /home/ros/base_ws/install/setup.bash && ros2 node list && ros2 topic list" + "source /opt/ros/humble/setup.bash && \ + source /home/ros/base_ws/install/setup.bash && \ + ros2 topic list" - name: Check for /dbscan_hulls Topic Message run: | From 5b800e01a18dc11fd938c1659d4f526adfb2d23c Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:05:41 +0100 Subject: [PATCH 53/59] workflow tests added --- .github/workflows/run_tests.yml | 14 +++- bash_scripts/container/bashrc_extensions.sh | 89 +++++++++++++++++++++ 2 files changed, 99 insertions(+), 4 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 404ffe5..0fdad38 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -62,8 +62,14 @@ jobs: source /home/ros/base_ws/install/setup.bash && \ ros2 topic list" - - name: Check for /dbscan_hulls Topic Message + - name: Initialise and Check for /dbscan_hulls run: | - timeout 30 docker exec -i auto_shepherd_simulation_ros2_humble \ - bash -c 'source /home/ros/base_ws/install/setup.bash && \ - ros2 topic echo /dbscan_hulls --once' + docker exec auto_shepherd_simulation_ros2_humble bash -c " + set -e + source /opt/ros/humble/setup.bash + source /home/ros/bash_scripts/bashrc_extensions.sh + github_action_initialise_tmule + github_action_initialise_dog + github_action_initialise_goal + timeout 30s ros2 topic echo /dbscan_hulls --once + " diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index 050b7e3..6771238 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -64,3 +64,92 @@ function con(){ tmule -c $CONNECTED $1 ; } # TMuLe for injecting data to inputs export INJECTED=${BASE_WS}/src/auto_shepherd_simulation_ros2/tmule/injected.tmule.yaml function inj(){ tmule -c $INJECTED $1 ; } + + +################################ +## Aliases for github actions ## +################################ + +function github_action_initialise_tmule() { tmule -c $INJECTED launch ; } + +function github_action_initialise_goal() +{ +ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "header: + stamp: + sec: 1752234451 + nanosec: 558437227 + frame_id: field_frame +pose: + position: + x: -15.754144668579102 + y: 44.95618438720703 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0721578213596985 + w: 0.9973932267749877 +" ; +} + +function github_action_initialise_dog() +{ +ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "header: + stamp: + sec: 1752234463 + nanosec: 6465847 + frame_id: field_frame +pose: + pose: + position: + x: -19.47688865661621 + y: 8.73109245300293 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + covariance: + - 0.25 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.25 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.06853891909122467 +" ; +} + + + + From edcdfe1ff445995aa92476c5d753401fe081e45f Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:09:37 +0100 Subject: [PATCH 54/59] workflow tests added --- bash_scripts/container/bashrc_extensions.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bash_scripts/container/bashrc_extensions.sh b/bash_scripts/container/bashrc_extensions.sh index 6771238..b53aed7 100644 --- a/bash_scripts/container/bashrc_extensions.sh +++ b/bash_scripts/container/bashrc_extensions.sh @@ -89,7 +89,7 @@ pose: y: 0.0 z: -0.0721578213596985 w: 0.9973932267749877 -" ; +" --once ; } function github_action_initialise_dog() @@ -147,7 +147,7 @@ pose: - 0.0 - 0.0 - 0.06853891909122467 -" ; +" --once ; } From 1ca26be59a1410067efc82bac675fce2f3fb87e3 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:21:30 +0100 Subject: [PATCH 55/59] workflow tests added --- .github/workflows/run_tests.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 0fdad38..cffc165 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -71,5 +71,8 @@ jobs: github_action_initialise_tmule github_action_initialise_dog github_action_initialise_goal - timeout 30s ros2 topic echo /dbscan_hulls --once + echo 'Waiting for /dbscan_hulls to appear...' + ros2 topic list + ros2 topic info /dbscan_hulls || echo 'No info for /dbscan_hulls yet' + timeout 60s ros2 topic echo /dbscan_hulls --once " From 6d84e11aba00809b96a19142a13636fcf37b1e9f Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:29:22 +0100 Subject: [PATCH 56/59] workflow tests added --- .github/workflows/run_tests.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index cffc165..c1d5913 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -62,7 +62,7 @@ jobs: source /home/ros/base_ws/install/setup.bash && \ ros2 topic list" - - name: Initialise and Check for /dbscan_hulls + - name: Wait for and Echo /dbscan_hulls run: | docker exec auto_shepherd_simulation_ros2_humble bash -c " set -e @@ -71,8 +71,9 @@ jobs: github_action_initialise_tmule github_action_initialise_dog github_action_initialise_goal - echo 'Waiting for /dbscan_hulls to appear...' + echo 'Waiting for /dbscan_hulls' ros2 topic list ros2 topic info /dbscan_hulls || echo 'No info for /dbscan_hulls yet' - timeout 60s ros2 topic echo /dbscan_hulls --once + echo 'Waiting for /dbscan_hulls to publish...' + timeout 60s ros2 topic echo /dbscan_hulls visualization_msgs/msg/MarkerArray --once " From 872d54641bfe9875b94d8b5702527328012e112b Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:44:07 +0100 Subject: [PATCH 57/59] workflow tests added --- .github/workflows/run_tests.yml | 32 +++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index c1d5913..e2da96b 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -68,12 +68,30 @@ jobs: set -e source /opt/ros/humble/setup.bash source /home/ros/bash_scripts/bashrc_extensions.sh + github_action_initialise_tmule - github_action_initialise_dog - github_action_initialise_goal - echo 'Waiting for /dbscan_hulls' - ros2 topic list - ros2 topic info /dbscan_hulls || echo 'No info for /dbscan_hulls yet' - echo 'Waiting for /dbscan_hulls to publish...' - timeout 60s ros2 topic echo /dbscan_hulls visualization_msgs/msg/MarkerArray --once + + for attempt in {1..3}; do + echo \"\\n🌀 Attempt \$attempt to trigger and read /dbscan_hulls\" + + github_action_initialise_dog + github_action_initialise_goal + + echo \"🔍 Checking topic list and /dbscan_hulls info\" + ros2 topic list + ros2 topic info /dbscan_hulls -v || echo 'No info for /dbscan_hulls yet' + + echo \"⏳ Waiting up to 60s for a MarkerArray message on /dbscan_hulls...\" + if timeout 60s ros2 topic echo /dbscan_hulls visualization_msgs/msg/MarkerArray --once; then + echo \"✅ Successfully received /dbscan_hulls on attempt \$attempt\" + exit 0 + else + echo \"⚠️ No message received on /dbscan_hulls (attempt \$attempt)\" + fi + + sleep 5 + done + + echo \"❌ Failed to receive /dbscan_hulls after 3 attempts\" + exit 1 " From 9b71c548fda13c464921ab4dd9c967bc02a935cd Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 11 Jul 2025 13:49:27 +0100 Subject: [PATCH 58/59] workflow simplified output --- .github/workflows/run_tests.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index e2da96b..5344199 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -72,21 +72,21 @@ jobs: github_action_initialise_tmule for attempt in {1..3}; do - echo \"\\n🌀 Attempt \$attempt to trigger and read /dbscan_hulls\" + echo \"Attempt \$attempt to trigger and read /dbscan_hulls\" github_action_initialise_dog github_action_initialise_goal - echo \"🔍 Checking topic list and /dbscan_hulls info\" + echo \"Checking topic list and /dbscan_hulls info\" ros2 topic list ros2 topic info /dbscan_hulls -v || echo 'No info for /dbscan_hulls yet' - echo \"⏳ Waiting up to 60s for a MarkerArray message on /dbscan_hulls...\" - if timeout 60s ros2 topic echo /dbscan_hulls visualization_msgs/msg/MarkerArray --once; then - echo \"✅ Successfully received /dbscan_hulls on attempt \$attempt\" + echo \"Waiting up to 60s for a MarkerArray message on /dbscan_hulls...\" + if timeout 60s ros2 topic echo /dbscan_hulls visualization_msgs/msg/MarkerArray --no-arr --once; then + echo \"Successfully received /dbscan_hulls on attempt \$attempt\" exit 0 else - echo \"⚠️ No message received on /dbscan_hulls (attempt \$attempt)\" + echo \"No message received on /dbscan_hulls (attempt \$attempt)\" fi sleep 5 From 8e19207001311167d862b855c1ede54d07cb39c3 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 17 Jul 2025 14:34:45 +0100 Subject: [PATCH 59/59] 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