diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml index 74380a40..2afa73a5 100644 --- a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -1,37 +1,30 @@ /**/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 - - # Speed Map - speed_map_slow: 8.0 - speed_map_medium: 11.0 - speed_map_high: 14.0 - - - # Graph - graph_type: "voronoi" # 'visibility' ou 'voronoi' - + # Cross-track error + cte_kp: 3.0 + cte_ki: 0.05 + cte_integral_max: 10.0 + heading_kp: 8.0 + hard_buffer: 10.0 + soft_buffer: 30.0 + geofence_timeout: 5.0 - # Lateral_normal - kp_lateral: 0.3 - kp_heading: 0.8 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 5c2e4c72..c6c59314 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 e0502ec0..f9728448 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,109 @@ 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) - """ - - 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 + 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): Ld = self.Ld_base + self.Ld_gain * robot_v - lookahead_idx = cursor + 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 + kappa = 0.0 if dist_to_target < 0.1 else 2.0 * math.sin(alpha) / dist_to_target + + omega_pp = robot_v * kappa + + 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 + nx = -dy / seg_len + ny = dx / seg_len + cross_error = (robot_x - foot_x) * nx + (robot_y - foot_y) * ny + + self._cte_integral = max( + -self.cte_integral_max, + min(self.cte_integral_max, self._cte_integral + cross_error * dt) + ) + omega_cte_deg = -(self.cte_kp * cross_error + self.cte_ki * self._cte_integral) - omega_rad = robot_v * kappa + ff_omega_rad - omega_deg = math.degrees(omega_rad) + _, _, path_yaw = path[cursor] + heading_error = math.atan2(math.sin(path_yaw - robot_yaw), + math.cos(path_yaw - robot_yaw)) + omega_heading_deg = self.heading_kp * heading_error + 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 - # ───────────────────────────────────────────────────────────────────────────── -# Action +# Action # ───────────────────────────────────────────────────────────────────────────── -class EvoloMovePath(): +class EvoloMovePath: class WP: def __init__(self, p, tol, speed_kn): @@ -133,42 +114,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 +158,116 @@ 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=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.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 + + geofence_just_arrived = False + + 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() + + + # ── 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 +279,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 +296,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 - 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 + # ── Speed ───────────────────────────────────────────────────────────── + v = max(self.V_MIN, min(self.V_MAX, self.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) - + # ── 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 - ################################################################################################### - - # 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 - - - 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 +516,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 +565,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 +591,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 07d6825a..8886adf7 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