From 49ce4d486df42a268004ee959c29ebfcbb60c61a Mon Sep 17 00:00:00 2001 From: Baptiste Date: Wed, 13 May 2026 16:57:04 +0200 Subject: [PATCH 1/3] for next test --- .../evolo_move_path/config/evolo_params.yaml | 39 +- .../evolo_move_path/evolo_move_path/client.py | 482 ++++++++---- .../evolo_move_path_server_dubins_curves.py | 714 ++++++++---------- behaviours/evolo/evolo_move_path/setup.py | 15 +- 4 files changed, 685 insertions(+), 565 deletions(-) diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml index 74380a40c..fb9a04da8 100644 --- a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -1,37 +1,28 @@ /**/evolo_move_path_action_server: ros__parameters: frame_id: "evolo/odom" - timeout: 1200.0 + timeout: 1200.0 - # Speed and limits + # Speed v_min: 8.0 v_max: 14.0 omega_max: 16.0 # Pure Pursuit - ld_base: 5.0 # Change here - ld_gain: 0.5 - ff_gain: 0.1 + ld_base: 5.0 + ld_gain: 0.2 - # Dubins - min_turning_radius: 30.0 # Change here + # Dubins planner + min_turning_radius: 30.0 dubins_step: 0.5 + dubins_mode: "vwp" # "vwp" or "smooth" - # Buffer - hard_buffer: 20.0 # Change here - soft_buffer: 30.0 # Change here + # Cross-track error + cte_kp: 3.0 + cte_ki: 0.05 + cte_integral_max: 10.0 + heading_kp: 8.0 - # Speed Map - speed_map_slow: 8.0 - speed_map_medium: 11.0 - speed_map_high: 14.0 - - - # Graph - graph_type: "voronoi" # 'visibility' ou 'voronoi' - - - - # Lateral_normal - kp_lateral: 0.3 - kp_heading: 0.8 + hard_buffer: 10.0 + soft_buffer: 30.0 + geofence_timeout: 5.0 \ No newline at end of file diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py index 5c2e4c722..c6c593144 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py @@ -1,20 +1,25 @@ import rclpy import math +import json + from rclpy.node import Node from rclpy.action import ActionClient from rclpy.time import Duration, Time from smarc_msgs.action import BaseAction -import json from nav_msgs.msg import Path, Odometry from visualization_msgs.msg import Marker, MarkerArray from geometry_msgs.msg import Point, PoseStamped, Quaternion from geographic_msgs.msg import GeoPoint -from std_msgs.msg import String from tf2_ros import Buffer, TransformListener -from tf2_geometry_msgs import do_transform_pose_stamped +from tf2_geometry_msgs import do_transform_pose_stamped, do_transform_point +from geometry_msgs.msg import PointStamped from smarc_utilities import georef_utils import tf_transformations -import numpy as np + +from smarc_msgs.msg import GeofencePolygonsStamped +from sensor_msgs.msg import NavSatFix +from smarc_msgs.msg import Topics as SmarcTopics +from rclpy.action import ActionClient as RosActionClient class EvoloMovePathClient(Node): @@ -23,31 +28,47 @@ def __init__(self): super().__init__('evolo_move_path_client') self._action_client = ActionClient(self, BaseAction, 'move_path') - # TF buffer pour latlon_to_local_frame ← AJOUT self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self, spin_thread=True) - # Publishers de visualisation + self.frame_id = 'evolo/odom' + self.target_list = [] + + # ── Publishers ──────────────────────────────────────────────────────── self.marker_pub = self.create_publisher(MarkerArray, 'waypoints_viz', 10) self.path_pub = self.create_publisher(Path, 'visual_path', 10) self.viz_pub = self.create_publisher(MarkerArray, 'visualisation', 10) self.dubins_path_pub = self.create_publisher(Path, 'dubins_path', 10) self.attractor_pub = self.create_publisher(Marker, 'attractor_marker', 10) - self.boundary_pub = self.create_publisher(MarkerArray, 'path_boundaries', 10) - - - # Subscription odométrie - self.odom_sub = self.create_subscription(Odometry, 'evolo/smarc/odom', self._odom_callback, 10) - - self.poses_history = [] - self.frame_id = 'evolo/odom' - self.target_list = [] + self.geofence_inside_pub = self.create_publisher(MarkerArray, 'rviz/geofence_inside', 10) + self.geofence_outside_pub = self.create_publisher(MarkerArray, 'rviz/geofence_outside', 10) + self.geopoint_pub = self.create_publisher(GeoPoint, SmarcTopics.POS_LATLON_TOPIC, 10) + + # ── Island buffer publishers ────────────────────────────────────────── + # Soft buffer (white) — traversable, used as Dijkstra node source + self.island_soft_pub = self.create_publisher( + MarkerArray, 'rviz/island_buffer_soft', 10) + # Hard buffer (orange) — absolute exclusion zone + self.island_hard_pub = self.create_publisher( + MarkerArray, 'rviz/island_buffer_hard', 10) + + # ── Subscribers ─────────────────────────────────────────────────────── + self.gps_sub = self.create_subscription( + NavSatFix, '/evolo/Lidar/gps', self._gps_callback, 10) + self.polygons_sub = self.create_subscription( + GeofencePolygonsStamped, '/smarc/geofence_polygons', + self._geofence_polygons_callback, 10) + self.odom_sub = self.create_subscription( + Odometry, 'evolo/smarc/odom', self._odom_callback, 10) + + self._geofence_start_client = RosActionClient( + self, BaseAction, 'smarc_start_geofence') self.robot_path_msg = Path() self.robot_path_msg.header.frame_id = self.frame_id # ───────────────────────────────────────────────────────────────────────── - # Conversion lat/lon → repère local (identique au serveur) + # TF helper # ───────────────────────────────────────────────────────────────────────── def latlon_to_local_frame(self, point_list): geopoint = GeoPoint() @@ -70,20 +91,18 @@ def latlon_to_local_frame(self, point_list): ) return do_transform_pose_stamped(ps, t) except Exception as e: - self.get_logger().error(f"TF failed: {e}") + self.get_logger().error(f'TF failed: {e}') return None # ───────────────────────────────────────────────────────────────────────── - # Odométrie → historique robot + # Odometry → robot trail # ───────────────────────────────────────────────────────────────────────── def _odom_callback(self, msg: Odometry): ps = PoseStamped() - ps.header.frame_id = self.frame_id # ← forcer le même frame que le Path + ps.header.frame_id = self.frame_id ps.header.stamp = msg.header.stamp ps.pose = msg.pose.pose - # Si l'odom est déjà dans evolo/odom, c'est bon directement. - # Sinon, transformer : if msg.header.frame_id != self.frame_id: try: t = self._tf_buffer.lookup_transform( @@ -97,126 +116,194 @@ def _odom_callback(self, msg: Odometry): raw.pose = msg.pose.pose ps = do_transform_pose_stamped(raw, t) except Exception as e: - self.get_logger().error(f"Odom TF failed: {e}") + self.get_logger().error(f'Odom TF failed: {e}') return self.robot_path_msg.poses.append(ps) self.robot_path_msg.header.stamp = self.get_clock().now().to_msg() self.path_pub.publish(self.robot_path_msg) - - """ # ───────────────────────────────────────────────────────────────────────── + # Geofence polygons → RViz (raw island and safe-zone outlines) + # ───────────────────────────────────────────────────────────────────────── + def _geofence_polygons_callback(self, msg: GeofencePolygonsStamped): + stamp = msg.header.stamp + frame_id = self.frame_id - def publish_boundaries(self): - ma = MarkerArray() + try: + t = self._tf_buffer.lookup_transform( + self.frame_id, msg.header.frame_id, + Time(seconds=0), timeout=Duration(seconds=1)) + except Exception as e: + self.get_logger().error(f'TF geofence failed: {e}') + return - left_bound = [ - [58.839967, 17.653462], [58.839967, 17.656493], [58.840957, 17.656493], - [58.840957, 17.660781], [58.839967, 17.660781], [58.839967, 17.663812] - ] - - right_bound = [ - [58.839517, 17.653462], [58.839517, 17.657331], [58.840507, 17.657331], - [58.840507, 17.659943], [58.839517, 17.659943], [58.839517, 17.663812] - ] - - for i, bound in enumerate([left_bound, right_bound]): - marker = Marker() - marker.header.frame_id = self.frame_id - marker.header.stamp = self.get_clock().now().to_msg() - marker.ns = "boundaries" - marker.id = i - marker.type = Marker.LINE_STRIP - marker.action = Marker.ADD - - marker.scale.x = 0.7 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - - for wp in bound: - ps = self.latlon_to_local_frame([wp[0], wp[1]]) - if ps: - p = Point() - p.x = ps.pose.position.x - p.y = ps.pose.position.y - p.z = 0.0 - marker.points.append(p) - - ma.markers.append(marker) - - self.boundary_pub.publish(ma) - """ + def _poly_to_marker(polygon, marker_id, r, g, b) -> Marker: + m = Marker() + m.header.stamp = stamp + m.header.frame_id = frame_id + m.ns = 'geofence' + m.id = marker_id + m.type = Marker.LINE_STRIP + m.action = Marker.ADD + m.scale.x = 2.0 + m.color.r, m.color.g, m.color.b = r, g, b + m.color.a = 0.9 + for pt in polygon.points: + ps_in = PointStamped() + ps_in.header = msg.header + ps_in.point = Point(x=pt.x, y=pt.y, z=pt.z) + m.points.append(do_transform_point(ps_in, t).point) + if polygon.points: + ps_in = PointStamped() + ps_in.header = msg.header + ps_in.point = Point(x=polygon.points[0].x, + y=polygon.points[0].y, + z=polygon.points[0].z) + m.points.append(do_transform_point(ps_in, t).point) + return m + + inside_array = MarkerArray() + for i, poly in enumerate(msg.geofence): + inside_array.markers.append(_poly_to_marker(poly, i, 0.0, 1.0, 0.0)) + self.geofence_inside_pub.publish(inside_array) + + outside_array = MarkerArray() + for i, poly in enumerate(msg.islands): + outside_array.markers.append(_poly_to_marker(poly, i, 1.0, 0.0, 0.0)) + self.geofence_outside_pub.publish(outside_array) + + self.get_logger().info( + f'Geofence: {len(msg.geofence)} inside (green), ' + f'{len(msg.islands)} outside (red)' + ) # ───────────────────────────────────────────────────────────────────────── - def publish_waypoints(self, waypoint_list): - marker_array = MarkerArray() - for i, pt in enumerate(waypoint_list): - marker = Marker() - marker.header.frame_id = self.frame_id - marker.type = Marker.SPHERE - marker.action = Marker.ADD - marker.id = i - marker.pose.position.x = pt[0] - marker.pose.position.y = pt[1] - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker_array.markers.append(marker) - self.marker_pub.publish(marker_array) - - def _publish_attractor(self, ax, ay): - m = Marker() - m.header.frame_id = self.frame_id - m.header.stamp = self.get_clock().now().to_msg() - m.ns = "attractor"; m.id = 0 - m.type = Marker.SPHERE; m.action = Marker.ADD - m.pose.position.x = ax - m.pose.position.y = ay - m.pose.position.z = 1.0 - m.scale.x = 3.0; m.scale.y = 3.0; m.scale.z = 3.0 - m.color.r = 1.0; m.color.g = 1.0; m.color.b = 0.0; m.color.a = 0.9 - self.attractor_pub.publish(m) + # GPS → GeoPoint relay + # ───────────────────────────────────────────────────────────────────────── + def _gps_callback(self, msg: NavSatFix): + gp = GeoPoint() + gp.latitude = msg.latitude + gp.longitude = msg.longitude + gp.altitude = msg.altitude + self.geopoint_pub.publish(gp) # ───────────────────────────────────────────────────────────────────────── + # Goal dispatch + # ───────────────────────────────────────────────────────────────────────── + def _send_polygons_to_geofence(self, polygons: list): + if not self._geofence_start_client.wait_for_server(timeout_sec=5.0): + self.get_logger().warn('smarc_start_geofence not available, skipping') + return + for poly_def in polygons: + goal_msg = BaseAction.Goal() + payload = { + 'stay_inside': poly_def['stay_inside'], + 'ceiling_altitude': 1000.0, + 'floor_altitude': 0.0, + 'waypoints': [ + {'latitude': pt['lat'], 'longitude': pt['lon'], 'altitude': 0.0} + for pt in poly_def['points'] + ], + } + goal_msg.goal.data = json.dumps(payload) + self.get_logger().info( + f"Sending polygon '{poly_def['name']}' to geofence " + f"(stay_inside={poly_def['stay_inside']})" + ) + self._geofence_start_client.send_goal_async(goal_msg) + def send_goal(self): - self.get_logger().info("Wait for Action Server...") + self.get_logger().info('Wait for Action Server…') if not self._action_client.wait_for_server(timeout_sec=10.0): - self.get_logger().error("No server") + self.get_logger().error('No server') return - while not self._tf_buffer.can_transform(self.frame_id, 'utm', Time(seconds=0)): - self.get_logger().info("Waiting for TF...") + while not self._tf_buffer.can_transform( + self.frame_id, 'utm', Time(seconds=0)): + self.get_logger().info('Waiting for TF…') rclpy.spin_once(self, timeout_sec=0.5) - - # self.publish_boundaries() - - goal_msg = BaseAction.Goal() + goal_msg = BaseAction.Goal() payload = { 'speed': 'high', - 'waypoints': [ - {'latitude': 58.8397422670, 'longitude': 17.6534623045, 'tolerance': 3.0}, + {'latitude': 58.8389422670, 'longitude': 17.6534623045, 'tolerance': 3.0}, {'latitude': 58.8400922670, 'longitude': 17.6540122932, 'tolerance': 3.0}, {'latitude': 58.8403922670, 'longitude': 17.6533123075, 'tolerance': 3.0}, - {'latitude': 58.8398922670, 'longitude': 17.6528123177, 'tolerance': 3.0}, + {'latitude': 58.8398922670, 'longitude': 17.6518123177, 'tolerance': 3.0}, {'latitude': 58.8397922670, 'longitude': 17.6543122871, 'tolerance': 3.0}, - ] - - + ], + 'polygons': [ + { + 'name': 'Big Area', + 'stay_inside': True, + 'points': [ + {'lat': 58.8380, 'lon': 17.6500}, + {'lat': 58.8420, 'lon': 17.6500}, + {'lat': 58.8420, 'lon': 17.6570}, + {'lat': 58.8380, 'lon': 17.6570}, + ], + }, + { + 'name': 'Island 1', + 'stay_inside': False, + 'points': [ + {'lat': 58.8395, 'lon': 17.6530}, + {'lat': 58.8400, 'lon': 17.6530}, + {'lat': 58.8400, 'lon': 17.6532}, + {'lat': 58.8395, 'lon': 17.6535}, + ], + }, + { + 'name': 'Island 2', + 'stay_inside': False, + 'points': [ + {'lat': 58.8405, 'lon': 17.6540}, + {'lat': 58.8407, 'lon': 17.6540}, + {'lat': 58.8407, 'lon': 17.6542}, + {'lat': 58.8405, 'lon': 17.6542}, + ], + }, + { + 'name': 'Island 3', + 'stay_inside': False, + 'points': [ + {'lat': 58.8410, 'lon': 17.6510}, + {'lat': 58.8415, 'lon': 17.6510}, + {'lat': 58.8415, 'lon': 17.6515}, + {'lat': 58.8410, 'lon': 17.6515}, + ], + }, + { + 'name': 'Island 4', + 'stay_inside': False, + 'points': [ + {'lat': 58.8385, 'lon': 17.6550}, + {'lat': 58.8390, 'lon': 17.6550}, + {'lat': 58.8390, 'lon': 17.6555}, + {'lat': 58.8385, 'lon': 17.6555}, + ], + }, + { + 'name': 'Island 5', + 'stay_inside': False, + 'points': [ + {'lat': 58.8412, 'lon': 17.6560}, + {'lat': 58.8416, 'lon': 17.6560}, + {'lat': 58.8416, 'lon': 17.6565}, + {'lat': 58.8412, 'lon': 17.6565}, + ], + }, + ], } goal_msg.goal.data = json.dumps(payload) + if 'polygons' in payload: + self._send_polygons_to_geofence(payload['polygons']) - self.get_logger().info("Send mission...") + self.get_logger().info('Send mission…') self._send_goal_future = self._action_client.send_goal_async( - goal_msg, - feedback_callback=self.feedback_callback, - ) + goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): @@ -228,6 +315,13 @@ def goal_response_callback(self, future): self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) + def get_result_callback(self, future): + future.result().result + self.get_logger().info('Final Result') + rclpy.shutdown() + + # ───────────────────────────────────────────────────────────────────────── + # Feedback — dispatch all server data to RViz # ───────────────────────────────────────────────────────────────────────── def feedback_callback(self, feedback_msg): try: @@ -241,23 +335,159 @@ def feedback_callback(self, feedback_msg): self._publish_attractor(data['ax'], data['ay']) if 'full_path' in data: - path_msg = self._convert_list_to_path(data['full_path']) - self.dubins_path_pub.publish(path_msg) + self.dubins_path_pub.publish( + self._convert_list_to_path(data['full_path'])) + + # ── Island buffers received once after planning ──────────────────── + if 'visibility_graph' in data: + vg = data['visibility_graph'] + if 'island_contours_soft' in vg: + self._publish_buffer_contours( + vg['island_contours_soft'], + publisher = self.island_soft_pub, + ns = 'island_soft', + r=1.0, g=1.0, b=1.0, # white + line_width = 0.25, + dot_scale = 0.8, + ) + if 'island_contours_hard' in vg: + self._publish_buffer_contours( + vg['island_contours_hard'], + publisher = self.island_hard_pub, + ns = 'island_hard', + r=1.0, g=0.5, b=0.0, # orange + line_width = 0.35, + dot_scale = 0.6, + ) except Exception as e: - self.get_logger().error(f"Erreur feedback: {e}") + self.get_logger().error(f'Feedback error: {e}') + + # ───────────────────────────────────────────────────────────────────────── + # Island buffer visualisation + # ───────────────────────────────────────────────────────────────────────── + def _publish_buffer_contours( + self, + contours: list, + publisher, + ns: str, + r: float, g: float, b: float, + line_width: float, + dot_scale: float, + ): + """ + Publishes a MarkerArray for a list of (x, y) contours. + + For each contour two markers are emitted: + • LINE_STRIP — closed polygon outline + • SPHERE_LIST — one sphere per vertex (the "waypoints" of the buffer) + + Parameters + ---------- + contours : list[list[(x, y)]] received from server JSON + publisher : ROS publisher (MarkerArray) + ns : marker namespace prefix + r, g, b : RGB colour [0-1] + line_width : LINE_STRIP scale.x in metres + dot_scale : SPHERE_LIST sphere diameter in metres + """ + if not contours: + return + + ma = MarkerArray() + stamp = self.get_clock().now().to_msg() + mid = 0 + + for contour in contours: + if len(contour) < 2: + continue + + pts = [Point(x=float(c[0]), y=float(c[1]), z=0.0) for c in contour] + + # ── Closed polygon outline ──────────────────────────────────────── + line = Marker() + line.header.frame_id = self.frame_id + line.header.stamp = stamp + line.ns, line.id = f'{ns}_line', mid; mid += 1 + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.scale.x = line_width + line.color.r = r + line.color.g = g + line.color.b = b + line.color.a = 0.85 + line.points = pts + [pts[0]] # close the loop + ma.markers.append(line) + + # ── Vertex spheres ("waypoints" of the buffer contour) ──────────── + dots = Marker() + dots.header.frame_id = self.frame_id + dots.header.stamp = stamp + dots.ns, dots.id = f'{ns}_dots', mid; mid += 1 + dots.type = Marker.SPHERE_LIST + dots.action = Marker.ADD + dots.scale.x = dots.scale.y = dots.scale.z = dot_scale + dots.color.r = r + dots.color.g = g + dots.color.b = b + dots.color.a = 1.0 + dots.points = pts + ma.markers.append(dots) + + # ── Vertex index labels ─────────────────────────────────────────── + for k, pt in enumerate(pts): + label = Marker() + label.header.frame_id = self.frame_id + label.header.stamp = stamp + label.ns, label.id = f'{ns}_labels', mid; mid += 1 + label.type = Marker.TEXT_VIEW_FACING + label.action = Marker.ADD + label.pose.position.x = pt.x + label.pose.position.y = pt.y + label.pose.position.z = 1.5 + label.scale.z = 0.8 + label.color.r = r + label.color.g = g + label.color.b = b + label.color.a = 1.0 + label.text = str(k) + ma.markers.append(label) + + publisher.publish(ma) + self.get_logger().info( + f'[{ns}] {len(contours)} contour(s) | ' + f'{sum(len(c) for c in contours)} vertex markers published' + ) + # ───────────────────────────────────────────────────────────────────────── + # Attractor marker + # ───────────────────────────────────────────────────────────────────────── + def _publish_attractor(self, ax, ay): + m = Marker() + m.header.frame_id = self.frame_id + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'attractor'; m.id = 0 + m.type = Marker.SPHERE; m.action = Marker.ADD + m.pose.position.x = ax + m.pose.position.y = ay + m.pose.position.z = 1.0 + m.scale.x = 3.0; m.scale.y = 3.0; m.scale.z = 3.0 + m.color.r = 1.0; m.color.g = 1.0; m.color.b = 0.0; m.color.a = 0.9 + self.attractor_pub.publish(m) + + # ───────────────────────────────────────────────────────────────────────── + # Waypoint markers # ───────────────────────────────────────────────────────────────────────── def publish_waypoints_markers(self): if not self.target_list: return - ma = MarkerArray() for i, wp in enumerate(self.target_list): + # Tolerance sphere m = Marker() m.header.frame_id = self.frame_id m.header.stamp = self.get_clock().now().to_msg() - m.ns = "waypoints"; m.id = i + m.ns = 'waypoints'; m.id = i m.type = Marker.SPHERE; m.action = Marker.ADD m.pose.position.x = float(wp['x']) m.pose.position.y = float(wp['y']) @@ -268,22 +498,25 @@ def publish_waypoints_markers(self): m.color.g = 1.0; m.color.a = 0.3 ma.markers.append(m) + # Label t = Marker() t.header = m.header - t.ns = "waypoint_labels"; t.id = i + 1000 + t.ns = 'waypoint_labels'; t.id = i + 1000 t.type = Marker.TEXT_VIEW_FACING; t.action = Marker.ADD t.pose.position.x = m.pose.position.x t.pose.position.y = m.pose.position.y t.pose.position.z = 2.0 t.scale.z = 1.5 - t.color.r = 1.0; t.color.g = 1.0; t.color.b = 1.0; t.color.a = 1.0 - t.text = f"WP{i + 1}" + t.color.r = t.color.g = t.color.b = t.color.a = 1.0 + t.text = f'WP{i + 1}' ma.markers.append(t) self.viz_pub.publish(ma) - def _convert_list_to_path(self, points): - """Transforme une liste [[x, y, yaw], ...] en nav_msgs/Path.""" + # ───────────────────────────────────────────────────────────────────────── + # Dubins path helper + # ───────────────────────────────────────────────────────────────────────── + def _convert_list_to_path(self, points) -> Path: path_msg = Path() path_msg.header.frame_id = self.frame_id path_msg.header.stamp = self.get_clock().now().to_msg() @@ -295,11 +528,6 @@ def _convert_list_to_path(self, points): path_msg.poses.append(ps) return path_msg - def get_result_callback(self, future): - result = future.result().result - self.get_logger().info("Final Result") - rclpy.shutdown() - # ───────────────────────────────────────────────────────────────────────────── def main(args=None): diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py index e0502ec05..06f48a396 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py @@ -1,128 +1,143 @@ import rclpy +import math +import json from rclpy.node import Node -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from smarc_action_base.gentler_action_server import GentlerActionServer -from geodesy import utm from geographic_msgs.msg import GeoPoint from tf2_geometry_msgs import do_transform_pose_stamped from tf_transformations import euler_from_quaternion from rclpy.time import Duration, Time -from nav_msgs.srv import SetMap -from nav_msgs.msg import OccupancyGrid -from nav_msgs.msg import MapMetaData -from nav_msgs.srv import GetPlan -from nav_msgs.msg import Path -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Twist, TwistStamped -from geometry_msgs.msg import Point -from visualization_msgs.msg import Marker, MarkerArray -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Float32, Empty -from std_msgs.msg import String +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Twist, TwistStamped, PoseStamped, Quaternion +from tf2_ros import Buffer, TransformListener +from smarc_utilities import georef_utils +import tf_transformations + from evolo_msgs.msg import Topics as evoloTopics from smarc_msgs.msg import Topics as smarcTopics from smarc_control_msgs.msg import Topics as controlTopics -from tf2_ros import Buffer, TransformException, TransformListener -import numpy as np -import time -import math -import json -from geometry_msgs.msg import Quaternion -from smarc_utilities import georef_utils - -import tf_transformations -import os -from enum import Enum from dubins_planner.dubins import Waypoint, calc_dubins_path, dubins_traj - -from sensor_msgs.msg import Image, CompressedImage, CameraInfo +from smarc_msgs.msg import GeofencePolygonsStamped +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point +# ───────────────────────────────────────────────────────────────────────────── +# Pure Pursuit Controller +# ───────────────────────────────────────────────────────────────────────────── class PurePursuitController: """ - Parameters: - Ld_base : Base lookahead distance (m) - Ld_gain : Lookahead velocity gain (Ld = Ld_base + Ld_gain * v) - omega_max : Angular velocity saturation (deg/s) - ff_gain : Dubins curvature feedforward gain (0 = disabled) + Combined path-tracking controller: + - Pure Pursuit : geometric lookahead steering + - Feedforward : anticipates upcoming curvature from the Dubins path + - CTE proportional : immediate correction of lateral offset (cte_kp) + - CTE integral : cancels persistent drift from wind / current (cte_ki) + - Heading alignment : corrects residual heading error against the path (heading_kp) + + All angular outputs are in degrees (matching the Evolo command interface). """ - def __init__(self, - Ld_base, - Ld_gain, - omega_max, - ff_gain, - dubins_step): - self.Ld_base = Ld_base - self.Ld_gain = Ld_gain - self.omega_max = omega_max - self.ff_gain = ff_gain - self.dubins_step = dubins_step - - def compute(self, - robot_x: float, - robot_y: float, - robot_yaw: float, - robot_v: float, - path: list, - cursor: int, - ) -> tuple[float, float, int]: - + def __init__(self, Ld_base, Ld_gain, omega_max, dubins_step, + cte_kp=0.0, cte_ki=0.0, cte_integral_max=10.0, heading_kp=0.0): + self.Ld_base = Ld_base + self.Ld_gain = Ld_gain + self.omega_max = omega_max + self.dubins_step = dubins_step + self.cte_kp = cte_kp + self.cte_ki = cte_ki + self.cte_integral_max = cte_integral_max + self.heading_kp = heading_kp # [deg/rad] + self._cte_integral = 0.0 + + def reset(self): + self._cte_integral = 0.0 + + def compute(self, robot_x, robot_y, robot_yaw, robot_v, path, cursor, dt): + # ── Dynamic lookahead distance ──────────────────────────────────────── Ld = self.Ld_base + self.Ld_gain * robot_v - lookahead_idx = cursor + # ── Find lookahead point ────────────────────────────────────────────── + lookahead_idx = len(path) - 1 for i in range(cursor, len(path)): px, py, _ = path[i] if math.hypot(px - robot_x, py - robot_y) >= Ld: lookahead_idx = i break - else: - lookahead_idx = len(path) - 1 - - lx, ly, lyaw = path[lookahead_idx] + lx, ly, _ = path[lookahead_idx] angle_to_target = math.atan2(ly - robot_y, lx - robot_x) alpha = math.atan2( math.sin(angle_to_target - robot_yaw), math.cos(angle_to_target - robot_yaw), ) - dist_to_target = math.hypot(lx - robot_x, ly - robot_y) - if dist_to_target < 0.1: - kappa = 0.0 - else: - kappa = 2.0 * math.sin(alpha) / dist_to_target - - ff_omega_rad = 0.0 - if self.ff_gain > 0.0 and lookahead_idx + 1 < len(path): - _, _, yaw_next = path[min(lookahead_idx + 1, len(path) - 1)] - _, _, yaw_cur = path[lookahead_idx] - dyaw = math.atan2(math.sin(yaw_next - yaw_cur), - math.cos(yaw_next - yaw_cur)) - kappa_local = dyaw / self.dubins_step - ff_omega_rad = self.ff_gain * robot_v * kappa_local - - omega_rad = robot_v * kappa + ff_omega_rad - omega_deg = math.degrees(omega_rad) + kappa = 0.0 if dist_to_target < 0.1 else 2.0 * math.sin(alpha) / dist_to_target + + # ── Pure Pursuit base command (rad/s) ───────────────────────────────── + omega_pp = robot_v * kappa + + # ── Feedforward from Dubins path curvature (rad/s) ──────────────────── + # Reads the heading derivative at the lookahead point to anticipate turns. + ff_omega = 0.0 + + # ── Cross-track error (CTE) on the current path segment ─────────────── + x1, y1, _ = path[cursor] + x2, y2, _ = path[min(cursor + 1, len(path) - 1)] + dx, dy = x2 - x1, y2 - y1 + seg_len = math.hypot(dx, dy) + cross_error = 0.0 + if seg_len > 1e-3: + t = ((robot_x - x1) * dx + (robot_y - y1) * dy) / seg_len ** 2 + t = max(0.0, min(1.0, t)) + foot_x = x1 + t * dx + foot_y = y1 + t * dy + # Normal vector pointing to the left of the path direction + nx = -dy / seg_len + ny = dx / seg_len + cross_error = (robot_x - foot_x) * nx + (robot_y - foot_y) * ny + + # ── CTE integral — always active, including in curves ───────────────── + # Never freezing the integral ensures that persistent external forces + # (wind, current) are compensated regardless of path curvature. + self._cte_integral = max( + -self.cte_integral_max, + min(self.cte_integral_max, self._cte_integral + cross_error * dt) + ) + # CTE proportional + integral correction (degrees) + # cte_kp [deg/m] : immediate correction proportional to lateral offset + # cte_ki [deg/(m·s)] : slow correction that cancels steady-state drift + omega_cte_deg = -(self.cte_kp * cross_error + self.cte_ki * self._cte_integral) + + # ── Heading alignment with the path ─────────────────────────────────── + # Pure Pursuit alone can leave a residual heading error (especially in + # curves or under side-forces). This term drives the robot heading toward + # the path tangent at the cursor position. + _, _, path_yaw = path[cursor] + heading_error = math.atan2(math.sin(path_yaw - robot_yaw), + math.cos(path_yaw - robot_yaw)) + # heading_kp [deg/rad]: proportional gain on heading error + omega_heading_deg = self.heading_kp * heading_error + + # ── Total omega (degrees) ───────────────────────────────────────────── + omega_deg = math.degrees(omega_pp + ff_omega) + omega_cte_deg + omega_heading_deg omega_deg = max(-self.omega_max, min(self.omega_max, omega_deg)) return omega_deg, lookahead_idx # ───────────────────────────────────────────────────────────────────────────── -# Action +# Action # ───────────────────────────────────────────────────────────────────────────── -class EvoloMovePath(): +class EvoloMovePath: class WP: def __init__(self, p, tol, speed_kn): @@ -133,42 +148,41 @@ def __init__(self, p, tol, speed_kn): def __init__(self, node: Node, action_name: str): self._node = node - self._node.declare_parameters( - namespace='', - parameters=[ - ('v_min', rclpy.Parameter.Type.DOUBLE), - ('v_max', rclpy.Parameter.Type.DOUBLE), - ('omega_max', rclpy.Parameter.Type.DOUBLE), - ('ld_base', rclpy.Parameter.Type.DOUBLE), - ('ld_gain', rclpy.Parameter.Type.DOUBLE), - ('ff_gain', rclpy.Parameter.Type.DOUBLE), - ('min_turning_radius', rclpy.Parameter.Type.DOUBLE), - ('dubins_step', rclpy.Parameter.Type.DOUBLE), - ('timeout', rclpy.Parameter.Type.DOUBLE), - ('speed_map_slow', rclpy.Parameter.Type.DOUBLE), - ('speed_map_medium', rclpy.Parameter.Type.DOUBLE), - ('speed_map_high', rclpy.Parameter.Type.DOUBLE), - ('frame_id', rclpy.Parameter.Type.STRING) - ] - ) - self.V_MIN = self._node.get_parameter('v_min').value - self.V_MAX = self._node.get_parameter('v_max').value - self.OMEGA_MAX = self._node.get_parameter('omega_max').value - self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value - self.DUBINS_STEP = self._node.get_parameter('dubins_step').value - self.timeout = self._node.get_parameter('timeout').value - self.frame_id = self._node.get_parameter('frame_id').value - self.SPEED_MAP = { - "slow": self._node.get_parameter('speed_map_slow').value, - "medium": self._node.get_parameter('speed_map_medium').value, - "high": self._node.get_parameter('speed_map_high').value - } + self._node.declare_parameters(namespace='', parameters=[ + ('v_min', rclpy.Parameter.Type.DOUBLE), + ('v_max', rclpy.Parameter.Type.DOUBLE), + ('omega_max', rclpy.Parameter.Type.DOUBLE), + ('ld_base', rclpy.Parameter.Type.DOUBLE), + ('ld_gain', rclpy.Parameter.Type.DOUBLE), + ('min_turning_radius', rclpy.Parameter.Type.DOUBLE), + ('dubins_step', rclpy.Parameter.Type.DOUBLE), + ('timeout', rclpy.Parameter.Type.DOUBLE), + ('frame_id', rclpy.Parameter.Type.STRING), + ('cte_kp', rclpy.Parameter.Type.DOUBLE), # proportional CTE gain + ('cte_ki', rclpy.Parameter.Type.DOUBLE), + ('cte_integral_max', rclpy.Parameter.Type.DOUBLE), + ('heading_kp', rclpy.Parameter.Type.DOUBLE), # heading alignment gain + ('dubins_mode', rclpy.Parameter.Type.STRING), + ]) + + self.V_MIN = self._node.get_parameter('v_min').value + self.V_MAX = self._node.get_parameter('v_max').value + self.OMEGA_MAX = self._node.get_parameter('omega_max').value + self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value + self.DUBINS_STEP = self._node.get_parameter('dubins_step').value + self.DUBINS_MODE = self._node.get_parameter('dubins_mode').value + self.timeout = self._node.get_parameter('timeout').value + self.frame_id = self._node.get_parameter('frame_id').value + self.controller = PurePursuitController( - Ld_base = self._node.get_parameter('ld_base').value, - Ld_gain = self._node.get_parameter('ld_gain').value, - omega_max = self.OMEGA_MAX, - ff_gain = self._node.get_parameter('ff_gain').value, - dubins_step = self.DUBINS_STEP + Ld_base = self._node.get_parameter('ld_base').value, + Ld_gain = self._node.get_parameter('ld_gain').value, + omega_max = self.OMEGA_MAX, + dubins_step = self.DUBINS_STEP, + cte_kp = self._node.get_parameter('cte_kp').value, + cte_ki = self._node.get_parameter('cte_ki').value, + cte_integral_max = self._node.get_parameter('cte_integral_max').value, + heading_kp = self._node.get_parameter('heading_kp').value, ) self._as = GentlerActionServer( @@ -178,68 +192,58 @@ def __init__(self, node: Node, action_name: str): self._prepare_loop, self._loop_inner, self._give_feedback, - loop_frequency=30, + loop_frequency=10, ) self._tf_buffer = Buffer() - self._tf_listener = TransformListener(self._tf_buffer, self._node, - spin_thread=True) + self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=True) - self.robot_position = PoseStamped() - self.robot_position_time = None - self.current_yaw = None - self.distance_to_target = None + self.robot_position = PoseStamped() + self.robot_position_time = None + self.current_yaw = None self.current_linear_speed = 0.0 self.current_angular_speed = 0.0 - self.target_index = None - self.target_list = None - # Analyze - self._precision_ticks_close = None - self._precision_ticks_total = None - self._distance_travelled = None - self._last_robot_pos = None - self._too_far_since = None - - self.dubins_path = None - self.wp_end_indices = None - self.path_cursor = 0 - - self._last_calculated_path = None - self.current_ax = 0.0 - self.current_ay = 0.0 - self._prev_omega = 0.0 + self.target_list = None + self.speed_kn = 10.0 - self.action_started_time = None + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 - self.publisher_callback_group = ReentrantCallbackGroup() - self.subscriber_callback_group = ReentrantCallbackGroup() + self._last_calculated_path = None + self._prev_omega = 0.0 + self.action_started_time = None + self._precision_ticks_close = 0 + self._precision_ticks_total = 0 + self._distance_travelled = 0.0 + self._last_robot_pos = None - # Publishers and Subscribers - self.dubins_path_pub = self._node.create_publisher(Path, "rviz/planned_path", 10, callback_group=self.publisher_callback_group) - self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=self.publisher_callback_group) - # self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/ctrl/twist_setpoint', 10, callback_group=self.publisher_callback_group) - self.robot_sub = self._node.create_subscription(Odometry, smarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10,callback_group=self.subscriber_callback_group) - # self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10,callback_group=self.subscriber_callback_group) - + pub_cbg = ReentrantCallbackGroup() + sub_cbg = ReentrantCallbackGroup() + self.dubins_path_pub = self._node.create_publisher(Path, "rviz/planned_path", 10, callback_group=pub_cbg) + self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/evolo_cmd', 10, callback_group=pub_cbg) + self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10, callback_group=sub_cbg) + # self.robot_sub = self._node.create_subscription(Odometry, smarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10,callback_group=self.subscriber_callback_group) + # self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=self.publisher_callback_group) + # Subscriber geofence polygons + self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons', self._geofence_polygons_callback, 10,) + self._node.get_logger().info("EvoloMovePath started") - # ───────────────────────────────────────────────────────────────────────── + # ── Goal / Cancel ───────────────────────────────────────────────────────── def _on_goal_received(self, goal_request: dict) -> bool: - raw_speed = goal_request['speed'] - if isinstance(raw_speed, str) and raw_speed.lower() in self.SPEED_MAP: - speed_kn = self.SPEED_MAP[raw_speed.lower()] - else: - try: speed_kn = float(raw_speed) - except: speed_kn = self.SPEED_MAP['medium'] + try: + self.speed_kn = float(goal_request['speed']) + except (ValueError, TypeError): + self.speed_kn = 10.0 waypoints = goal_request.get('waypoints', []) if not waypoints: return False - self.target_index = 0 self.target_list = [] self.dubins_path = None self.wp_end_indices = None @@ -251,13 +255,16 @@ def _on_goal_received(self, goal_request: dict) -> bool: pose = self.latlon_to_local_frame([lat, lon]) if pose is None: return False - self.target_list.append(self.WP(p=pose, tol=tol, speed_kn=speed_kn)) + self.target_list.append(self.WP(p=pose, tol=tol, speed_kn=self.speed_kn)) self._node.get_logger().info( f" WP{len(self.target_list)}: " - f"({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f}) tol={tol}m" + f"({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f})" ) - self._waypoints_for_client = [{'x': wp.p.pose.position.x, 'y': wp.p.pose.position.y, 'tol': wp.tol} for wp in self.target_list] + self._waypoints_for_client = [ + {'x': wp.p.pose.position.x, 'y': wp.p.pose.position.y, 'tol': wp.tol} + for wp in self.target_list + ] return True def _on_cancel_received(self) -> bool: @@ -265,280 +272,202 @@ def _on_cancel_received(self) -> bool: return True def _prepare_loop(self) -> None: - self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) - self.dubins_path = None - self.wp_end_indices = None - self.path_cursor = 0 - self.target_index = 0 - self._precision_ticks_close = 0 - self._precision_ticks_total = 0 - self._distance_travelled = 0.0 - self._last_robot_pos = None - self._too_far_since = None - - - def _get_local_curvature(self, path: list, cursor: int) -> float: - - if cursor + 2 >= len(path): - return 0.0 - - x1, y1, yaw1 = path[cursor] - x2, y2, yaw2 = path[cursor + 1] - x3, y3, yaw3 = path[cursor + 2] - - dyaw1 = math.atan2(math.sin(yaw2 - yaw1), math.cos(yaw2 - yaw1)) - dyaw2 = math.atan2(math.sin(yaw3 - yaw2), math.cos(yaw3 - yaw2)) - - avg_dyaw = (dyaw1 + dyaw2) / 2.0 - kappa = avg_dyaw / self.DUBINS_STEP - - return kappa - - - # ───────────────────────────────────────────────────────────────────────── + self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 + self._precision_ticks_close = 0 + self._precision_ticks_total = 0 + self._distance_travelled = 0.0 + self._last_robot_pos = None + self._prev_omega = 0.0 + self.controller.reset() + + # ── Main loop ───────────────────────────────────────────────────────────── def _loop_inner(self) -> bool | None: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) - runtime = time_now - self.action_started_time - - if runtime > self.timeout: + if time_now - self.action_started_time > self.timeout: self._send_stop() return False if self.robot_position_time is None or self.current_yaw is None: return None - if not self.target_list or self.target_index >= len(self.target_list): - return True - robot_pos = self.robot_position.pose.position + # ── Distance travelled ──────────────────────────────────────────────── if self._last_robot_pos is not None: - step = math.hypot( + self._distance_travelled += math.hypot( robot_pos.x - self._last_robot_pos[0], robot_pos.y - self._last_robot_pos[1], ) - self._distance_travelled += step self._last_robot_pos = (robot_pos.x, robot_pos.y) - # Planification + # ── Plan once ───────────────────────────────────────────────────────── if self.dubins_path is None: if not self._plan_global_dubins(): return None path = self.dubins_path - ####################################################################################################################### - search_end = min(len(path), self.path_cursor + 400) - candidate = self._find_closest(robot_pos, self.path_cursor, search_end) + + # ── Cursor: small fixed window to prevent cross-loop jumps ──────────── + # At v=14 m/s and 10 Hz → ~1.4 m/tick ≈ 3 pts/tick (step=0.5 m). + # A 40-point window (20 m) is wide enough to advance but too narrow + # to accidentally skip an entire Dubins arc. + WINDOW = 40 + search_end = min(len(path), self.path_cursor + WINDOW) + candidate = self._find_closest(robot_pos, self.path_cursor, search_end) self.path_cursor = max(self.path_cursor, candidate) self.path_cursor = min(self.path_cursor, len(path) - 1) + # ── End of path ─────────────────────────────────────────────────────── + if self.path_cursor >= len(path) - 1: + self._node.get_logger().info("End of Dubins path reached") + self._send_stop() + return True - # Accuracy + # ── Precision metric (lateral distance to path) ─────────────────────── + cx, cy, cyaw = path[self.path_cursor] + dx = robot_pos.x - cx + dy = robot_pos.y - cy + dist_to_curve = abs(math.cos(cyaw) * dy - math.sin(cyaw) * dx) self._precision_ticks_total += 1 - cx, cy, _ = path[self.path_cursor] - dist_to_curve = math.hypot(robot_pos.x - cx, robot_pos.y - cy) - if dist_to_curve < 1.0: self._precision_ticks_close += 1 - - """ - # REPLAN : if more than 3 m during 5 sec or - if dist_to_curve > 3.0: - if self._too_far_since is None: - self._too_far_since = time_now - elif time_now - self._too_far_since >= 5.0: - self._node.get_logger().warn(f"REPLAN BECAUSE OF TIME") - self._too_far_since = None - if self._plan_global_dubins(): - path = self.dubins_path - self.path_cursor = 0 - else: - return None - else: - self._too_far_since = None - - if dist_to_curve > 5.0: - self._node.get_logger().warn(f"REPLAN BECAUSE OF DISTANCE") - self._too_far_since = None - if self._plan_global_dubins(): - path = self.dubins_path - self.path_cursor = 0 - else: - return None - """ - - current_wp = self.target_list[self.target_index] - dist_to_wp = math.hypot( - current_wp.p.pose.position.x - robot_pos.x, - current_wp.p.pose.position.y - robot_pos.y, - ) - self.distance_to_target = dist_to_wp - - wp_end_idx = self.wp_end_indices[self.target_index] - wp_start_idx = self.wp_end_indices[self.target_index - 1] if self.target_index > 0 else 0 - wp_arc_len = max(1, wp_end_idx - wp_start_idx) - arc_done = (self.path_cursor - wp_start_idx) / wp_arc_len >= 0.90 - - if dist_to_wp < current_wp.tol and arc_done: - self._node.get_logger().info( - f"✓ WP{self.target_index + 1} atteint (dist={dist_to_wp:.1f}m)") - self.target_index += 1 - if self.target_index >= len(self.target_list): - self._send_stop() - return True - self.path_cursor = self.wp_end_indices[self.target_index - 1] - return None - attr_idx = min(self.path_cursor + 40, len(path) - 1) - ax, ay, _ = path[attr_idx] - self.current_ax = ax - self.current_ay = ay + # ── Speed ───────────────────────────────────────────────────────────── + v = max(self.V_MIN, min(self.V_MAX, self.speed_kn)) - desired_angle = math.atan2(ay - robot_pos.y, ax - robot_pos.x) - angle_error = math.atan2( - math.sin(desired_angle - self.current_yaw), - math.cos(desired_angle - self.current_yaw), - ) - abs_err_deg = abs(math.degrees(angle_error)) - - # Control - v = current_wp.speed_kn - - omega, lookahead_idx = self.controller.compute( + # ── Controller ──────────────────────────────────────────────────────── + omega, _ = self.controller.compute( robot_x = float(robot_pos.x), robot_y = float(robot_pos.y), robot_yaw = float(self.current_yaw), - robot_v = float(self.current_linear_speed) or v, + robot_v = float(self.current_linear_speed) if self.current_linear_speed > 0.5 else v, path = path, cursor = self.path_cursor, + dt = 0.1, ) - v = self.V_MIN + (v - self.V_MIN) - - - ################################################################################################### - - # Smooth - SPIKE_THRESHOLD = 5.0 - DEAD_BAND = 0.5 - - delta = omega - self._prev_omega - if abs(delta) > SPIKE_THRESHOLD: - omega = self._prev_omega + math.copysign(SPIKE_THRESHOLD, delta) - if abs(omega - self._prev_omega) < DEAD_BAND: - omega = self._prev_omega + # ── Slew-rate limiter (prevents abrupt rudder jumps) ────────────────── + MAX_DELTA = 4.0 # deg/tick + omega_smoothed = self._prev_omega + max(-MAX_DELTA, min(MAX_DELTA, omega - self._prev_omega)) + self._prev_omega = omega_smoothed - - ALPHA = 0.3 - omega = ALPHA * omega + (1.0 - ALPHA) * self._prev_omega - - self._prev_omega = omega - - ################################################################################################### - - - - - - # Publication - cmd = TwistStamped() + # ── Publish ─────────────────────────────────────────────────────────── + cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() - cmd.header.frame_id = "evolo/base_link" + cmd.header.frame_id = self.frame_id cmd.twist.linear.x = v - cmd.twist.angular.z = math.radians(omega) + cmd.twist.angular.z = omega_smoothed self.speed_pub.publish(cmd) - - if not hasattr(self, '_log_counter'): - self._log_counter = 0 - self._log_counter += 1 - if self._log_counter % 30 == 0: - self._node.get_logger().info( - f"WP: {self.target_index} err={abs_err_deg:.1f}° v={v:.2f} ω={omega:.2f}°/s | " - f"cursor={self.path_cursor}/{len(path)} DTT={dist_to_wp:.1f}m" - ) - if self.target_index >= len(self.target_list): - self._send_stop() - if self._precision_ticks_total > 0: - precision_pct = 100.0 * self._precision_ticks_close / self._precision_ticks_total - self._node.get_logger().info( - f"Accuracy : {precision_pct:.1f}% " - f"({self._precision_ticks_close}/{self._precision_ticks_total} ticks < 1m " - f" Distance : {self._distance_travelled:.1f} m" - ) - return True return None - # ───────────────────────────────────────────────────────────────────────── + # ── Dubins global planner ───────────────────────────────────────────────── def _plan_global_dubins(self) -> bool: if self.current_yaw is None: return False robot_pos = self.robot_position.pose.position full_path, wp_ends = [], [] - q_prev = (robot_pos.x, robot_pos.y, self.current_yaw) - self._node.get_logger().info( - f"Planning Dubins | start=({q_prev[0]:.1f},{q_prev[1]:.1f}," - f"{math.degrees(q_prev[2]):.0f}°) | {len(self.target_list)} WPs" - ) + # Build the flat list of (x, y) positions: robot + all target waypoints + positions = [(robot_pos.x, robot_pos.y)] + for wp in self.target_list: + positions.append((wp.p.pose.position.x, wp.p.pose.position.y)) - for i, wp in enumerate(self.target_list): - wp_pos = wp.p.pose.position - wp_ori = wp.p.pose.orientation - is_identity = (abs(wp_ori.w - 1.0) < 0.01 and - abs(wp_ori.x) < 0.01 and abs(wp_ori.y) < 0.01 and - abs(wp_ori.z) < 0.01) - target_yaw = (math.atan2(wp_pos.y - q_prev[1], wp_pos.x - q_prev[0]) - if is_identity - else euler_from_quaternion([wp_ori.x, wp_ori.y, - wp_ori.z, wp_ori.w])[2]) - q_next = (wp_pos.x, wp_pos.y, target_yaw) - try: - w1 = Waypoint(q_prev[0], q_prev[1], math.degrees(q_prev[2])) - w2 = Waypoint(q_next[0], q_next[1], math.degrees(q_next[2])) - param = calc_dubins_path(w1, w2, self.MIN_TURNING_RADIUS) - seg_array = dubins_traj(param, self.DUBINS_STEP) - - seg = [(p[0], p[1], math.radians(p[2])) for p in seg_array] - - full_path.extend(seg) + R = self.MIN_TURNING_RADIUS + n = len(positions) + + # Build the list of Dubins states (x, y, heading) + states = [(positions[0][0], positions[0][1], self.current_yaw)] + real_wp_state_indices = [] + + for i in range(1, n): + curr = positions[i] + prev = positions[i - 1] + + if self.DUBINS_MODE == 'vwp': + # Heading arriving at the current waypoint + h_in = math.atan2(curr[1] - prev[1], curr[0] - prev[0]) + + if i == n - 1: + # ── Last waypoint ───────────────────────────────────────── + # Stop exactly here; do NOT add a virtual exit waypoint. + # The old code used h_exit = h_in + π which forced Dubins + # to generate an unnecessary U-turn loop at the end. + states.append((curr[0], curr[1], h_in)) + else: + # ── Intermediate waypoint ───────────────────────────────── + # Place a virtual waypoint (vwp) one turning-radius ahead + # in the incoming direction to ensure the robot passes + # through the real waypoint smoothly. + vwp = (curr[0] + R * math.cos(h_in), + curr[1] + R * math.sin(h_in)) + nxt = positions[i + 1] + h_next = math.atan2(nxt[1] - curr[1], nxt[0] - curr[0]) + next_vwp = (nxt[0] + R * math.cos(h_next), + nxt[1] + R * math.sin(h_next)) + h_exit = math.atan2(next_vwp[1] - curr[1], next_vwp[0] - curr[0]) + states.append((vwp[0], vwp[1], h_in)) + states.append((curr[0], curr[1], h_exit)) + + else: # smooth mode + h = (math.atan2(positions[i+1][1] - prev[1], positions[i+1][0] - prev[0]) + if i < n - 1 + else math.atan2(curr[1] - prev[1], curr[0] - prev[0])) + states.append((curr[0], curr[1], h)) + + real_wp_state_indices.append(len(states) - 1) + + # Generate Dubins segments between consecutive states + for i in range(len(states) - 1): + s1, s2 = states[i], states[i + 1] + w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) + w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) + params = calc_dubins_path(w1, w2, R) + if params: + seg = dubins_traj(params, self.DUBINS_STEP) + seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] + full_path.extend(seg if i == 0 else seg[1:]) + else: + full_path.append(list(s2)) + + if (i + 1) in real_wp_state_indices: wp_ends.append(len(full_path) - 1) - - except Exception as e: - self._node.get_logger().error(f"Dubins failed: {e}") - return False - q_prev = q_next - #Publish planned path for visualization self.dubins_path_pub.publish(self._path_msg(full_path)) - - self.dubins_path = full_path - self.wp_end_indices = wp_ends - self.path_cursor = 0 - self._node.get_logger().info( - f"✓ Dubins: {len(full_path)} pts | boundaries={wp_ends}") + self.dubins_path = full_path + self.wp_end_indices = wp_ends + self.path_cursor = 0 self._last_calculated_path = full_path + self._node.get_logger().info(f"Dubins path planned: {len(full_path)} points") return True - # ───────────────────────────────────────────────────────────────────────── + # ── Cursor helper ───────────────────────────────────────────────────────── def _find_closest(self, robot_pos, start: int, end: int) -> int: - path = self.dubins_path - path_len = len(path) - yaw = self.current_yaw or 0.0 - best_idx, best_score = start, float('inf') + """ + Score = distance + heading penalty. + No regressive term — higher indices are never penalised. + The fixed window prevents cross-loop jumps. + """ + path = self.dubins_path + yaw = self.current_yaw or 0.0 + best_idx = start + best_score = float('inf') for i in range(start, end): x, y, curve_yaw = path[i] dist = math.hypot(x - robot_pos.x, y - robot_pos.y) heading_diff = math.atan2(math.sin(curve_yaw - yaw), math.cos(curve_yaw - yaw)) - score = dist + 8.0 * (1.0 - math.cos(heading_diff)) \ - + 3.0 * (1.0 - i / path_len) + score = dist + 4.0 * (1.0 - math.cos(heading_diff)) if score < best_score: best_score = score best_idx = i return best_idx + # ── Utilities ───────────────────────────────────────────────────────────── def _send_stop(self): cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() @@ -563,45 +492,29 @@ def _path_msg(self, configurations) -> Path: msg.poses.append(ps) return msg - - def _give_feedback(self) -> str: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) runtime = time_now - self.action_started_time - n = len(self.target_list) if self.target_list else '?' - dtt = f"{self.distance_to_target:.1f}" if self.distance_to_target is not None else "?" - - fb_dict = { - "runtime": runtime, - "wp_idx": self.target_index, - "wp_total": n, - "dtt": dtt, - "ax": self.current_ax, - "ay": self.current_ay, - "precision_close": self._precision_ticks_close if hasattr(self, '_precision_ticks_close') else 0, - "precision_total": self._precision_ticks_total if hasattr(self, '_precision_ticks_total') else 0, - "precision_pct": round(100.0 * self._precision_ticks_close / self._precision_ticks_total, 2) - if hasattr(self, '_precision_ticks_total') and self._precision_ticks_total > 0 - else 0.0, - "distance_travelled": round(self._distance_travelled, 2) if hasattr(self, '_distance_travelled') else 0.0, - } - precision_pct = ( - round(100.0 * self._precision_ticks_close / self._precision_ticks_total, 2) - if self._precision_ticks_total > 0 - else 0.0 - ) - + pct = (round(100.0 * self._precision_ticks_close / self._precision_ticks_total, 2) + if self._precision_ticks_total > 0 else 0.0) self._node.get_logger().info( - f"precision={precision_pct}% | distance_travelled={self._distance_travelled:.2f}m" + f"precision={pct}% | dist={self._distance_travelled:.1f}m" + f" | cursor={self.path_cursor}/{len(self.dubins_path) if self.dubins_path else '?'}" ) + fb = { + "runtime": runtime, + "precision_pct": pct, + "precision_close": self._precision_ticks_close, + "precision_total": self._precision_ticks_total, + "distance_travelled": round(self._distance_travelled, 2), + } if self._last_calculated_path is not None: - fb_dict["full_path"] = self._last_calculated_path - self._last_calculated_path = None + fb["full_path"] = self._last_calculated_path + self._last_calculated_path = None if hasattr(self, '_waypoints_for_client') and self._waypoints_for_client: - fb_dict['wps'] = self._waypoints_for_client + fb['wps'] = self._waypoints_for_client self._waypoints_for_client = None - return json.dumps(fb_dict) - + return json.dumps(fb) def latlon_to_local_frame(self, point_list): geopoint = GeoPoint() @@ -628,9 +541,6 @@ def latlon_to_local_frame(self, point_list): return None def robot_odom_callback(self, msg: Odometry): - if self.frame_id is None: - self.frame_id = msg.header.frame_id - if msg.header.frame_id == self.frame_id: self.robot_position = PoseStamped() self.robot_position.header = msg.header @@ -657,16 +567,6 @@ def robot_odom_callback(self, msg: Odometry): self.current_linear_speed = msg.twist.twist.linear.x self.current_angular_speed = msg.twist.twist.angular.z - if not hasattr(self, '_odom_log_counter'): - self._odom_log_counter = 0 - self._odom_log_counter += 1 - if self._odom_log_counter % 30 == 0: - self._node.get_logger().info( - f"Odom: ({self.robot_position.pose.position.x:.2f}, " - f"{self.robot_position.pose.position.y:.2f}), " - f"yaw={math.degrees(self.current_yaw):.1f}°" - ) - def main(): rclpy.init() diff --git a/behaviours/evolo/evolo_move_path/setup.py b/behaviours/evolo/evolo_move_path/setup.py index 07d6825ac..8886adf79 100644 --- a/behaviours/evolo/evolo_move_path/setup.py +++ b/behaviours/evolo/evolo_move_path/setup.py @@ -29,15 +29,16 @@ # Client 'move_path_client = evolo_move_path.client:main', # Classical algorithms - 'move_path_server_coordinate = evolo_move_path.evolo_move_path_server_coordinate:main', - 'move_path_server_potential_field = evolo_move_path.evolo_move_path_server_potential_field:main', 'move_path_server_a_star = evolo_move_path.evolo_move_path_server_a_star:main', 'move_path_server_dubins_curves = evolo_move_path.evolo_move_path_server_dubins_curves:main', - # Discretized algorithms - 'move_path_server_discrete_point = evolo_move_path.evolo_move_path_server_discrete_point:main', - 'move_path_server_potential_field_discrete = evolo_move_path.evolo_move_path_server_potential_field_discrete:main', - # Other controller - 'move_path_server_potential_field_mpc = evolo_move_path.evolo_move_path_server_potential_field_mpc:main', + 'move_path_server_dubins_curves_lateral_normal = evolo_move_path.evolo_move_path_server_dubins_curves_lateral_normal:main', + 'move_path_server_dubins_curves_rrt = evolo_move_path.evolo_move_path_server_dubins_curves_rrt:main', + 'move_path_server_dubins_curves_rrt_star = evolo_move_path.evolo_move_path_server_dubins_curves_rrt_star:main', + 'move_path_server_shapely = evolo_move_path.evolo_move_path_server_shapely:main', + 'move_path_server_visibility = evolo_move_path.evolo_move_path_server_visibility:main', + + # Geofence + 'move_path_server_geofence = evolo_move_path.geofence_checker_evolo:main' ], }, ) \ No newline at end of file From 522617843251ef371c6da9b8e5dae426f44c498c Mon Sep 17 00:00:00 2001 From: Baptiste Date: Thu, 21 May 2026 18:01:45 +0200 Subject: [PATCH 2/3] field testing friday --- .../evolo_move_path/config/evolo_params.yaml | 15 +- .../evolo_move_path/evolo_move_path/client.py | 26 +- .../evolo_move_path_server_dubins_curves.py | 943 ++++++++++++++---- 3 files changed, 780 insertions(+), 204 deletions(-) diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml index fb9a04da8..ef49148f2 100644 --- a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -8,6 +8,12 @@ v_max: 14.0 omega_max: 16.0 + # Speed + speed_slow: 7.0 + speed_standard: 9.0 + speed_high: 12.0 + + # Pure Pursuit ld_base: 5.0 ld_gain: 0.2 @@ -15,7 +21,7 @@ # Dubins planner min_turning_radius: 30.0 dubins_step: 0.5 - dubins_mode: "vwp" # "vwp" or "smooth" + dubins_mode: "smooth" # "vwp" or "smooth" # Cross-track error cte_kp: 3.0 @@ -23,6 +29,9 @@ cte_integral_max: 10.0 heading_kp: 8.0 - hard_buffer: 10.0 + hard_buffer: 5.0 soft_buffer: 30.0 - geofence_timeout: 5.0 \ No newline at end of file + geofence_timeout: 5.0 + + + graph_type: "voronoi" \ No newline at end of file diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py index c6c593144..f471419a3 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py @@ -15,7 +15,8 @@ from geometry_msgs.msg import PointStamped from smarc_utilities import georef_utils import tf_transformations - +from evolo_msgs.msg import Topics as evoloTopics +from smarc_msgs.msg import Topics as SmarcTopics from smarc_msgs.msg import GeofencePolygonsStamped from sensor_msgs.msg import NavSatFix from smarc_msgs.msg import Topics as SmarcTopics @@ -46,23 +47,18 @@ def __init__(self): # ── Island buffer publishers ────────────────────────────────────────── # Soft buffer (white) — traversable, used as Dijkstra node source - self.island_soft_pub = self.create_publisher( - MarkerArray, 'rviz/island_buffer_soft', 10) + self.island_soft_pub = self.create_publisher(MarkerArray, 'rviz/island_buffer_soft', 10) # Hard buffer (orange) — absolute exclusion zone - self.island_hard_pub = self.create_publisher( - MarkerArray, 'rviz/island_buffer_hard', 10) + self.island_hard_pub = self.create_publisher(MarkerArray, 'rviz/island_buffer_hard', 10) # ── Subscribers ─────────────────────────────────────────────────────── - self.gps_sub = self.create_subscription( - NavSatFix, '/evolo/Lidar/gps', self._gps_callback, 10) - self.polygons_sub = self.create_subscription( - GeofencePolygonsStamped, '/smarc/geofence_polygons', - self._geofence_polygons_callback, 10) - self.odom_sub = self.create_subscription( - Odometry, 'evolo/smarc/odom', self._odom_callback, 10) - - self._geofence_start_client = RosActionClient( - self, BaseAction, 'smarc_start_geofence') + self.gps_sub = self.create_subscription(NavSatFix, '/evolo/Lidar/gps', self._gps_callback, 10) + # self.polygons_sub = self.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons',self._geofence_polygons_callback, 10) + self.polygons_sub = self.create_subscription(GeofencePolygonsStamped, SmarcTopics.GEOFENCE_POLYGONS_TOPIC, self._geofence_polygons_callback, 10,) + # self.odom_sub = self.create_subscription(Odometry, 'evolo/smarc/odom', self._odom_callback, 10) + self.odom_sub = self.create_subscription(Odometry, SmarcTopics.ODOM_TOPIC, self._odom_callback, 10) + + self._geofence_start_client = RosActionClient(self, BaseAction, 'smarc_start_geofence') self.robot_path_msg = Path() self.robot_path_msg.header.frame_id = self.frame_id diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py index 06f48a396..e134354ce 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py @@ -1,6 +1,8 @@ import rclpy import math import json +import threading +import heapq from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup @@ -12,39 +14,25 @@ from tf_transformations import euler_from_quaternion from rclpy.time import Duration, Time from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import Twist, TwistStamped, PoseStamped, Quaternion +from geometry_msgs.msg import TwistStamped, PoseStamped, Quaternion, Point from tf2_ros import Buffer, TransformListener from smarc_utilities import georef_utils import tf_transformations - from evolo_msgs.msg import Topics as evoloTopics -from smarc_msgs.msg import Topics as smarcTopics -from smarc_control_msgs.msg import Topics as controlTopics - +from smarc_msgs.msg import Topics as SmarcTopics from dubins_planner.dubins import Waypoint, calc_dubins_path, dubins_traj from smarc_msgs.msg import GeofencePolygonsStamped from visualization_msgs.msg import Marker, MarkerArray -from geometry_msgs.msg import Point - +from shapely.geometry import Polygon, MultiPolygon, LineString, Point as SPoint +from shapely.ops import unary_union # ───────────────────────────────────────────────────────────────────────────── # Pure Pursuit Controller # ───────────────────────────────────────────────────────────────────────────── class PurePursuitController: - """ - Combined path-tracking controller: - - Pure Pursuit : geometric lookahead steering - - Feedforward : anticipates upcoming curvature from the Dubins path - - CTE proportional : immediate correction of lateral offset (cte_kp) - - CTE integral : cancels persistent drift from wind / current (cte_ki) - - Heading alignment : corrects residual heading error against the path (heading_kp) - - All angular outputs are in degrees (matching the Evolo command interface). - """ - def __init__(self, Ld_base, Ld_gain, omega_max, dubins_step, cte_kp=0.0, cte_ki=0.0, cte_integral_max=10.0, heading_kp=0.0): self.Ld_base = Ld_base @@ -54,17 +42,15 @@ def __init__(self, Ld_base, Ld_gain, omega_max, dubins_step, self.cte_kp = cte_kp self.cte_ki = cte_ki self.cte_integral_max = cte_integral_max - self.heading_kp = heading_kp # [deg/rad] + self.heading_kp = heading_kp self._cte_integral = 0.0 def reset(self): self._cte_integral = 0.0 def compute(self, robot_x, robot_y, robot_yaw, robot_v, path, cursor, dt): - # ── Dynamic lookahead distance ──────────────────────────────────────── Ld = self.Ld_base + self.Ld_gain * robot_v - # ── Find lookahead point ────────────────────────────────────────────── lookahead_idx = len(path) - 1 for i in range(cursor, len(path)): px, py, _ = path[i] @@ -81,14 +67,8 @@ def compute(self, robot_x, robot_y, robot_yaw, robot_v, path, cursor, dt): dist_to_target = math.hypot(lx - robot_x, ly - robot_y) kappa = 0.0 if dist_to_target < 0.1 else 2.0 * math.sin(alpha) / dist_to_target - # ── Pure Pursuit base command (rad/s) ───────────────────────────────── omega_pp = robot_v * kappa - # ── Feedforward from Dubins path curvature (rad/s) ──────────────────── - # Reads the heading derivative at the lookahead point to anticipate turns. - ff_omega = 0.0 - - # ── Cross-track error (CTE) on the current path segment ─────────────── x1, y1, _ = path[cursor] x2, y2, _ = path[min(cursor + 1, len(path) - 1)] dx, dy = x2 - x1, y2 - y1 @@ -99,36 +79,22 @@ def compute(self, robot_x, robot_y, robot_yaw, robot_v, path, cursor, dt): t = max(0.0, min(1.0, t)) foot_x = x1 + t * dx foot_y = y1 + t * dy - # Normal vector pointing to the left of the path direction nx = -dy / seg_len ny = dx / seg_len cross_error = (robot_x - foot_x) * nx + (robot_y - foot_y) * ny - # ── CTE integral — always active, including in curves ───────────────── - # Never freezing the integral ensures that persistent external forces - # (wind, current) are compensated regardless of path curvature. self._cte_integral = max( -self.cte_integral_max, min(self.cte_integral_max, self._cte_integral + cross_error * dt) ) - - # CTE proportional + integral correction (degrees) - # cte_kp [deg/m] : immediate correction proportional to lateral offset - # cte_ki [deg/(m·s)] : slow correction that cancels steady-state drift omega_cte_deg = -(self.cte_kp * cross_error + self.cte_ki * self._cte_integral) - # ── Heading alignment with the path ─────────────────────────────────── - # Pure Pursuit alone can leave a residual heading error (especially in - # curves or under side-forces). This term drives the robot heading toward - # the path tangent at the cursor position. _, _, path_yaw = path[cursor] heading_error = math.atan2(math.sin(path_yaw - robot_yaw), math.cos(path_yaw - robot_yaw)) - # heading_kp [deg/rad]: proportional gain on heading error omega_heading_deg = self.heading_kp * heading_error - # ── Total omega (degrees) ───────────────────────────────────────────── - omega_deg = math.degrees(omega_pp + ff_omega) + omega_cte_deg + omega_heading_deg + omega_deg = math.degrees(omega_pp) + omega_cte_deg + omega_heading_deg omega_deg = max(-self.omega_max, min(self.omega_max, omega_deg)) return omega_deg, lookahead_idx @@ -158,21 +124,33 @@ def __init__(self, node: Node, action_name: str): ('dubins_step', rclpy.Parameter.Type.DOUBLE), ('timeout', rclpy.Parameter.Type.DOUBLE), ('frame_id', rclpy.Parameter.Type.STRING), - ('cte_kp', rclpy.Parameter.Type.DOUBLE), # proportional CTE gain + ('cte_kp', rclpy.Parameter.Type.DOUBLE), ('cte_ki', rclpy.Parameter.Type.DOUBLE), ('cte_integral_max', rclpy.Parameter.Type.DOUBLE), - ('heading_kp', rclpy.Parameter.Type.DOUBLE), # heading alignment gain + ('heading_kp', rclpy.Parameter.Type.DOUBLE), ('dubins_mode', rclpy.Parameter.Type.STRING), + ('hard_buffer', rclpy.Parameter.Type.DOUBLE), + ('soft_buffer', rclpy.Parameter.Type.DOUBLE), + ('geofence_timeout', rclpy.Parameter.Type.DOUBLE), + ('speed_slow', rclpy.Parameter.Type.DOUBLE), + ('speed_standard', rclpy.Parameter.Type.DOUBLE), + ('speed_high', rclpy.Parameter.Type.DOUBLE), ]) - self.V_MIN = self._node.get_parameter('v_min').value - self.V_MAX = self._node.get_parameter('v_max').value - self.OMEGA_MAX = self._node.get_parameter('omega_max').value - self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value - self.DUBINS_STEP = self._node.get_parameter('dubins_step').value - self.DUBINS_MODE = self._node.get_parameter('dubins_mode').value - self.timeout = self._node.get_parameter('timeout').value - self.frame_id = self._node.get_parameter('frame_id').value + self.V_MIN = self._node.get_parameter('v_min').value + self.V_MAX = self._node.get_parameter('v_max').value + self.OMEGA_MAX = self._node.get_parameter('omega_max').value + self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value + self.DUBINS_STEP = self._node.get_parameter('dubins_step').value + self.DUBINS_MODE = self._node.get_parameter('dubins_mode').value + self.timeout = self._node.get_parameter('timeout').value + self.frame_id = self._node.get_parameter('frame_id').value + self.HARD_BUFFER = self._node.get_parameter('hard_buffer').value + self.SOFT_BUFFER = self._node.get_parameter('soft_buffer').value + self.GEOFENCE_TIMEOUT = self._node.get_parameter('geofence_timeout').value + self.SPEED_SLOW = self._node.get_parameter('speed_slow').value + self.SPEED_STANDARD = self._node.get_parameter('speed_standard').value + self.SPEED_HIGH = self._node.get_parameter('speed_high').value self.controller = PurePursuitController( Ld_base = self._node.get_parameter('ld_base').value, @@ -196,7 +174,8 @@ def __init__(self, node: Node, action_name: str): ) self._tf_buffer = Buffer() - self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=True) + self._tf_listener = TransformListener(self._tf_buffer, self._node, + spin_thread=True) self.robot_position = PoseStamped() self.robot_position_time = None @@ -204,37 +183,582 @@ def __init__(self, node: Node, action_name: str): self.current_linear_speed = 0.0 self.current_angular_speed = 0.0 - self.target_list = None - self.speed_kn = 10.0 - - self.dubins_path = None - self.wp_end_indices = None - self.path_cursor = 0 - - self._last_calculated_path = None - self._prev_omega = 0.0 - self.action_started_time = None + self.target_list = None + self.speed_kn = 10.0 + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 + self._last_calculated_path = None + self._prev_omega = 0.0 + self.action_started_time = None self._precision_ticks_close = 0 self._precision_ticks_total = 0 self._distance_travelled = 0.0 self._last_robot_pos = None + self._islands_lock = threading.Lock() + self._shapely_hard_zones = None + self._shapely_soft_zones = None + # Unions fusionnées — un seul polygone Shapely pour les checks rapides + self._hard_union_single = None + self._soft_union_single = None + self._contours_hard = [] + self._contours_soft = [] + self._geofence_received_time = None + + self._visibility_graph_data = None + pub_cbg = ReentrantCallbackGroup() sub_cbg = ReentrantCallbackGroup() - self.dubins_path_pub = self._node.create_publisher(Path, "rviz/planned_path", 10, callback_group=pub_cbg) - self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/evolo_cmd', 10, callback_group=pub_cbg) - self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10, callback_group=sub_cbg) - # self.robot_sub = self._node.create_subscription(Odometry, smarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10,callback_group=self.subscriber_callback_group) - # self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=self.publisher_callback_group) - # Subscriber geofence polygons - self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons', self._geofence_polygons_callback, 10,) + self.dubins_path_pub = self._node.create_publisher(Path, 'rviz/planned_path', 10, callback_group=pub_cbg) + # self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/evolo_cmd', 10, callback_group=pub_cbg) + self.viz_island_pub = self._node.create_publisher(MarkerArray, 'rviz/island_contours', 10, callback_group=pub_cbg) + + self.robot_sub = self._node.create_subscription(Odometry, SmarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10, callback_group=sub_cbg) + self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=pub_cbg) + + # self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10, callback_group=sub_cbg) + # self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons', self._geofence_polygons_callback, 10, callback_group=sub_cbg) + self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, SmarcTopics.GEOFENCE_POLYGONS_TOPIC, self._geofence_polygons_callback, 10, callback_group=sub_cbg) - self._node.get_logger().info("EvoloMovePath started") + self._node.create_timer(2.0, self._publish_island_visuals) + self._node.get_logger().info('EvoloMovePath started') + + # ───────────────────────────────────────────────────────────────────────── + # Geofence callback + # ───────────────────────────────────────────────────────────────────────── + def _geofence_polygons_callback(self, msg: GeofencePolygonsStamped): + if not msg.islands: + return + + try: + if not self._tf_buffer.can_transform( + self.frame_id, msg.header.frame_id, Time(seconds=0), + timeout=Duration(seconds=0, nanoseconds=100_000_000)): + return + tf = self._tf_buffer.lookup_transform( + self.frame_id, msg.header.frame_id, + Time(seconds=0), timeout=Duration(seconds=1)) + except Exception as e: + self._node.get_logger().warn(f'[Geofence] TF: {e}') + return + + from tf2_geometry_msgs import do_transform_point + from geometry_msgs.msg import PointStamped + + def _tr(pt): + ps = PointStamped() + ps.header = msg.header + ps.point.x, ps.point.y, ps.point.z = pt.x, pt.y, pt.z + try: + out = do_transform_point(ps, tf) + return (out.point.x, out.point.y) + except Exception: + return None + + islands_local = [] + for polygon in msg.islands: + pts = [_tr(p) for p in polygon.points] + pts = [p for p in pts if p is not None] + if len(pts) >= 3: + islands_local.append(pts) + + if not islands_local: + self._node.get_logger().warn('[Geofence] No valid polygons after TF') + return - # ── Goal / Cancel ───────────────────────────────────────────────────────── - def _on_goal_received(self, goal_request: dict) -> bool: + try: + raw_union = unary_union([Polygon(isl).buffer(0) for isl in islands_local]) + hard_union = raw_union.buffer(self.HARD_BUFFER) + soft_union = raw_union.buffer(self.SOFT_BUFFER) + except Exception as e: + self._node.get_logger().error(f'[Geofence] Shapely buffer: {e}') + return + + def _zones(union): + return list(union.geoms) if union.geom_type == 'MultiPolygon' else [union] + + def _contours(union, tol=1.0): + result = [] + for poly in _zones(union): + coords = list( + poly.simplify(tol, preserve_topology=True).exterior.coords[:-1]) + result.append([(c[0], c[1]) for c in coords]) + return result + + geofence_just_arrived = False + with self._islands_lock: + had_zones = self._shapely_hard_zones is not None + self._shapely_hard_zones = _zones(hard_union) + self._shapely_soft_zones = _zones(soft_union) + # Unions fusionnées précalculées — évite de boucler sur les zones à chaque check + self._hard_union_single = hard_union + self._soft_union_single = soft_union + self._contours_hard = _contours(hard_union) + self._contours_soft = _contours(soft_union) + self._geofence_received_time = int( + self._node.get_clock().now().nanoseconds * 1e-9) + if not had_zones: + geofence_just_arrived = True + + self._node.get_logger().info( + f'[Geofence] {len(islands_local)} island(s) | ' + f'hard={self.HARD_BUFFER}m ({len(self._shapely_hard_zones)} poly) | ' + f'soft={self.SOFT_BUFFER}m ({len(self._shapely_soft_zones)} poly)' + ) + + if geofence_just_arrived and self.dubins_path is not None: + self._node.get_logger().warn( + '[Geofence] Path planned without avoidance — invalidating') + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 + self.controller.reset() + + # ───────────────────────────────────────────────────────────────────────── + # Shapely helpers — toujours sur l'union fusionnée (plus rapide) + # ───────────────────────────────────────────────────────────────────────── + def _point_in_hard(self, x, y): + with self._islands_lock: + u = self._hard_union_single + return u is not None and u.contains(SPoint(x, y)) + + def _point_in_soft(self, x, y): + with self._islands_lock: + u = self._soft_union_single + return u is not None and u.contains(SPoint(x, y)) + + def _line_of_sight_hard(self, ax, ay, bx, by): + """True quand le segment ne pénètre pas la HARD zone.""" + with self._islands_lock: + u = self._hard_union_single + if u is None: + return True + seg = LineString([(ax, ay), (bx, by)]) + return not u.intersects(seg) + + def _segment_collides_soft(self, ax, ay, bx, by): + """True quand le segment touche la SOFT zone (déclencheur bypass).""" + with self._islands_lock: + u = self._soft_union_single + if u is None: + return False + seg = LineString([(ax, ay), (bx, by)]) + return u.intersects(seg) + + # ───────────────────────────────────────────────────────────────────────── + # RViz visualisation + # ───────────────────────────────────────────────────────────────────────── + def _publish_island_visuals(self): + with self._islands_lock: + contours_soft = list(self._contours_soft) + contours_hard = list(self._contours_hard) + if not contours_soft and not contours_hard: + return + + ma = MarkerArray() + stamp = self._node.get_clock().now().to_msg() + mid = 0 + + def _add(contour, ns_dots, ns_line, r, g, b, dot_sz, line_sz): + nonlocal mid + if len(contour) < 2: + return + pts = [Point(x=float(c[0]), y=float(c[1]), z=0.0) for c in contour] + sphere = Marker() + sphere.header.frame_id = self.frame_id + sphere.header.stamp = stamp + sphere.ns, sphere.id = ns_dots, mid; mid += 1 + sphere.type = Marker.SPHERE_LIST + sphere.action = Marker.ADD + sphere.scale.x = sphere.scale.y = sphere.scale.z = dot_sz + sphere.color.r, sphere.color.g, sphere.color.b, sphere.color.a = r, g, b, 1.0 + sphere.points = pts + ma.markers.append(sphere) + line = Marker() + line.header.frame_id = self.frame_id + line.header.stamp = stamp + line.ns, line.id = ns_line, mid; mid += 1 + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.scale.x = line_sz + line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 0.85 + line.points = pts + [pts[0]] + ma.markers.append(line) + + for c in contours_soft: + _add(c, 'soft_dots', 'soft_lines', 1.0, 1.0, 1.0, 0.8, 0.25) + for c in contours_hard: + _add(c, 'hard_dots', 'hard_lines', 1.0, 0.5, 0.0, 0.6, 0.30) + self.viz_island_pub.publish(ma) + + # ───────────────────────────────────────────────────────────────────────── + # Dijkstra + # ───────────────────────────────────────────────────────────────────────── + def _dijkstra(self, graph, start, end): + queue = [(0.0, start, [start])] + visited = set() + while queue: + cost, u, path = heapq.heappop(queue) + if u in visited: + continue + visited.add(u) + if u == end: + return path + for w, v in graph.get(u, []): + if v not in visited: + heapq.heappush(queue, (cost + w, v, path + [v])) + return None + + def _project_outside_soft(self, xy, soft_zones): + p = SPoint(xy[0], xy[1]) + for zone in soft_zones: + if zone.contains(p): + nearest = zone.exterior.interpolate(zone.exterior.project(p)) + cx, cy = zone.centroid.x, zone.centroid.y + dx, dy = nearest.x - cx, nearest.y - cy + d = math.hypot(dx, dy) or 1.0 + return (nearest.x + (dx / d) * 0.5, + nearest.y + (dy / d) * 0.5) + return xy + + # ───────────────────────────────────────────────────────────────────────── + # Bypass Dijkstra — graphe de visibilité optimisé + # ───────────────────────────────────────────────────────────────────────── + def _bypass_segment(self, start_xy, end_xy): + """ + Graphe de visibilité Dijkstra sur les vertices du contour soft. + Arêtes vérifiées contre la HARD zone uniquement. + Filtrage spatial : ne considère que les nœuds proches du segment. + Retourne une liste de (x,y) se terminant par end_xy. + """ + with self._islands_lock: + hard_union = self._hard_union_single + soft_zones = list(self._shapely_soft_zones) if self._shapely_soft_zones else [] + + if not soft_zones: + return [end_xy] + + safe_start = self._project_outside_soft(start_xy, soft_zones) + safe_end = self._project_outside_soft(end_xy, soft_zones) + + # ── Candidats : vertices du contour soft poussés vers l'extérieur ──── + push_dist = 1.0 + edge_subdivisions = 3 + candidate_pts = [] + + for zone in soft_zones: + cx, cy = zone.centroid.x, zone.centroid.y + coords = list( + zone.simplify(0.5, preserve_topology=True).exterior.coords[:-1]) + n_coords = len(coords) + + for idx in range(n_coords): + vx0, vy0 = coords[idx] + vx1, vy1 = coords[(idx + 1) % n_coords] + + for k in range(edge_subdivisions + 1): + t = k / edge_subdivisions + vx = vx0 + t * (vx1 - vx0) + vy = vy0 + t * (vy1 - vy0) + ddx, ddy = vx - cx, vy - cy + d = math.hypot(ddx, ddy) + if d < 1e-6: + continue + nx_u, ny_u = ddx / d, ddy / d + px = vx + nx_u * push_dist + py = vy + ny_u * push_dist + if hard_union is None or not hard_union.contains(SPoint(px, py)): + candidate_pts.append((px, py)) + + all_nodes = [(float(safe_start[0]), float(safe_start[1])), + (float(safe_end[0]), float(safe_end[1]))] + candidate_pts + + # ── Filtrage spatial : ne garder que les nœuds proches du segment ──── + sx, sy = safe_start + ex, ey = safe_end + seg_len = math.hypot(ex - sx, ey - sy) + margin = max(self.SOFT_BUFFER * 2.0, seg_len * 0.3) + mid_x = (sx + ex) / 2 + mid_y = (sy + ey) / 2 + half_diag = seg_len / 2 + margin + + filtered = [0, 1] # start et end toujours inclus + for i, (nx_c, ny_c) in enumerate(all_nodes[2:], 2): + if math.hypot(nx_c - mid_x, ny_c - mid_y) <= half_diag: + filtered.append(i) + + self._node.get_logger().info( + f' Dijkstra: {len(filtered) - 2}/{len(all_nodes) - 2} candidates ' + f'(spatial filter margin={margin:.1f}m)') + + # ── Construction du graphe sur les nœuds filtrés uniquement ────────── + graph = {i: [] for i in filtered} + for ii, i in enumerate(filtered): + ax, ay = all_nodes[i] + for j in filtered[ii + 1:]: + bx, by = all_nodes[j] + seg = LineString([(ax, ay), (bx, by)]) + if hard_union is None or not hard_union.intersects(seg): + dist = math.hypot(bx - ax, by - ay) + graph[i].append((dist, j)) + graph[j].append((dist, i)) + + route = self._dijkstra(graph, 0, 1) + if not route or len(route) < 2: + self._node.get_logger().warn(' Dijkstra: no path found') + return [end_xy] + + self._node.get_logger().info( + f' Dijkstra: route has {len(route) - 1} segment(s)') + + result = [] + for k, node_id in enumerate(route[1:], 1): + is_last = (k == len(route) - 1) + result.append(end_xy if is_last else all_nodes[node_id]) + return result + + # ───────────────────────────────────────────────────────────────────────── + # Segment-level avoidance + # ───────────────────────────────────────────────────────────────────────── + def _extend_positions_for_avoidance(self, positions): + with self._islands_lock: + has_zones = self._soft_union_single is not None + if not has_zones: + self._node.get_logger().warn( + 'Avoidance: no geofence data — planning without avoidance') + return positions + + extended = [positions[0]] + for i in range(len(positions) - 1): + ax, ay = positions[i] + bx, by = positions[i + 1] + if self._segment_collides_soft(ax, ay, bx, by): + self._node.get_logger().warn( + f' Segment {i}→{i+1} crosses soft buffer ' + f'({ax:.1f},{ay:.1f})→({bx:.1f},{by:.1f}) — running Dijkstra') + bypass = self._bypass_segment((ax, ay), (bx, by)) + extended.extend(bypass) + else: + extended.append((bx, by)) + return extended + + # ───────────────────────────────────────────────────────────────────────── + # Arc validation — lock pris une seule fois, pas dans la boucle + # ───────────────────────────────────────────────────────────────────────── + def _validate_dubins_arc(self, arc, check_step=5): + """True si aucun point échantillonné de l'arc n'est dans la HARD zone.""" + with self._islands_lock: + u = self._hard_union_single + if u is None: + return True + indices = list(range(0, len(arc), check_step)) + if indices[-1] != len(arc) - 1: + indices.append(len(arc) - 1) + for i in indices: + if u.contains(SPoint(arc[i][0], arc[i][1])): + return False + return True + + # ───────────────────────────────────────────────────────────────────────── + # Arc Dubins direct — sans récursion (utilisé par contour walk) + # ───────────────────────────────────────────────────────────────────────── + def _build_arc_direct(self, s1, s2, radius): + """ + Arc Dubins sans aucun fallback récursif. + Retourne une ligne droite uniquement si le Dubins échoue complètement. + Jamais appelé avec depth — ne peut pas boucler. + """ + w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) + w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) + params = calc_dubins_path(w1, w2, radius) + if params: + seg = dubins_traj(params, self.DUBINS_STEP) + seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] + if self._validate_dubins_arc(seg): + return seg + # Ligne droite uniquement ici — jamais depuis _build_arc principal + n = max(2, int(math.hypot(s2[0] - s1[0], s2[1] - s1[1]) / self.DUBINS_STEP)) + return [ + [s1[0] + k / (n - 1) * (s2[0] - s1[0]), + s1[1] + k / (n - 1) * (s2[1] - s1[1]), + math.atan2(s2[1] - s1[1], s2[0] - s1[0])] + for k in range(n) + ] + + # ───────────────────────────────────────────────────────────────────────── + # Contour walk — dernier recours avant ligne droite forcée + # ───────────────────────────────────────────────────────────────────────── + def _contour_walk_fallback(self, s1, s2, radius): + """ + Identifie la zone soft bloquant s1→s2. + Marche le contour dans les deux sens (CW et CCW) depuis le vertex + le plus proche de s1, jusqu'à trouver une ligne de vue vers s2. + Chaîne ensuite des arcs Dubins smooth via _build_arc_direct. + Retourne la liste de points ou None si aucune direction ne fonctionne. + """ + with self._islands_lock: + soft_zones = list(self._shapely_soft_zones) if self._shapely_soft_zones else [] + hard_union = self._hard_union_single + + if not soft_zones: + return None + + # Trouver la zone qui bloque le segment s1→s2 + seg_line = LineString([(s1[0], s1[1]), (s2[0], s2[1])]) + blocking_zone = None + for zone in soft_zones: + if zone.intersects(seg_line) or zone.contains(SPoint(s1[0], s1[1])): + blocking_zone = zone + break + if blocking_zone is None: + blocking_zone = min(soft_zones, key=lambda z: z.distance(seg_line)) + + # Vertices du contour poussés vers l'extérieur + push_dist = 1.0 + cx, cy = blocking_zone.centroid.x, blocking_zone.centroid.y + coords = list( + blocking_zone.simplify(0.5, preserve_topology=True).exterior.coords[:-1]) + + vertices = [] + for vx, vy in coords: + ddx, ddy = vx - cx, vy - cy + d = math.hypot(ddx, ddy) + if d < 1e-6: + continue + px = vx + (ddx / d) * push_dist + py = vy + (ddy / d) * push_dist + if hard_union is None or not hard_union.contains(SPoint(px, py)): + vertices.append((px, py)) + + if not vertices: + self._node.get_logger().warn( + ' Contour walk: no valid vertices on blocking zone') + return None + + n = len(vertices) + + # Vertex le plus proche de s1 + start_idx = min(range(n), + key=lambda i: math.hypot(vertices[i][0] - s1[0], + vertices[i][1] - s1[1])) + + # Marcher dans les deux sens jusqu'à avoir LoS vers s2 + def walk(direction): + pts = [] + for step in range(1, n + 1): + idx = (start_idx + direction * step) % n + vx, vy = vertices[idx] + pts.append((vx, vy)) + seg = LineString([(vx, vy), (s2[0], s2[1])]) + if hard_union is None or not hard_union.intersects(seg): + return pts + return None + + cw = walk(+1) + ccw = walk(-1) + + if cw is None and ccw is None: + self._node.get_logger().warn( + " Contour walk: aucune direction n'atteint la LoS vers s2") + return None + + if cw is None: + chosen = ccw + elif ccw is None: + chosen = cw + else: + chosen = cw if len(cw) <= len(ccw) else ccw + + self._node.get_logger().info( + f' Contour walk: {len(chosen)} vertex(es) pour atteindre la LoS') + + # Construire les états Dubins : s1 → vertices → s2 + all_states = [s1] + prev_xy = (s1[0], s1[1]) + for wx, wy in chosen: + h = math.atan2(wy - prev_xy[1], wx - prev_xy[0]) + all_states.append((wx, wy, h)) + prev_xy = (wx, wy) + all_states.append(s2) # heading original de s2 conservé + + # Chaîner les arcs Dubins smooth sans récursion + result = [] + for i in range(len(all_states) - 1): + seg = self._build_arc_direct(all_states[i], all_states[i + 1], radius) + result.extend(seg if i == 0 else seg[1:]) + return result + + # ───────────────────────────────────────────────────────────────────────── + # Arc principal avec fallback en cascade + # ───────────────────────────────────────────────────────────────────────── + def _build_arc(self, s1, s2, radius, depth=0): + """ + Construit un arc Dubins de s1=(x,y,yaw_rad) vers s2. + + Cascade : + depth 0 → arc OK ? retourne + → KO → Dijkstra midpoint (depth 1) + depth 1 → arc OK ? retourne + → KO → Dijkstra midpoint (depth 2) + depth 2 → arc OK ? retourne + → KO → contour walk (arcs Dubins sur vertices contour) + → si contour walk échoue : ligne droite forcée (ERROR) + """ + w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) + w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) + params = calc_dubins_path(w1, w2, radius) + + if params: + seg = dubins_traj(params, self.DUBINS_STEP) + seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] + if self._validate_dubins_arc(seg): + return seg # ✓ arc propre + + # ── Fallback Dijkstra (jusqu'à depth < 3) ──────────────────────────── + if depth < 3: + self._node.get_logger().warn( + f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' + f'hard collision at R={radius:.1f} — ' + f'inserting bypass midpoint (depth={depth})') + bypass = self._bypass_segment((s1[0], s1[1]), (s2[0], s2[1])) + + if len(bypass) > 1: + mid_xy = bypass[0] + h_to_mid = math.atan2(mid_xy[1] - s1[1], mid_xy[0] - s1[0]) + s_mid = (mid_xy[0], mid_xy[1], h_to_mid) + + seg_a = self._build_arc(s1, s_mid, radius, depth=depth + 1) + seg_b = self._build_arc(s_mid, s2, radius, depth=depth + 1) + return seg_a + seg_b[1:] + + # ── Last resort : contour walk smooth ──────────────────────────────── + self._node.get_logger().warn( + f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' + f'Dijkstra exhausted — contour walk fallback') + result = self._contour_walk_fallback(s1, s2, radius) + if result: + return result + + # Absolument dernier recours — ne devrait jamais arriver + self._node.get_logger().error( + f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' + f'contour walk failed — forced straight line') + n_interp = max(2, int( + math.hypot(s2[0] - s1[0], s2[1] - s1[1]) / self.DUBINS_STEP)) + return [ + [s1[0] + k / (n_interp - 1) * (s2[0] - s1[0]), + s1[1] + k / (n_interp - 1) * (s2[1] - s1[1]), + math.atan2(s2[1] - s1[1], s2[0] - s1[0])] + for k in range(n_interp) + ] + + # ───────────────────────────────────────────────────────────────────────── + # Goal / Cancel / Prepare + # ───────────────────────────────────────────────────────────────────────── + def _on_goal_received(self, goal_request): try: self.speed_kn = float(goal_request['speed']) except (ValueError, TypeError): @@ -257,9 +781,8 @@ def _on_goal_received(self, goal_request: dict) -> bool: return False self.target_list.append(self.WP(p=pose, tol=tol, speed_kn=self.speed_kn)) self._node.get_logger().info( - f" WP{len(self.target_list)}: " - f"({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f})" - ) + f' WP{len(self.target_list)}: ' + f'({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f})') self._waypoints_for_client = [ {'x': wp.p.pose.position.x, 'y': wp.p.pose.position.y, 'tol': wp.tol} @@ -267,11 +790,11 @@ def _on_goal_received(self, goal_request: dict) -> bool: ] return True - def _on_cancel_received(self) -> bool: + def _on_cancel_received(self): self._send_stop() return True - def _prepare_loop(self) -> None: + def _prepare_loop(self): self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) self.dubins_path = None self.wp_end_indices = None @@ -283,8 +806,10 @@ def _prepare_loop(self) -> None: self._prev_omega = 0.0 self.controller.reset() - # ── Main loop ───────────────────────────────────────────────────────────── - def _loop_inner(self) -> bool | None: + # ───────────────────────────────────────────────────────────────────────── + # Main control loop + # ───────────────────────────────────────────────────────────────────────── + def _loop_inner(self): time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) if time_now - self.action_started_time > self.timeout: self._send_stop() @@ -293,52 +818,57 @@ def _loop_inner(self) -> bool | None: if self.robot_position_time is None or self.current_yaw is None: return None + if self.dubins_path is None: + with self._islands_lock: + has_geofence = self._soft_union_single is not None + + if not has_geofence: + elapsed = time_now - self.action_started_time + if elapsed < self.GEOFENCE_TIMEOUT: + if int(elapsed) % 2 == 0: + self._node.get_logger().info( + f'Waiting for geofence… ({elapsed:.1f}s / ' + f'{self.GEOFENCE_TIMEOUT}s timeout)') + return None + else: + self._node.get_logger().warn( + f'Geofence timeout ({self.GEOFENCE_TIMEOUT}s) — ' + 'planning without avoidance') + + if not self._plan_global_dubins(): + return None + robot_pos = self.robot_position.pose.position - # ── Distance travelled ──────────────────────────────────────────────── if self._last_robot_pos is not None: self._distance_travelled += math.hypot( robot_pos.x - self._last_robot_pos[0], - robot_pos.y - self._last_robot_pos[1], - ) + robot_pos.y - self._last_robot_pos[1]) self._last_robot_pos = (robot_pos.x, robot_pos.y) - # ── Plan once ───────────────────────────────────────────────────────── - if self.dubins_path is None: - if not self._plan_global_dubins(): - return None - path = self.dubins_path - # ── Cursor: small fixed window to prevent cross-loop jumps ──────────── - # At v=14 m/s and 10 Hz → ~1.4 m/tick ≈ 3 pts/tick (step=0.5 m). - # A 40-point window (20 m) is wide enough to advance but too narrow - # to accidentally skip an entire Dubins arc. WINDOW = 40 search_end = min(len(path), self.path_cursor + WINDOW) candidate = self._find_closest(robot_pos, self.path_cursor, search_end) self.path_cursor = max(self.path_cursor, candidate) self.path_cursor = min(self.path_cursor, len(path) - 1) - # ── End of path ─────────────────────────────────────────────────────── if self.path_cursor >= len(path) - 1: - self._node.get_logger().info("End of Dubins path reached") + self._node.get_logger().info('End of Dubins path reached') self._send_stop() return True - # ── Precision metric (lateral distance to path) ─────────────────────── cx, cy, cyaw = path[self.path_cursor] - dx = robot_pos.x - cx - dy = robot_pos.y - cy - dist_to_curve = abs(math.cos(cyaw) * dy - math.sin(cyaw) * dx) + dist_to_curve = abs( + math.cos(cyaw) * (robot_pos.y - cy) - + math.sin(cyaw) * (robot_pos.x - cx)) self._precision_ticks_total += 1 if dist_to_curve < 1.0: self._precision_ticks_close += 1 - # ── Speed ───────────────────────────────────────────────────────────── v = max(self.V_MIN, min(self.V_MAX, self.speed_kn)) - # ── Controller ──────────────────────────────────────────────────────── omega, _ = self.controller.compute( robot_x = float(robot_pos.x), robot_y = float(robot_pos.y), @@ -349,12 +879,11 @@ def _loop_inner(self) -> bool | None: dt = 0.1, ) - # ── Slew-rate limiter (prevents abrupt rudder jumps) ────────────────── - MAX_DELTA = 4.0 # deg/tick - omega_smoothed = self._prev_omega + max(-MAX_DELTA, min(MAX_DELTA, omega - self._prev_omega)) + MAX_DELTA = 4.0 + omega_smoothed = (self._prev_omega + + max(-MAX_DELTA, min(MAX_DELTA, omega - self._prev_omega))) self._prev_omega = omega_smoothed - # ── Publish ─────────────────────────────────────────────────────────── cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() cmd.header.frame_id = self.frame_id @@ -364,94 +893,131 @@ def _loop_inner(self) -> bool | None: return None - # ── Dubins global planner ───────────────────────────────────────────────── - def _plan_global_dubins(self) -> bool: + # ───────────────────────────────────────────────────────────────────────── + # Global Dubins planner + # ───────────────────────────────────────────────────────────────────────── + def _plan_global_dubins(self): if self.current_yaw is None: return False robot_pos = self.robot_position.pose.position - full_path, wp_ends = [], [] - # Build the flat list of (x, y) positions: robot + all target waypoints - positions = [(robot_pos.x, robot_pos.y)] + # Step 1: positions brutes + raw_positions = [(robot_pos.x, robot_pos.y)] for wp in self.target_list: - positions.append((wp.p.pose.position.x, wp.p.pose.position.y)) + raw_positions.append( + (wp.p.pose.position.x, wp.p.pose.position.y)) + + self._node.get_logger().info( + f'Planning | start=({robot_pos.x:.1f},{robot_pos.y:.1f},' + f'{math.degrees(self.current_yaw):.0f}°) | ' + f'{len(self.target_list)} WP(s)') + + + # Step 2: insertion des bypass waypoints Dijkstra + # On mémorise quels indices dans `positions` sont des WP réels + with self._islands_lock: + has_geofence = self._soft_union_single is not None + + if has_geofence: + positions = self._extend_positions_for_avoidance(raw_positions) + else: + self._node.get_logger().warn('Planning without avoidance (geofence timeout)') + positions = raw_positions + + # Indices dans `positions` qui correspondent à un vrai WP utilisateur + # raw_positions[0] = robot, raw_positions[1..] = WPs réels + # _extend_positions_for_avoidance conserve les points originaux en dernier + # de chaque segment, donc on les retrouve en reconstruisant le mapping + real_wp_positions = set() + raw_idx = 0 + pos_idx = 0 + while pos_idx < len(positions) and raw_idx < len(raw_positions): + if (abs(positions[pos_idx][0] - raw_positions[raw_idx][0]) < 1e-3 and + abs(positions[pos_idx][1] - raw_positions[raw_idx][1]) < 1e-3): + if raw_idx > 0: # ne pas marquer le robot lui-même + real_wp_positions.add(pos_idx) + raw_idx += 1 + pos_idx += 1 + else: + pos_idx += 1 # bypass waypoint, on saute - R = self.MIN_TURNING_RADIUS - n = len(positions) + n_bypass = len(positions) - len(raw_positions) + if n_bypass: + self._node.get_logger().info( + f' Avoidance: {n_bypass} bypass waypoint(s) inserted') - # Build the list of Dubins states (x, y, heading) + # Step 3: construction des états Dubins depuis les positions + n = len(positions) + R = self.MIN_TURNING_RADIUS states = [(positions[0][0], positions[0][1], self.current_yaw)] real_wp_state_indices = [] for i in range(1, n): - curr = positions[i] - prev = positions[i - 1] + curr = positions[i] + prev = positions[i - 1] + is_real_wp = i in real_wp_positions + is_last = (i == n - 1) - if self.DUBINS_MODE == 'vwp': - # Heading arriving at the current waypoint + if self.DUBINS_MODE == 'vwp' and is_real_wp and not is_last: + # VWP uniquement sur les vrais waypoints intermédiaires h_in = math.atan2(curr[1] - prev[1], curr[0] - prev[0]) - - if i == n - 1: - # ── Last waypoint ───────────────────────────────────────── - # Stop exactly here; do NOT add a virtual exit waypoint. - # The old code used h_exit = h_in + π which forced Dubins - # to generate an unnecessary U-turn loop at the end. - states.append((curr[0], curr[1], h_in)) - else: - # ── Intermediate waypoint ───────────────────────────────── - # Place a virtual waypoint (vwp) one turning-radius ahead - # in the incoming direction to ensure the robot passes - # through the real waypoint smoothly. - vwp = (curr[0] + R * math.cos(h_in), - curr[1] + R * math.sin(h_in)) - nxt = positions[i + 1] - h_next = math.atan2(nxt[1] - curr[1], nxt[0] - curr[0]) - next_vwp = (nxt[0] + R * math.cos(h_next), - nxt[1] + R * math.sin(h_next)) - h_exit = math.atan2(next_vwp[1] - curr[1], next_vwp[0] - curr[0]) - states.append((vwp[0], vwp[1], h_in)) - states.append((curr[0], curr[1], h_exit)) - - else: # smooth mode - h = (math.atan2(positions[i+1][1] - prev[1], positions[i+1][0] - prev[0]) - if i < n - 1 - else math.atan2(curr[1] - prev[1], curr[0] - prev[0])) + vwp = (curr[0] + R * math.cos(h_in), + curr[1] + R * math.sin(h_in)) + nxt = positions[i + 1] + h_next = math.atan2(nxt[1] - curr[1], nxt[0] - curr[0]) + next_vwp = (nxt[0] + R * math.cos(h_next), + nxt[1] + R * math.sin(h_next)) + h_exit = math.atan2(next_vwp[1] - curr[1], + next_vwp[0] - curr[0]) + states.append((vwp[0], vwp[1], h_in)) + states.append((curr[0], curr[1], h_exit)) + else: + # Bypass waypoints et dernier WP : cap interpolé simple + h = (math.atan2(positions[i + 1][1] - prev[1], + positions[i + 1][0] - prev[0]) + if not is_last + else math.atan2(curr[1] - prev[1], curr[0] - prev[0])) states.append((curr[0], curr[1], h)) real_wp_state_indices.append(len(states) - 1) - # Generate Dubins segments between consecutive states - for i in range(len(states) - 1): - s1, s2 = states[i], states[i + 1] - w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) - w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) - params = calc_dubins_path(w1, w2, R) - if params: - seg = dubins_traj(params, self.DUBINS_STEP) - seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] - full_path.extend(seg if i == 0 else seg[1:]) - else: - full_path.append(list(s2)) + + + + # Step 4: chaîner les arcs avec _build_arc (cascade de fallbacks) + full_path, wp_ends = [], [] + + for i in range(len(states) - 1): + seg = self._build_arc(states[i], states[i + 1], R) + full_path.extend(seg if i == 0 else seg[1:]) if (i + 1) in real_wp_state_indices: wp_ends.append(len(full_path) - 1) + # Step 5: publication et stockage self.dubins_path_pub.publish(self._path_msg(full_path)) self.dubins_path = full_path self.wp_end_indices = wp_ends self.path_cursor = 0 self._last_calculated_path = full_path - self._node.get_logger().info(f"Dubins path planned: {len(full_path)} points") + + with self._islands_lock: + self._visibility_graph_data = { + 'island_contours_soft': list(self._contours_soft), + 'island_contours_hard': list(self._contours_hard), + } + + self._node.get_logger().info( + f'✓ Dubins path: {len(full_path)} pts | ' + f'{len(wp_ends)} WP end(s) | ' + f'{n_bypass} bypass pt(s)') return True - # ── Cursor helper ───────────────────────────────────────────────────────── - def _find_closest(self, robot_pos, start: int, end: int) -> int: - """ - Score = distance + heading penalty. - No regressive term — higher indices are never penalised. - The fixed window prevents cross-loop jumps. - """ + # ───────────────────────────────────────────────────────────────────────── + # Cursor helper + # ───────────────────────────────────────────────────────────────────────── + def _find_closest(self, robot_pos, start, end): path = self.dubins_path yaw = self.current_yaw or 0.0 best_idx = start @@ -467,7 +1033,9 @@ def _find_closest(self, robot_pos, start: int, end: int) -> int: best_idx = i return best_idx - # ── Utilities ───────────────────────────────────────────────────────────── + # ───────────────────────────────────────────────────────────────────────── + # Utilities + # ───────────────────────────────────────────────────────────────────────── def _send_stop(self): cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() @@ -475,7 +1043,7 @@ def _send_stop(self): cmd.twist.angular.z = 0.0 self.speed_pub.publish(cmd) - def _path_msg(self, configurations) -> Path: + def _path_msg(self, configurations): msg = Path() msg.header.frame_id = self.frame_id msg.header.stamp = self._node.get_clock().now().to_msg() @@ -492,28 +1060,31 @@ def _path_msg(self, configurations) -> Path: msg.poses.append(ps) return msg - def _give_feedback(self) -> str: + def _give_feedback(self): time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) runtime = time_now - self.action_started_time pct = (round(100.0 * self._precision_ticks_close / self._precision_ticks_total, 2) if self._precision_ticks_total > 0 else 0.0) self._node.get_logger().info( - f"precision={pct}% | dist={self._distance_travelled:.1f}m" - f" | cursor={self.path_cursor}/{len(self.dubins_path) if self.dubins_path else '?'}" - ) + f'precision={pct}% | dist={self._distance_travelled:.1f}m' + f' | cursor={self.path_cursor}/' + f'{len(self.dubins_path) if self.dubins_path else "?"}') fb = { - "runtime": runtime, - "precision_pct": pct, - "precision_close": self._precision_ticks_close, - "precision_total": self._precision_ticks_total, - "distance_travelled": round(self._distance_travelled, 2), + 'runtime': runtime, + 'precision_pct': pct, + 'precision_close': self._precision_ticks_close, + 'precision_total': self._precision_ticks_total, + 'distance_travelled': round(self._distance_travelled, 2), } if self._last_calculated_path is not None: - fb["full_path"] = self._last_calculated_path + fb['full_path'] = self._last_calculated_path self._last_calculated_path = None if hasattr(self, '_waypoints_for_client') and self._waypoints_for_client: fb['wps'] = self._waypoints_for_client self._waypoints_for_client = None + if self._visibility_graph_data is not None: + fb['visibility_graph'] = self._visibility_graph_data + self._visibility_graph_data = None return json.dumps(fb) def latlon_to_local_frame(self, point_list): @@ -533,11 +1104,10 @@ def latlon_to_local_frame(self, point_list): target_frame=self.frame_id, source_frame=ps.header.frame_id, time=Time(seconds=0), - timeout=Duration(seconds=1), - ) + timeout=Duration(seconds=1)) return do_transform_pose_stamped(ps, t) except Exception as e: - self._node.get_logger().error(f"TF failed: {e}") + self._node.get_logger().error(f'TF failed: {e}') return None def robot_odom_callback(self, msg: Odometry): @@ -554,34 +1124,35 @@ def robot_odom_callback(self, msg: Odometry): target_frame=self.frame_id, source_frame=msg.header.frame_id, time=Time(seconds=0), - timeout=Duration(seconds=1), - ) + timeout=Duration(seconds=1)) self.robot_position = do_transform_pose_stamped(raw, t) except Exception as e: - self._node.get_logger().error(f"Odom TF failed: {e}") + self._node.get_logger().error(f'Odom TF failed: {e}') return - self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) + self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) oq = self.robot_position.pose.orientation - (_, _, self.current_yaw) = euler_from_quaternion([oq.x, oq.y, oq.z, oq.w]) + (_, _, self.current_yaw) = euler_from_quaternion([oq.x, oq.y, oq.z, oq.w]) self.current_linear_speed = msg.twist.twist.linear.x self.current_angular_speed = msg.twist.twist.angular.z +# ───────────────────────────────────────────────────────────────────────────── def main(): rclpy.init() - node = Node("evolo_move_path_action_server") - EvoloMovePath(node, "move_path") + node = Node('evolo_move_path_action_server') + EvoloMovePath(node, 'move_path') executor = MultiThreadedExecutor() executor.add_node(node) try: executor.spin() except KeyboardInterrupt: - node.get_logger().info("Shutting down") + node.get_logger().info('Shutting down') finally: executor.shutdown() node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() \ No newline at end of file From 10ae600e7eb8208399b353d1612215ba298e9eed Mon Sep 17 00:00:00 2001 From: Baptiste Date: Fri, 22 May 2026 02:01:11 +0200 Subject: [PATCH 3/3] without turning around islands but still using geofence to know where we are --- .../evolo_move_path/config/evolo_params.yaml | 11 +- .../evolo_move_path/evolo_move_path/client.py | 26 +- .../evolo_move_path_server_dubins_curves.py | 855 ++++-------------- 3 files changed, 171 insertions(+), 721 deletions(-) diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml index ef49148f2..2afa73a58 100644 --- a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -8,11 +8,6 @@ v_max: 14.0 omega_max: 16.0 - # Speed - speed_slow: 7.0 - speed_standard: 9.0 - speed_high: 12.0 - # Pure Pursuit ld_base: 5.0 @@ -21,7 +16,7 @@ # Dubins planner min_turning_radius: 30.0 dubins_step: 0.5 - dubins_mode: "smooth" # "vwp" or "smooth" + dubins_mode: "vwp" # "vwp" or "smooth" # Cross-track error cte_kp: 3.0 @@ -29,9 +24,7 @@ cte_integral_max: 10.0 heading_kp: 8.0 - hard_buffer: 5.0 + hard_buffer: 10.0 soft_buffer: 30.0 geofence_timeout: 5.0 - - graph_type: "voronoi" \ No newline at end of file diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py index f471419a3..c6c593144 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py @@ -15,8 +15,7 @@ from geometry_msgs.msg import PointStamped from smarc_utilities import georef_utils import tf_transformations -from evolo_msgs.msg import Topics as evoloTopics -from smarc_msgs.msg import Topics as SmarcTopics + from smarc_msgs.msg import GeofencePolygonsStamped from sensor_msgs.msg import NavSatFix from smarc_msgs.msg import Topics as SmarcTopics @@ -47,18 +46,23 @@ def __init__(self): # ── Island buffer publishers ────────────────────────────────────────── # Soft buffer (white) — traversable, used as Dijkstra node source - self.island_soft_pub = self.create_publisher(MarkerArray, 'rviz/island_buffer_soft', 10) + self.island_soft_pub = self.create_publisher( + MarkerArray, 'rviz/island_buffer_soft', 10) # Hard buffer (orange) — absolute exclusion zone - self.island_hard_pub = self.create_publisher(MarkerArray, 'rviz/island_buffer_hard', 10) + self.island_hard_pub = self.create_publisher( + MarkerArray, 'rviz/island_buffer_hard', 10) # ── Subscribers ─────────────────────────────────────────────────────── - self.gps_sub = self.create_subscription(NavSatFix, '/evolo/Lidar/gps', self._gps_callback, 10) - # self.polygons_sub = self.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons',self._geofence_polygons_callback, 10) - self.polygons_sub = self.create_subscription(GeofencePolygonsStamped, SmarcTopics.GEOFENCE_POLYGONS_TOPIC, self._geofence_polygons_callback, 10,) - # self.odom_sub = self.create_subscription(Odometry, 'evolo/smarc/odom', self._odom_callback, 10) - self.odom_sub = self.create_subscription(Odometry, SmarcTopics.ODOM_TOPIC, self._odom_callback, 10) - - self._geofence_start_client = RosActionClient(self, BaseAction, 'smarc_start_geofence') + self.gps_sub = self.create_subscription( + NavSatFix, '/evolo/Lidar/gps', self._gps_callback, 10) + self.polygons_sub = self.create_subscription( + GeofencePolygonsStamped, '/smarc/geofence_polygons', + self._geofence_polygons_callback, 10) + self.odom_sub = self.create_subscription( + Odometry, 'evolo/smarc/odom', self._odom_callback, 10) + + self._geofence_start_client = RosActionClient( + self, BaseAction, 'smarc_start_geofence') self.robot_path_msg = Path() self.robot_path_msg.header.frame_id = self.frame_id diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py index e134354ce..f97284481 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py @@ -1,8 +1,6 @@ import rclpy import math import json -import threading -import heapq from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup @@ -14,19 +12,22 @@ from tf_transformations import euler_from_quaternion from rclpy.time import Duration, Time from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import TwistStamped, PoseStamped, Quaternion, Point +from geometry_msgs.msg import Twist, TwistStamped, PoseStamped, Quaternion from tf2_ros import Buffer, TransformListener from smarc_utilities import georef_utils import tf_transformations + from evolo_msgs.msg import Topics as evoloTopics -from smarc_msgs.msg import Topics as SmarcTopics +from smarc_msgs.msg import Topics as smarcTopics +from smarc_control_msgs.msg import Topics as controlTopics + from dubins_planner.dubins import Waypoint, calc_dubins_path, dubins_traj from smarc_msgs.msg import GeofencePolygonsStamped from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point + -from shapely.geometry import Polygon, MultiPolygon, LineString, Point as SPoint -from shapely.ops import unary_union # ───────────────────────────────────────────────────────────────────────────── @@ -99,7 +100,6 @@ def compute(self, robot_x, robot_y, robot_yaw, robot_v, path, cursor, dt): return omega_deg, lookahead_idx - # ───────────────────────────────────────────────────────────────────────────── # Action # ───────────────────────────────────────────────────────────────────────────── @@ -124,33 +124,21 @@ def __init__(self, node: Node, action_name: str): ('dubins_step', rclpy.Parameter.Type.DOUBLE), ('timeout', rclpy.Parameter.Type.DOUBLE), ('frame_id', rclpy.Parameter.Type.STRING), - ('cte_kp', rclpy.Parameter.Type.DOUBLE), + ('cte_kp', rclpy.Parameter.Type.DOUBLE), # proportional CTE gain ('cte_ki', rclpy.Parameter.Type.DOUBLE), ('cte_integral_max', rclpy.Parameter.Type.DOUBLE), - ('heading_kp', rclpy.Parameter.Type.DOUBLE), + ('heading_kp', rclpy.Parameter.Type.DOUBLE), # heading alignment gain ('dubins_mode', rclpy.Parameter.Type.STRING), - ('hard_buffer', rclpy.Parameter.Type.DOUBLE), - ('soft_buffer', rclpy.Parameter.Type.DOUBLE), - ('geofence_timeout', rclpy.Parameter.Type.DOUBLE), - ('speed_slow', rclpy.Parameter.Type.DOUBLE), - ('speed_standard', rclpy.Parameter.Type.DOUBLE), - ('speed_high', rclpy.Parameter.Type.DOUBLE), ]) - self.V_MIN = self._node.get_parameter('v_min').value - self.V_MAX = self._node.get_parameter('v_max').value - self.OMEGA_MAX = self._node.get_parameter('omega_max').value - self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value - self.DUBINS_STEP = self._node.get_parameter('dubins_step').value - self.DUBINS_MODE = self._node.get_parameter('dubins_mode').value - self.timeout = self._node.get_parameter('timeout').value - self.frame_id = self._node.get_parameter('frame_id').value - self.HARD_BUFFER = self._node.get_parameter('hard_buffer').value - self.SOFT_BUFFER = self._node.get_parameter('soft_buffer').value - self.GEOFENCE_TIMEOUT = self._node.get_parameter('geofence_timeout').value - self.SPEED_SLOW = self._node.get_parameter('speed_slow').value - self.SPEED_STANDARD = self._node.get_parameter('speed_standard').value - self.SPEED_HIGH = self._node.get_parameter('speed_high').value + self.V_MIN = self._node.get_parameter('v_min').value + self.V_MAX = self._node.get_parameter('v_max').value + self.OMEGA_MAX = self._node.get_parameter('omega_max').value + self.MIN_TURNING_RADIUS = self._node.get_parameter('min_turning_radius').value + self.DUBINS_STEP = self._node.get_parameter('dubins_step').value + self.DUBINS_MODE = self._node.get_parameter('dubins_mode').value + self.timeout = self._node.get_parameter('timeout').value + self.frame_id = self._node.get_parameter('frame_id').value self.controller = PurePursuitController( Ld_base = self._node.get_parameter('ld_base').value, @@ -174,8 +162,7 @@ def __init__(self, node: Node, action_name: str): ) self._tf_buffer = Buffer() - self._tf_listener = TransformListener(self._tf_buffer, self._node, - spin_thread=True) + self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=True) self.robot_position = PoseStamped() self.robot_position_time = None @@ -183,48 +170,38 @@ def __init__(self, node: Node, action_name: str): self.current_linear_speed = 0.0 self.current_angular_speed = 0.0 - self.target_list = None - self.speed_kn = 10.0 - self.dubins_path = None - self.wp_end_indices = None - self.path_cursor = 0 + self.target_list = None + self.speed_kn = 10.0 + + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 + + self._last_calculated_path = None + self._prev_omega = 0.0 + self.action_started_time = None - self._last_calculated_path = None - self._prev_omega = 0.0 - self.action_started_time = None self._precision_ticks_close = 0 self._precision_ticks_total = 0 self._distance_travelled = 0.0 self._last_robot_pos = None - self._islands_lock = threading.Lock() - self._shapely_hard_zones = None - self._shapely_soft_zones = None - # Unions fusionnées — un seul polygone Shapely pour les checks rapides - self._hard_union_single = None - self._soft_union_single = None - self._contours_hard = [] - self._contours_soft = [] - self._geofence_received_time = None - - self._visibility_graph_data = None - pub_cbg = ReentrantCallbackGroup() sub_cbg = ReentrantCallbackGroup() - self.dubins_path_pub = self._node.create_publisher(Path, 'rviz/planned_path', 10, callback_group=pub_cbg) - # self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/evolo_cmd', 10, callback_group=pub_cbg) - self.viz_island_pub = self._node.create_publisher(MarkerArray, 'rviz/island_contours', 10, callback_group=pub_cbg) + self.dubins_path_pub = self._node.create_publisher(Path, "rviz/planned_path", 10, callback_group=pub_cbg) + self.speed_pub = self._node.create_publisher(TwistStamped, 'evolo/evolo_cmd', 10, callback_group=pub_cbg) + self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10, callback_group=sub_cbg) - self.robot_sub = self._node.create_subscription(Odometry, SmarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10, callback_group=sub_cbg) - self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=pub_cbg) - - # self.robot_sub = self._node.create_subscription(Odometry, 'evolo/smarc/odom', self.robot_odom_callback, 10, callback_group=sub_cbg) - # self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons', self._geofence_polygons_callback, 10, callback_group=sub_cbg) - self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, SmarcTopics.GEOFENCE_POLYGONS_TOPIC, self._geofence_polygons_callback, 10, callback_group=sub_cbg) + # self.robot_sub = self._node.create_subscription(Odometry, smarcTopics.ODOM_TOPIC, self.robot_odom_callback, 10, callback_group=sub_cbg) + # self.speed_pub = self._node.create_publisher(TwistStamped, evoloTopics.EVOLO_TWIST_PLANNED, 10, callback_group=pub_cbg) + # Subscriber geofence polygons + self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, '/smarc/geofence_polygons', self._geofence_polygons_callback, 10,) + # self.polygons_sub = self._node.create_subscription(GeofencePolygonsStamped, smarcTopics.GEOFENCE_POLYGONS_TOPIC, self._geofence_polygons_callback, 10, callback_group=sub_cbg) - self._node.create_timer(2.0, self._publish_island_visuals) - self._node.get_logger().info('EvoloMovePath started') + self._node.get_logger().info("EvoloMovePath started") + + # ───────────────────────────────────────────────────────────────────────── # Geofence callback @@ -269,45 +246,7 @@ def _tr(pt): self._node.get_logger().warn('[Geofence] No valid polygons after TF') return - try: - raw_union = unary_union([Polygon(isl).buffer(0) for isl in islands_local]) - hard_union = raw_union.buffer(self.HARD_BUFFER) - soft_union = raw_union.buffer(self.SOFT_BUFFER) - except Exception as e: - self._node.get_logger().error(f'[Geofence] Shapely buffer: {e}') - return - - def _zones(union): - return list(union.geoms) if union.geom_type == 'MultiPolygon' else [union] - - def _contours(union, tol=1.0): - result = [] - for poly in _zones(union): - coords = list( - poly.simplify(tol, preserve_topology=True).exterior.coords[:-1]) - result.append([(c[0], c[1]) for c in coords]) - return result - geofence_just_arrived = False - with self._islands_lock: - had_zones = self._shapely_hard_zones is not None - self._shapely_hard_zones = _zones(hard_union) - self._shapely_soft_zones = _zones(soft_union) - # Unions fusionnées précalculées — évite de boucler sur les zones à chaque check - self._hard_union_single = hard_union - self._soft_union_single = soft_union - self._contours_hard = _contours(hard_union) - self._contours_soft = _contours(soft_union) - self._geofence_received_time = int( - self._node.get_clock().now().nanoseconds * 1e-9) - if not had_zones: - geofence_just_arrived = True - - self._node.get_logger().info( - f'[Geofence] {len(islands_local)} island(s) | ' - f'hard={self.HARD_BUFFER}m ({len(self._shapely_hard_zones)} poly) | ' - f'soft={self.SOFT_BUFFER}m ({len(self._shapely_soft_zones)} poly)' - ) if geofence_just_arrived and self.dubins_path is not None: self._node.get_logger().warn( @@ -317,448 +256,9 @@ def _contours(union, tol=1.0): self.path_cursor = 0 self.controller.reset() - # ───────────────────────────────────────────────────────────────────────── - # Shapely helpers — toujours sur l'union fusionnée (plus rapide) - # ───────────────────────────────────────────────────────────────────────── - def _point_in_hard(self, x, y): - with self._islands_lock: - u = self._hard_union_single - return u is not None and u.contains(SPoint(x, y)) - - def _point_in_soft(self, x, y): - with self._islands_lock: - u = self._soft_union_single - return u is not None and u.contains(SPoint(x, y)) - - def _line_of_sight_hard(self, ax, ay, bx, by): - """True quand le segment ne pénètre pas la HARD zone.""" - with self._islands_lock: - u = self._hard_union_single - if u is None: - return True - seg = LineString([(ax, ay), (bx, by)]) - return not u.intersects(seg) - - def _segment_collides_soft(self, ax, ay, bx, by): - """True quand le segment touche la SOFT zone (déclencheur bypass).""" - with self._islands_lock: - u = self._soft_union_single - if u is None: - return False - seg = LineString([(ax, ay), (bx, by)]) - return u.intersects(seg) - - # ───────────────────────────────────────────────────────────────────────── - # RViz visualisation - # ───────────────────────────────────────────────────────────────────────── - def _publish_island_visuals(self): - with self._islands_lock: - contours_soft = list(self._contours_soft) - contours_hard = list(self._contours_hard) - if not contours_soft and not contours_hard: - return - - ma = MarkerArray() - stamp = self._node.get_clock().now().to_msg() - mid = 0 - - def _add(contour, ns_dots, ns_line, r, g, b, dot_sz, line_sz): - nonlocal mid - if len(contour) < 2: - return - pts = [Point(x=float(c[0]), y=float(c[1]), z=0.0) for c in contour] - sphere = Marker() - sphere.header.frame_id = self.frame_id - sphere.header.stamp = stamp - sphere.ns, sphere.id = ns_dots, mid; mid += 1 - sphere.type = Marker.SPHERE_LIST - sphere.action = Marker.ADD - sphere.scale.x = sphere.scale.y = sphere.scale.z = dot_sz - sphere.color.r, sphere.color.g, sphere.color.b, sphere.color.a = r, g, b, 1.0 - sphere.points = pts - ma.markers.append(sphere) - line = Marker() - line.header.frame_id = self.frame_id - line.header.stamp = stamp - line.ns, line.id = ns_line, mid; mid += 1 - line.type = Marker.LINE_STRIP - line.action = Marker.ADD - line.scale.x = line_sz - line.color.r, line.color.g, line.color.b, line.color.a = r, g, b, 0.85 - line.points = pts + [pts[0]] - ma.markers.append(line) - - for c in contours_soft: - _add(c, 'soft_dots', 'soft_lines', 1.0, 1.0, 1.0, 0.8, 0.25) - for c in contours_hard: - _add(c, 'hard_dots', 'hard_lines', 1.0, 0.5, 0.0, 0.6, 0.30) - self.viz_island_pub.publish(ma) - - # ───────────────────────────────────────────────────────────────────────── - # Dijkstra - # ───────────────────────────────────────────────────────────────────────── - def _dijkstra(self, graph, start, end): - queue = [(0.0, start, [start])] - visited = set() - while queue: - cost, u, path = heapq.heappop(queue) - if u in visited: - continue - visited.add(u) - if u == end: - return path - for w, v in graph.get(u, []): - if v not in visited: - heapq.heappush(queue, (cost + w, v, path + [v])) - return None - - def _project_outside_soft(self, xy, soft_zones): - p = SPoint(xy[0], xy[1]) - for zone in soft_zones: - if zone.contains(p): - nearest = zone.exterior.interpolate(zone.exterior.project(p)) - cx, cy = zone.centroid.x, zone.centroid.y - dx, dy = nearest.x - cx, nearest.y - cy - d = math.hypot(dx, dy) or 1.0 - return (nearest.x + (dx / d) * 0.5, - nearest.y + (dy / d) * 0.5) - return xy - - # ───────────────────────────────────────────────────────────────────────── - # Bypass Dijkstra — graphe de visibilité optimisé - # ───────────────────────────────────────────────────────────────────────── - def _bypass_segment(self, start_xy, end_xy): - """ - Graphe de visibilité Dijkstra sur les vertices du contour soft. - Arêtes vérifiées contre la HARD zone uniquement. - Filtrage spatial : ne considère que les nœuds proches du segment. - Retourne une liste de (x,y) se terminant par end_xy. - """ - with self._islands_lock: - hard_union = self._hard_union_single - soft_zones = list(self._shapely_soft_zones) if self._shapely_soft_zones else [] - - if not soft_zones: - return [end_xy] - - safe_start = self._project_outside_soft(start_xy, soft_zones) - safe_end = self._project_outside_soft(end_xy, soft_zones) - - # ── Candidats : vertices du contour soft poussés vers l'extérieur ──── - push_dist = 1.0 - edge_subdivisions = 3 - candidate_pts = [] - - for zone in soft_zones: - cx, cy = zone.centroid.x, zone.centroid.y - coords = list( - zone.simplify(0.5, preserve_topology=True).exterior.coords[:-1]) - n_coords = len(coords) - - for idx in range(n_coords): - vx0, vy0 = coords[idx] - vx1, vy1 = coords[(idx + 1) % n_coords] - - for k in range(edge_subdivisions + 1): - t = k / edge_subdivisions - vx = vx0 + t * (vx1 - vx0) - vy = vy0 + t * (vy1 - vy0) - ddx, ddy = vx - cx, vy - cy - d = math.hypot(ddx, ddy) - if d < 1e-6: - continue - nx_u, ny_u = ddx / d, ddy / d - px = vx + nx_u * push_dist - py = vy + ny_u * push_dist - if hard_union is None or not hard_union.contains(SPoint(px, py)): - candidate_pts.append((px, py)) - - all_nodes = [(float(safe_start[0]), float(safe_start[1])), - (float(safe_end[0]), float(safe_end[1]))] + candidate_pts - - # ── Filtrage spatial : ne garder que les nœuds proches du segment ──── - sx, sy = safe_start - ex, ey = safe_end - seg_len = math.hypot(ex - sx, ey - sy) - margin = max(self.SOFT_BUFFER * 2.0, seg_len * 0.3) - mid_x = (sx + ex) / 2 - mid_y = (sy + ey) / 2 - half_diag = seg_len / 2 + margin - - filtered = [0, 1] # start et end toujours inclus - for i, (nx_c, ny_c) in enumerate(all_nodes[2:], 2): - if math.hypot(nx_c - mid_x, ny_c - mid_y) <= half_diag: - filtered.append(i) - - self._node.get_logger().info( - f' Dijkstra: {len(filtered) - 2}/{len(all_nodes) - 2} candidates ' - f'(spatial filter margin={margin:.1f}m)') - - # ── Construction du graphe sur les nœuds filtrés uniquement ────────── - graph = {i: [] for i in filtered} - for ii, i in enumerate(filtered): - ax, ay = all_nodes[i] - for j in filtered[ii + 1:]: - bx, by = all_nodes[j] - seg = LineString([(ax, ay), (bx, by)]) - if hard_union is None or not hard_union.intersects(seg): - dist = math.hypot(bx - ax, by - ay) - graph[i].append((dist, j)) - graph[j].append((dist, i)) - - route = self._dijkstra(graph, 0, 1) - if not route or len(route) < 2: - self._node.get_logger().warn(' Dijkstra: no path found') - return [end_xy] - - self._node.get_logger().info( - f' Dijkstra: route has {len(route) - 1} segment(s)') - - result = [] - for k, node_id in enumerate(route[1:], 1): - is_last = (k == len(route) - 1) - result.append(end_xy if is_last else all_nodes[node_id]) - return result - - # ───────────────────────────────────────────────────────────────────────── - # Segment-level avoidance - # ───────────────────────────────────────────────────────────────────────── - def _extend_positions_for_avoidance(self, positions): - with self._islands_lock: - has_zones = self._soft_union_single is not None - if not has_zones: - self._node.get_logger().warn( - 'Avoidance: no geofence data — planning without avoidance') - return positions - - extended = [positions[0]] - for i in range(len(positions) - 1): - ax, ay = positions[i] - bx, by = positions[i + 1] - if self._segment_collides_soft(ax, ay, bx, by): - self._node.get_logger().warn( - f' Segment {i}→{i+1} crosses soft buffer ' - f'({ax:.1f},{ay:.1f})→({bx:.1f},{by:.1f}) — running Dijkstra') - bypass = self._bypass_segment((ax, ay), (bx, by)) - extended.extend(bypass) - else: - extended.append((bx, by)) - return extended - - # ───────────────────────────────────────────────────────────────────────── - # Arc validation — lock pris une seule fois, pas dans la boucle - # ───────────────────────────────────────────────────────────────────────── - def _validate_dubins_arc(self, arc, check_step=5): - """True si aucun point échantillonné de l'arc n'est dans la HARD zone.""" - with self._islands_lock: - u = self._hard_union_single - if u is None: - return True - indices = list(range(0, len(arc), check_step)) - if indices[-1] != len(arc) - 1: - indices.append(len(arc) - 1) - for i in indices: - if u.contains(SPoint(arc[i][0], arc[i][1])): - return False - return True - - # ───────────────────────────────────────────────────────────────────────── - # Arc Dubins direct — sans récursion (utilisé par contour walk) - # ───────────────────────────────────────────────────────────────────────── - def _build_arc_direct(self, s1, s2, radius): - """ - Arc Dubins sans aucun fallback récursif. - Retourne une ligne droite uniquement si le Dubins échoue complètement. - Jamais appelé avec depth — ne peut pas boucler. - """ - w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) - w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) - params = calc_dubins_path(w1, w2, radius) - if params: - seg = dubins_traj(params, self.DUBINS_STEP) - seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] - if self._validate_dubins_arc(seg): - return seg - # Ligne droite uniquement ici — jamais depuis _build_arc principal - n = max(2, int(math.hypot(s2[0] - s1[0], s2[1] - s1[1]) / self.DUBINS_STEP)) - return [ - [s1[0] + k / (n - 1) * (s2[0] - s1[0]), - s1[1] + k / (n - 1) * (s2[1] - s1[1]), - math.atan2(s2[1] - s1[1], s2[0] - s1[0])] - for k in range(n) - ] - - # ───────────────────────────────────────────────────────────────────────── - # Contour walk — dernier recours avant ligne droite forcée - # ───────────────────────────────────────────────────────────────────────── - def _contour_walk_fallback(self, s1, s2, radius): - """ - Identifie la zone soft bloquant s1→s2. - Marche le contour dans les deux sens (CW et CCW) depuis le vertex - le plus proche de s1, jusqu'à trouver une ligne de vue vers s2. - Chaîne ensuite des arcs Dubins smooth via _build_arc_direct. - Retourne la liste de points ou None si aucune direction ne fonctionne. - """ - with self._islands_lock: - soft_zones = list(self._shapely_soft_zones) if self._shapely_soft_zones else [] - hard_union = self._hard_union_single - if not soft_zones: - return None - - # Trouver la zone qui bloque le segment s1→s2 - seg_line = LineString([(s1[0], s1[1]), (s2[0], s2[1])]) - blocking_zone = None - for zone in soft_zones: - if zone.intersects(seg_line) or zone.contains(SPoint(s1[0], s1[1])): - blocking_zone = zone - break - if blocking_zone is None: - blocking_zone = min(soft_zones, key=lambda z: z.distance(seg_line)) - - # Vertices du contour poussés vers l'extérieur - push_dist = 1.0 - cx, cy = blocking_zone.centroid.x, blocking_zone.centroid.y - coords = list( - blocking_zone.simplify(0.5, preserve_topology=True).exterior.coords[:-1]) - - vertices = [] - for vx, vy in coords: - ddx, ddy = vx - cx, vy - cy - d = math.hypot(ddx, ddy) - if d < 1e-6: - continue - px = vx + (ddx / d) * push_dist - py = vy + (ddy / d) * push_dist - if hard_union is None or not hard_union.contains(SPoint(px, py)): - vertices.append((px, py)) - - if not vertices: - self._node.get_logger().warn( - ' Contour walk: no valid vertices on blocking zone') - return None - - n = len(vertices) - - # Vertex le plus proche de s1 - start_idx = min(range(n), - key=lambda i: math.hypot(vertices[i][0] - s1[0], - vertices[i][1] - s1[1])) - - # Marcher dans les deux sens jusqu'à avoir LoS vers s2 - def walk(direction): - pts = [] - for step in range(1, n + 1): - idx = (start_idx + direction * step) % n - vx, vy = vertices[idx] - pts.append((vx, vy)) - seg = LineString([(vx, vy), (s2[0], s2[1])]) - if hard_union is None or not hard_union.intersects(seg): - return pts - return None - - cw = walk(+1) - ccw = walk(-1) - - if cw is None and ccw is None: - self._node.get_logger().warn( - " Contour walk: aucune direction n'atteint la LoS vers s2") - return None - - if cw is None: - chosen = ccw - elif ccw is None: - chosen = cw - else: - chosen = cw if len(cw) <= len(ccw) else ccw - - self._node.get_logger().info( - f' Contour walk: {len(chosen)} vertex(es) pour atteindre la LoS') - - # Construire les états Dubins : s1 → vertices → s2 - all_states = [s1] - prev_xy = (s1[0], s1[1]) - for wx, wy in chosen: - h = math.atan2(wy - prev_xy[1], wx - prev_xy[0]) - all_states.append((wx, wy, h)) - prev_xy = (wx, wy) - all_states.append(s2) # heading original de s2 conservé - - # Chaîner les arcs Dubins smooth sans récursion - result = [] - for i in range(len(all_states) - 1): - seg = self._build_arc_direct(all_states[i], all_states[i + 1], radius) - result.extend(seg if i == 0 else seg[1:]) - return result - - # ───────────────────────────────────────────────────────────────────────── - # Arc principal avec fallback en cascade - # ───────────────────────────────────────────────────────────────────────── - def _build_arc(self, s1, s2, radius, depth=0): - """ - Construit un arc Dubins de s1=(x,y,yaw_rad) vers s2. - - Cascade : - depth 0 → arc OK ? retourne - → KO → Dijkstra midpoint (depth 1) - depth 1 → arc OK ? retourne - → KO → Dijkstra midpoint (depth 2) - depth 2 → arc OK ? retourne - → KO → contour walk (arcs Dubins sur vertices contour) - → si contour walk échoue : ligne droite forcée (ERROR) - """ - w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) - w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) - params = calc_dubins_path(w1, w2, radius) - - if params: - seg = dubins_traj(params, self.DUBINS_STEP) - seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] - if self._validate_dubins_arc(seg): - return seg # ✓ arc propre - - # ── Fallback Dijkstra (jusqu'à depth < 3) ──────────────────────────── - if depth < 3: - self._node.get_logger().warn( - f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' - f'hard collision at R={radius:.1f} — ' - f'inserting bypass midpoint (depth={depth})') - bypass = self._bypass_segment((s1[0], s1[1]), (s2[0], s2[1])) - - if len(bypass) > 1: - mid_xy = bypass[0] - h_to_mid = math.atan2(mid_xy[1] - s1[1], mid_xy[0] - s1[0]) - s_mid = (mid_xy[0], mid_xy[1], h_to_mid) - - seg_a = self._build_arc(s1, s_mid, radius, depth=depth + 1) - seg_b = self._build_arc(s_mid, s2, radius, depth=depth + 1) - return seg_a + seg_b[1:] - - # ── Last resort : contour walk smooth ──────────────────────────────── - self._node.get_logger().warn( - f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' - f'Dijkstra exhausted — contour walk fallback') - result = self._contour_walk_fallback(s1, s2, radius) - if result: - return result - - # Absolument dernier recours — ne devrait jamais arriver - self._node.get_logger().error( - f' Arc ({s1[0]:.1f},{s1[1]:.1f})→({s2[0]:.1f},{s2[1]:.1f}): ' - f'contour walk failed — forced straight line') - n_interp = max(2, int( - math.hypot(s2[0] - s1[0], s2[1] - s1[1]) / self.DUBINS_STEP)) - return [ - [s1[0] + k / (n_interp - 1) * (s2[0] - s1[0]), - s1[1] + k / (n_interp - 1) * (s2[1] - s1[1]), - math.atan2(s2[1] - s1[1], s2[0] - s1[0])] - for k in range(n_interp) - ] - - # ───────────────────────────────────────────────────────────────────────── - # Goal / Cancel / Prepare - # ───────────────────────────────────────────────────────────────────────── - def _on_goal_received(self, goal_request): + # ── Goal / Cancel ───────────────────────────────────────────────────────── + def _on_goal_received(self, goal_request: dict) -> bool: try: self.speed_kn = float(goal_request['speed']) except (ValueError, TypeError): @@ -781,8 +281,9 @@ def _on_goal_received(self, goal_request): return False self.target_list.append(self.WP(p=pose, tol=tol, speed_kn=self.speed_kn)) self._node.get_logger().info( - f' WP{len(self.target_list)}: ' - f'({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f})') + f" WP{len(self.target_list)}: " + f"({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f})" + ) self._waypoints_for_client = [ {'x': wp.p.pose.position.x, 'y': wp.p.pose.position.y, 'tol': wp.tol} @@ -790,11 +291,11 @@ def _on_goal_received(self, goal_request): ] return True - def _on_cancel_received(self): + def _on_cancel_received(self) -> bool: self._send_stop() return True - def _prepare_loop(self): + def _prepare_loop(self) -> None: self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) self.dubins_path = None self.wp_end_indices = None @@ -806,10 +307,8 @@ def _prepare_loop(self): self._prev_omega = 0.0 self.controller.reset() - # ───────────────────────────────────────────────────────────────────────── - # Main control loop - # ───────────────────────────────────────────────────────────────────────── - def _loop_inner(self): + # ── Main loop ───────────────────────────────────────────────────────────── + def _loop_inner(self) -> bool | None: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) if time_now - self.action_started_time > self.timeout: self._send_stop() @@ -818,57 +317,52 @@ def _loop_inner(self): if self.robot_position_time is None or self.current_yaw is None: return None - if self.dubins_path is None: - with self._islands_lock: - has_geofence = self._soft_union_single is not None - - if not has_geofence: - elapsed = time_now - self.action_started_time - if elapsed < self.GEOFENCE_TIMEOUT: - if int(elapsed) % 2 == 0: - self._node.get_logger().info( - f'Waiting for geofence… ({elapsed:.1f}s / ' - f'{self.GEOFENCE_TIMEOUT}s timeout)') - return None - else: - self._node.get_logger().warn( - f'Geofence timeout ({self.GEOFENCE_TIMEOUT}s) — ' - 'planning without avoidance') - - if not self._plan_global_dubins(): - return None - robot_pos = self.robot_position.pose.position + # ── Distance travelled ──────────────────────────────────────────────── if self._last_robot_pos is not None: self._distance_travelled += math.hypot( robot_pos.x - self._last_robot_pos[0], - robot_pos.y - self._last_robot_pos[1]) + robot_pos.y - self._last_robot_pos[1], + ) self._last_robot_pos = (robot_pos.x, robot_pos.y) + # ── Plan once ───────────────────────────────────────────────────────── + if self.dubins_path is None: + if not self._plan_global_dubins(): + return None + path = self.dubins_path + # ── Cursor: small fixed window to prevent cross-loop jumps ──────────── + # At v=14 m/s and 10 Hz → ~1.4 m/tick ≈ 3 pts/tick (step=0.5 m). + # A 40-point window (20 m) is wide enough to advance but too narrow + # to accidentally skip an entire Dubins arc. WINDOW = 40 search_end = min(len(path), self.path_cursor + WINDOW) candidate = self._find_closest(robot_pos, self.path_cursor, search_end) self.path_cursor = max(self.path_cursor, candidate) self.path_cursor = min(self.path_cursor, len(path) - 1) + # ── End of path ─────────────────────────────────────────────────────── if self.path_cursor >= len(path) - 1: - self._node.get_logger().info('End of Dubins path reached') + self._node.get_logger().info("End of Dubins path reached") self._send_stop() return True + # ── Precision metric (lateral distance to path) ─────────────────────── cx, cy, cyaw = path[self.path_cursor] - dist_to_curve = abs( - math.cos(cyaw) * (robot_pos.y - cy) - - math.sin(cyaw) * (robot_pos.x - cx)) + dx = robot_pos.x - cx + dy = robot_pos.y - cy + dist_to_curve = abs(math.cos(cyaw) * dy - math.sin(cyaw) * dx) self._precision_ticks_total += 1 if dist_to_curve < 1.0: self._precision_ticks_close += 1 + # ── Speed ───────────────────────────────────────────────────────────── v = max(self.V_MIN, min(self.V_MAX, self.speed_kn)) + # ── Controller ──────────────────────────────────────────────────────── omega, _ = self.controller.compute( robot_x = float(robot_pos.x), robot_y = float(robot_pos.y), @@ -879,11 +373,12 @@ def _loop_inner(self): dt = 0.1, ) - MAX_DELTA = 4.0 - omega_smoothed = (self._prev_omega - + max(-MAX_DELTA, min(MAX_DELTA, omega - self._prev_omega))) + # ── Slew-rate limiter (prevents abrupt rudder jumps) ────────────────── + MAX_DELTA = 4.0 # deg/tick + omega_smoothed = self._prev_omega + max(-MAX_DELTA, min(MAX_DELTA, omega - self._prev_omega)) self._prev_omega = omega_smoothed + # ── Publish ─────────────────────────────────────────────────────────── cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() cmd.header.frame_id = self.frame_id @@ -893,131 +388,94 @@ def _loop_inner(self): return None - # ───────────────────────────────────────────────────────────────────────── - # Global Dubins planner - # ───────────────────────────────────────────────────────────────────────── - def _plan_global_dubins(self): + # ── Dubins global planner ───────────────────────────────────────────────── + def _plan_global_dubins(self) -> bool: if self.current_yaw is None: return False robot_pos = self.robot_position.pose.position + full_path, wp_ends = [], [] - # Step 1: positions brutes - raw_positions = [(robot_pos.x, robot_pos.y)] + # Build the flat list of (x, y) positions: robot + all target waypoints + positions = [(robot_pos.x, robot_pos.y)] for wp in self.target_list: - raw_positions.append( - (wp.p.pose.position.x, wp.p.pose.position.y)) - - self._node.get_logger().info( - f'Planning | start=({robot_pos.x:.1f},{robot_pos.y:.1f},' - f'{math.degrees(self.current_yaw):.0f}°) | ' - f'{len(self.target_list)} WP(s)') - - - # Step 2: insertion des bypass waypoints Dijkstra - # On mémorise quels indices dans `positions` sont des WP réels - with self._islands_lock: - has_geofence = self._soft_union_single is not None - - if has_geofence: - positions = self._extend_positions_for_avoidance(raw_positions) - else: - self._node.get_logger().warn('Planning without avoidance (geofence timeout)') - positions = raw_positions - - # Indices dans `positions` qui correspondent à un vrai WP utilisateur - # raw_positions[0] = robot, raw_positions[1..] = WPs réels - # _extend_positions_for_avoidance conserve les points originaux en dernier - # de chaque segment, donc on les retrouve en reconstruisant le mapping - real_wp_positions = set() - raw_idx = 0 - pos_idx = 0 - while pos_idx < len(positions) and raw_idx < len(raw_positions): - if (abs(positions[pos_idx][0] - raw_positions[raw_idx][0]) < 1e-3 and - abs(positions[pos_idx][1] - raw_positions[raw_idx][1]) < 1e-3): - if raw_idx > 0: # ne pas marquer le robot lui-même - real_wp_positions.add(pos_idx) - raw_idx += 1 - pos_idx += 1 - else: - pos_idx += 1 # bypass waypoint, on saute + positions.append((wp.p.pose.position.x, wp.p.pose.position.y)) - n_bypass = len(positions) - len(raw_positions) - if n_bypass: - self._node.get_logger().info( - f' Avoidance: {n_bypass} bypass waypoint(s) inserted') + R = self.MIN_TURNING_RADIUS + n = len(positions) - # Step 3: construction des états Dubins depuis les positions - n = len(positions) - R = self.MIN_TURNING_RADIUS + # Build the list of Dubins states (x, y, heading) states = [(positions[0][0], positions[0][1], self.current_yaw)] real_wp_state_indices = [] for i in range(1, n): - curr = positions[i] - prev = positions[i - 1] - is_real_wp = i in real_wp_positions - is_last = (i == n - 1) + curr = positions[i] + prev = positions[i - 1] - if self.DUBINS_MODE == 'vwp' and is_real_wp and not is_last: - # VWP uniquement sur les vrais waypoints intermédiaires + if self.DUBINS_MODE == 'vwp': + # Heading arriving at the current waypoint h_in = math.atan2(curr[1] - prev[1], curr[0] - prev[0]) - vwp = (curr[0] + R * math.cos(h_in), - curr[1] + R * math.sin(h_in)) - nxt = positions[i + 1] - h_next = math.atan2(nxt[1] - curr[1], nxt[0] - curr[0]) - next_vwp = (nxt[0] + R * math.cos(h_next), - nxt[1] + R * math.sin(h_next)) - h_exit = math.atan2(next_vwp[1] - curr[1], - next_vwp[0] - curr[0]) - states.append((vwp[0], vwp[1], h_in)) - states.append((curr[0], curr[1], h_exit)) - else: - # Bypass waypoints et dernier WP : cap interpolé simple - h = (math.atan2(positions[i + 1][1] - prev[1], - positions[i + 1][0] - prev[0]) - if not is_last - else math.atan2(curr[1] - prev[1], curr[0] - prev[0])) + + if i == n - 1: + # ── Last waypoint ───────────────────────────────────────── + # Stop exactly here; do NOT add a virtual exit waypoint. + # The old code used h_exit = h_in + π which forced Dubins + # to generate an unnecessary U-turn loop at the end. + states.append((curr[0], curr[1], h_in)) + else: + # ── Intermediate waypoint ───────────────────────────────── + # Place a virtual waypoint (vwp) one turning-radius ahead + # in the incoming direction to ensure the robot passes + # through the real waypoint smoothly. + vwp = (curr[0] + R * math.cos(h_in), + curr[1] + R * math.sin(h_in)) + nxt = positions[i + 1] + h_next = math.atan2(nxt[1] - curr[1], nxt[0] - curr[0]) + next_vwp = (nxt[0] + R * math.cos(h_next), + nxt[1] + R * math.sin(h_next)) + h_exit = math.atan2(next_vwp[1] - curr[1], next_vwp[0] - curr[0]) + states.append((vwp[0], vwp[1], h_in)) + states.append((curr[0], curr[1], h_exit)) + + else: # smooth mode + h = (math.atan2(positions[i+1][1] - prev[1], positions[i+1][0] - prev[0]) + if i < n - 1 + else math.atan2(curr[1] - prev[1], curr[0] - prev[0])) states.append((curr[0], curr[1], h)) real_wp_state_indices.append(len(states) - 1) - - - - - # Step 4: chaîner les arcs avec _build_arc (cascade de fallbacks) - full_path, wp_ends = [], [] - + # Generate Dubins segments between consecutive states for i in range(len(states) - 1): - seg = self._build_arc(states[i], states[i + 1], R) - full_path.extend(seg if i == 0 else seg[1:]) + s1, s2 = states[i], states[i + 1] + w1 = Waypoint(s1[0], s1[1], math.degrees(s1[2])) + w2 = Waypoint(s2[0], s2[1], math.degrees(s2[2])) + params = calc_dubins_path(w1, w2, R) + if params: + seg = dubins_traj(params, self.DUBINS_STEP) + seg = [pt.tolist() if hasattr(pt, 'tolist') else list(pt) for pt in seg] + full_path.extend(seg if i == 0 else seg[1:]) + else: + full_path.append(list(s2)) + if (i + 1) in real_wp_state_indices: wp_ends.append(len(full_path) - 1) - # Step 5: publication et stockage self.dubins_path_pub.publish(self._path_msg(full_path)) self.dubins_path = full_path self.wp_end_indices = wp_ends self.path_cursor = 0 self._last_calculated_path = full_path - - with self._islands_lock: - self._visibility_graph_data = { - 'island_contours_soft': list(self._contours_soft), - 'island_contours_hard': list(self._contours_hard), - } - - self._node.get_logger().info( - f'✓ Dubins path: {len(full_path)} pts | ' - f'{len(wp_ends)} WP end(s) | ' - f'{n_bypass} bypass pt(s)') + self._node.get_logger().info(f"Dubins path planned: {len(full_path)} points") return True - # ───────────────────────────────────────────────────────────────────────── - # Cursor helper - # ───────────────────────────────────────────────────────────────────────── - def _find_closest(self, robot_pos, start, end): + # ── Cursor helper ───────────────────────────────────────────────────────── + def _find_closest(self, robot_pos, start: int, end: int) -> int: + """ + Score = distance + heading penalty. + No regressive term — higher indices are never penalised. + The fixed window prevents cross-loop jumps. + """ path = self.dubins_path yaw = self.current_yaw or 0.0 best_idx = start @@ -1033,9 +491,7 @@ def _find_closest(self, robot_pos, start, end): best_idx = i return best_idx - # ───────────────────────────────────────────────────────────────────────── - # Utilities - # ───────────────────────────────────────────────────────────────────────── + # ── Utilities ───────────────────────────────────────────────────────────── def _send_stop(self): cmd = TwistStamped() cmd.header.stamp = self._node.get_clock().now().to_msg() @@ -1043,7 +499,7 @@ def _send_stop(self): cmd.twist.angular.z = 0.0 self.speed_pub.publish(cmd) - def _path_msg(self, configurations): + def _path_msg(self, configurations) -> Path: msg = Path() msg.header.frame_id = self.frame_id msg.header.stamp = self._node.get_clock().now().to_msg() @@ -1060,31 +516,28 @@ def _path_msg(self, configurations): msg.poses.append(ps) return msg - def _give_feedback(self): + def _give_feedback(self) -> str: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) runtime = time_now - self.action_started_time pct = (round(100.0 * self._precision_ticks_close / self._precision_ticks_total, 2) if self._precision_ticks_total > 0 else 0.0) self._node.get_logger().info( - f'precision={pct}% | dist={self._distance_travelled:.1f}m' - f' | cursor={self.path_cursor}/' - f'{len(self.dubins_path) if self.dubins_path else "?"}') + f"precision={pct}% | dist={self._distance_travelled:.1f}m" + f" | cursor={self.path_cursor}/{len(self.dubins_path) if self.dubins_path else '?'}" + ) fb = { - 'runtime': runtime, - 'precision_pct': pct, - 'precision_close': self._precision_ticks_close, - 'precision_total': self._precision_ticks_total, - 'distance_travelled': round(self._distance_travelled, 2), + "runtime": runtime, + "precision_pct": pct, + "precision_close": self._precision_ticks_close, + "precision_total": self._precision_ticks_total, + "distance_travelled": round(self._distance_travelled, 2), } if self._last_calculated_path is not None: - fb['full_path'] = self._last_calculated_path + fb["full_path"] = self._last_calculated_path self._last_calculated_path = None if hasattr(self, '_waypoints_for_client') and self._waypoints_for_client: fb['wps'] = self._waypoints_for_client self._waypoints_for_client = None - if self._visibility_graph_data is not None: - fb['visibility_graph'] = self._visibility_graph_data - self._visibility_graph_data = None return json.dumps(fb) def latlon_to_local_frame(self, point_list): @@ -1104,10 +557,11 @@ def latlon_to_local_frame(self, point_list): target_frame=self.frame_id, source_frame=ps.header.frame_id, time=Time(seconds=0), - timeout=Duration(seconds=1)) + timeout=Duration(seconds=1), + ) return do_transform_pose_stamped(ps, t) except Exception as e: - self._node.get_logger().error(f'TF failed: {e}') + self._node.get_logger().error(f"TF failed: {e}") return None def robot_odom_callback(self, msg: Odometry): @@ -1124,35 +578,34 @@ def robot_odom_callback(self, msg: Odometry): target_frame=self.frame_id, source_frame=msg.header.frame_id, time=Time(seconds=0), - timeout=Duration(seconds=1)) + timeout=Duration(seconds=1), + ) self.robot_position = do_transform_pose_stamped(raw, t) except Exception as e: - self._node.get_logger().error(f'Odom TF failed: {e}') + self._node.get_logger().error(f"Odom TF failed: {e}") return - self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) + self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) oq = self.robot_position.pose.orientation - (_, _, self.current_yaw) = euler_from_quaternion([oq.x, oq.y, oq.z, oq.w]) + (_, _, self.current_yaw) = euler_from_quaternion([oq.x, oq.y, oq.z, oq.w]) self.current_linear_speed = msg.twist.twist.linear.x self.current_angular_speed = msg.twist.twist.angular.z -# ───────────────────────────────────────────────────────────────────────────── def main(): rclpy.init() - node = Node('evolo_move_path_action_server') - EvoloMovePath(node, 'move_path') + node = Node("evolo_move_path_action_server") + EvoloMovePath(node, "move_path") executor = MultiThreadedExecutor() executor.add_node(node) try: executor.spin() except KeyboardInterrupt: - node.get_logger().info('Shutting down') + node.get_logger().info("Shutting down") finally: executor.shutdown() node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() \ No newline at end of file