From d03e343f0a941a1ac5c263e6d5692442e7d71a35 Mon Sep 17 00:00:00 2001 From: Baptiste Date: Fri, 13 Feb 2026 16:15:37 +0100 Subject: [PATCH 1/4] first version of path planning --- .../evolo_move_path/evolo_move_path/client.py | 101 +++ .../evolo_move_path_server_potential_field.py | 622 ++++++++++++++++++ behaviours/evolo/evolo_move_path/setup.py | 6 +- 3 files changed, 728 insertions(+), 1 deletion(-) create mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/client.py create mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py new file mode 100644 index 000000000..fc60ac044 --- /dev/null +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py @@ -0,0 +1,101 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from smarc_msgs.action import BaseAction +import json + +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point + +class EvoloMovePathClient(Node): + def __init__(self): + super().__init__('evolo_move_path_client') + self._action_client = ActionClient(self, BaseAction, 'move_path') + self.marker_pub = self.create_publisher(MarkerArray, '/waypoints_viz', 10) + + def publish_waypoints(self, waypoint_list): + marker_array = MarkerArray() + for i, pt in enumerate(waypoint_list): + marker = Marker() + marker.header.frame_id = "evolo/odom" + 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 send_goal(self): + 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") + return + + goal_msg = BaseAction.Goal() + + from std_msgs.msg import String + + payload = { + 'speed': 'high', + 'waypoints': [ + {'latitude': 58.8397422670, 'longitude': 17.6534623045, 'tolerance': 5.0}, + {'latitude': 58.8400922670, 'longitude': 17.6540122932, 'tolerance': 5.0}, + {'latitude': 58.8403922670, 'longitude': 17.6533123075, 'tolerance': 5.0}, + {'latitude': 58.8398922670, 'longitude': 17.6528123177, 'tolerance': 5.0}, + {'latitude': 58.8397922670, 'longitude': 17.6543122871, 'tolerance': 5.0} + ], + 'obstacles': [ + # {'latitude': 58.8399222670, 'longitude': 17.6537422987, 'radius': 8.0}, + # {'latitude': 58.8402422670, 'longitude': 17.6536623004, 'radius': 10.0}, + # {'latitude': 58.8399422670, 'longitude': 17.6530123137, 'radius': 6.0} + ] + } + + goal_msg.goal = String() + goal_msg.goal.data = json.dumps(payload) + + self.get_logger().info("Send mission with obstacles...") + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Mission rejected') + return + self.get_logger().info('Mission accepted') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback_msg): + self.get_logger().info(f"Feedback : {feedback_msg.feedback}") + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info(f"Final Result") + rclpy.shutdown() + +def main(args=None): + rclpy.init(args=args) + client = EvoloMovePathClient() + client.send_goal() + rclpy.spin(client) + +if __name__ == '__main__': + main() + + + + + + diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py new file mode 100644 index 000000000..a46ed7690 --- /dev/null +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py @@ -0,0 +1,622 @@ +import rclpy + +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 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 + +import tf_transformations + +from enum import Enum + + + +class Attractor: + def __init__(self, x, w): + self.x = np.array(x, dtype=float) + self.w = w + + +class Repulsor: + def __init__(self, x, w): + self.x = np.array(x, dtype=float) + self.w = w + + + + +class EvoloMovePath(): + + class WP: + def __init__(self, p : PoseStamped, tol : float, speed : str): + self.p = p + self.tol = tol + self.speed = speed + + def __init__(self, + node: Node, + action_name: str): + self._node = node + + # Initialize the action server with the node and action name + # Give it all the necessary callbacks + self._as = GentlerActionServer( + node, + action_name, + self._on_goal_received, + self._on_cancel_received, + self._prepare_loop, + self._loop_inner, + self._give_feedback, + loop_frequency=2 + ) + + # Initialize any necessary state for your specific action + # These have nothing to do with the action server itself + + # Tf listener + self._tf_buffer = Buffer() + self._tf_listener = TransformListener( + self._tf_buffer, self._node, spin_thread=True + ) + + # State variables. gets updated from topic callbacks + self.robot_position = PoseStamped() #robot positon [geometry_msgs/msg/Pose] + self.robot_position_time = None #robot position time to be compared with current time + + self.distance_to_target = None + + self.target_index = None + self.target_list = None #self.WP + + self.poses_history = [] # for the path + self.obstacles = [] + + #Target frame + #self.frame_id = 'map_gt' + self.frame_id = 'evolo/odom' + + #Settings etc + self.timeout = 1800.0 + + self.current_yaw = 0.0 + + #Time of action start to check for timeout + self.action_started_time = None + + #Callback groups + self.publisher_callback_group = ReentrantCallbackGroup() + self.subscriber_callback_group = ReentrantCallbackGroup() + + # Publishers + self.evolo_pub = self._node.create_publisher(Float32, controlTopics.CONTROL_YAW_TOPIC,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.path_pub = self._node.create_publisher(Path, '/evolo/visual_path', 10, callback_group=self.publisher_callback_group) + self.marker_pub = self._node.create_publisher(Marker, '/evolo/force_markers', 10, callback_group=self.publisher_callback_group) + self.viz_markers_pub = self._node.create_publisher(MarkerArray, '/evolo/visualisation', 10, callback_group=self.publisher_callback_group) + + # Subscribers + self.robot_sub = self._node.create_subscription(Odometry, '/evolo/smarc/odom', self.robot_odom_callback,10, callback_group=self.subscriber_callback_group) + + self._node.get_logger().info("Action server started") + + + + + def _on_goal_received(self, goal_request: dict) -> bool: + self._node.get_logger().info(f"Received goal request: {goal_request}") + + speed = goal_request['speed'] + waypoints = goal_request['waypoints'] + obstacles = goal_request['obstacles'] + self.timeout = 600 + + if len(waypoints) == 0: + self._node.get_logger().info(f"Waypoint list was empty") + return False + + # Reset + self.target_index = 0 + self.target_list = [] + # Waypoints + if 'waypoints' in goal_request: + for wp in waypoints: + self._node.get_logger().info(f"WP: {wp}") + wp_params = wp + + self._node.get_logger().info(f"wp params: {wp_params}") + + lat = float(wp_params['latitude']) + lon = float(wp_params['longitude']) + self._node.get_logger().info(f"lat lon sent to function: {lat}, {lon}") + target_position = self.latlon_to_local_frame([lat,lon]) + target_speed = speed + target_tol = float(wp_params['tolerance']) + self.target_list.append(self.WP(p=target_position, speed = target_speed, tol = target_tol)) + + # Obstacles + self.obstacles = [] + if 'obstacles' in goal_request: + for obs in goal_request['obstacles']: + self._node.get_logger().info(f"Obstacle: {obs}") + lat = float(obs['latitude']) + lon = float(obs['longitude']) + radius = float(obs.get('radius', 5.0)) + + obs_position = self.latlon_to_local_frame([lat, lon]) + obs_x = obs_position.pose.position.x + obs_y = obs_position.pose.position.y + + self.obstacles.append(Repulsor(x=[obs_x, obs_y], w=radius)) + self._node.get_logger().info(f"Obstacle added at ({obs_x:.1f}, {obs_y:.1f})") + + # Publish markers for visualization + self.publish_waypoints_markers() + + return True + + def _on_cancel_received(self) -> bool: + self._node.get_logger().info("Received cancel request") + return True + + def _prepare_loop(self) -> None: + self._node.get_logger().info("Preparing loop for action execution") + self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) + + def compute_force(self, q, s, r): + dq = np.zeros(2) + + # Attractors + for a in s: + diff = a.x - q + d = np.linalg.norm(diff) + if d > 0.01: + dq += a.w * (diff / d) + + # Repulsors + for rep in r: + diff = q - rep.x + d = np.linalg.norm(diff) + + rho_0 = rep.w * 3.0 + + if d < rho_0 and d > 0.01: + strength_factor = 400.0 + + repulsion_magnitude = strength_factor * (1.0/d - 1.0/rho_0) * (1.0/(d**2)) + dq += repulsion_magnitude * (diff / d) + + self._node.get_logger().info( + f"Repulsion: dist={d:.1f}m, force_mag={repulsion_magnitude:.2f}" + ) + + return dq + + def publish_waypoints_markers(self): + marker_array = MarkerArray() + + # WAYPOINTS + for i, wp in enumerate(self.target_list): + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = self._node.get_clock().now().to_msg() + marker.ns = "waypoints" + marker.id = i + marker.type = Marker.SPHERE + marker.action = Marker.ADD + + marker.pose.position.x = wp.p.pose.position.x + marker.pose.position.y = wp.p.pose.position.y + marker.pose.position.z = 0.5 + + marker.scale.x = wp.tol * 2 + marker.scale.y = wp.tol * 2 + marker.scale.z = 1.0 + + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.3 + + marker_array.markers.append(marker) + + text_marker = Marker() + text_marker.header.frame_id = self.frame_id + text_marker.header.stamp = self._node.get_clock().now().to_msg() + text_marker.ns = "waypoint_labels" + text_marker.id = i + 1000 + text_marker.type = Marker.TEXT_VIEW_FACING + text_marker.action = Marker.ADD + + text_marker.pose.position.x = wp.p.pose.position.x + text_marker.pose.position.y = wp.p.pose.position.y + text_marker.pose.position.z = 2.0 + + text_marker.scale.z = 2.0 + + text_marker.color.r = 1.0 + text_marker.color.g = 1.0 + text_marker.color.b = 1.0 + text_marker.color.a = 1.0 + + text_marker.text = f"WP{i+1}" + + marker_array.markers.append(text_marker) + + # OBSTACLES + for i, obs in enumerate(self.obstacles): + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = self._node.get_clock().now().to_msg() + marker.ns = "obstacles" + marker.id = i + 2000 + marker.type = Marker.CYLINDER + marker.action = Marker.ADD + + marker.pose.position.x = obs.x[0] + marker.pose.position.y = obs.x[1] + marker.pose.position.z = 0.5 + + marker.scale.x = obs.w * 2 + marker.scale.y = obs.w * 2 + marker.scale.z = 1.0 + + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 0.5 + + marker_array.markers.append(marker) + + text_marker = Marker() + text_marker.header.frame_id = self.frame_id + text_marker.header.stamp = self._node.get_clock().now().to_msg() + text_marker.ns = "obstacle_labels" + text_marker.id = i + 3000 + text_marker.type = Marker.TEXT_VIEW_FACING + text_marker.action = Marker.ADD + + text_marker.pose.position.x = obs.x[0] + text_marker.pose.position.y = obs.x[1] + text_marker.pose.position.z = 2.0 + + text_marker.scale.z = 1.5 + + text_marker.color.r = 1.0 + text_marker.color.g = 0.0 + text_marker.color.b = 0.0 + text_marker.color.a = 1.0 + + text_marker.text = f"OBS{i+1}\nR={obs.w:.1f}m" + + marker_array.markers.append(text_marker) + + self._node.get_logger().info(f"Publishing {len(marker_array.markers)} markers") + self.viz_markers_pub.publish(marker_array) + + def publish_current_target_marker(self): + if self.target_index >= len(self.target_list): + return + + marker_array = MarkerArray() + current_wp = self.target_list[self.target_index] + + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = self._node.get_clock().now().to_msg() + marker.ns = "current_target" + marker.id = 9999 + marker.type = Marker.SPHERE + marker.action = Marker.ADD + + marker.pose.position.x = current_wp.p.pose.position.x + marker.pose.position.y = current_wp.p.pose.position.y + marker.pose.position.z = 1.5 + + marker.scale.x = 2.0 + marker.scale.y = 2.0 + marker.scale.z = 2.0 + + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.8 + + marker_array.markers.append(marker) + self.viz_markers_pub.publish(marker_array) + + def publish_force_marker(self, robot_pos, force): + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = self._node.get_clock().now().to_msg() + marker.ns = "force_vector" + marker.id = 0 + marker.type = Marker.ARROW + marker.action = Marker.ADD + + start = Point() + start.x = robot_pos.x + start.y = robot_pos.y + start.z = 0.5 + + scale_factor = 5.0 + end = Point() + end.x = robot_pos.x + force[0] * scale_factor + end.y = robot_pos.y + force[1] * scale_factor + end.z = 0.5 + + marker.points = [start, end] + + marker.scale.x = 0.3 + marker.scale.y = 0.5 + marker.scale.z = 0.5 + + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 1.0 + marker.color.a = 1.0 + + self.marker_pub.publish(marker) + + 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: + return False + + if self.robot_position_time is None or (time_now - self.robot_position_time) > 10: + self._node.get_logger().error("ERROR: no robot position") + return False + + if self.target_index >= len(self.target_list): + return True + + # Position data + current_wp = self.target_list[self.target_index] + target_pos = current_wp.p.pose.position + robot_pos = self.robot_position.pose.position + + self.distance_to_target = self.calculate_distance(self.robot_position, current_wp.p) + + # Waypoint reached + if self.distance_to_target < current_wp.tol: + self.target_index += 1 + self._node.get_logger().info(f"Waypoint {self.target_index} reached") + return None if self.target_index < len(self.target_list) else True + + self.publish_current_target_marker() + + + q = np.array([robot_pos.x, robot_pos.y]) + + attractors = [Attractor( + x=[target_pos.x, target_pos.y], + w=20.0, + )] + + # Potential field + force = self.compute_force(q, attractors, self.obstacles) + force_magnitude = np.linalg.norm(force) + + if force_magnitude > 0.01: + desired_angle = math.atan2(force[1], force[0]) + else: + dx = target_pos.x - robot_pos.x + dy = target_pos.y - robot_pos.y + desired_angle = math.atan2(dy, dx) + + + if not hasattr(self, 'current_yaw'): + self.current_yaw = 0.0 + + angle_error = math.atan2(math.sin(desired_angle - self.current_yaw), + math.cos(desired_angle - self.current_yaw)) + + + # Angular Error + angle_error = math.atan2(math.sin(desired_angle - self.current_yaw), + math.cos(desired_angle - self.current_yaw)) + angle_error_deg = abs(math.degrees(angle_error)) + abs_err_deg = abs(angle_error_deg) + + cmd_speed = TwistStamped() + + # Parameters + V_MIN = 8.0 + V_MAX = 14.0 + OMEGA_MAX = 16.0 + + min_obstacle_dist = float('inf') + for obs in self.obstacles: + dist_to_obs = np.linalg.norm(q - obs.x) + min_obstacle_dist = min(min_obstacle_dist, dist_to_obs) + + safety_factor = 1.0 + if min_obstacle_dist < 10.0: + safety_factor = max(0.4, min_obstacle_dist / 10.0) + + abs_err_deg = abs(math.degrees(angle_error)) + + # Movement + if abs_err_deg > 45: + omega = OMEGA_MAX if angle_error > 0 else -OMEGA_MAX + v = V_MIN + + elif abs_err_deg > 10: + ratio = (abs_err_deg - 10) / 35.0 + + omega_magnitude = 8.0 + ratio * (OMEGA_MAX - 8.0) + omega = omega_magnitude if angle_error > 0 else -omega_magnitude + + v = V_MAX - ratio * (V_MAX - V_MIN) + + else: + Kp = 1.2 + omega = Kp * angle_error + omega = max(-8.0, min(8.0, omega)) + v = V_MAX + + cmd_speed.twist.linear.x = v * safety_factor + cmd_speed.twist.angular.z = omega + cmd_speed.header.frame_id = 'evolo/base_link' + + if abs(cmd_speed.twist.angular.z) > OMEGA_MAX: + cmd_speed.twist.angular.z = OMEGA_MAX if cmd_speed.twist.angular.z > 0 else -OMEGA_MAX + + self.speed_pub.publish(cmd_speed) + self.publish_force_marker(robot_pos, force) + + self._node.get_logger().info( + f"DTT: {self.distance_to_target:.1f}m | " + f"Force: [{force[0]:.1f}, {force[1]:.1f}] (mag: {force_magnitude:.1f}) | " + f"Desired: {math.degrees(desired_angle):.1f}° | " + f"Err: {angle_error_deg:.1f}° | " + f"Cmd: v={cmd_speed.twist.linear.x:.2f}, ω={cmd_speed.twist.angular.z:.2f}" + ) + + # Publish target yaw for monitoring + ty_msg = Float32() + ty_msg.data = desired_angle + self.evolo_pub.publish(ty_msg) + + return None + + def _give_feedback(self) -> str: + time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) + runtime = time_now - self.action_started_time + + feedback = f"Action runtime: {runtime}. DTT: {self.distance_to_target}" + self._node.get_logger().info(feedback) + return feedback + + def calculate_distance(self, pose1:PoseStamped, pose2:PoseStamped) -> float: + dx = pose1.pose.position.x - pose2.pose.position.x + dy = pose1.pose.position.y - pose2.pose.position.y + return math.sqrt(dx*dx + dy*dy) + + def latlon_to_local_frame(self, point_list: list) -> PoseStamped: + + geopoint = GeoPoint() + geopoint.latitude = point_list[0] + geopoint.longitude = point_list[1] + geopoint.altitude = 0.0 + yaw = math.radians(point_list[2]) if len(point_list) > 2 else 0.0 + + point: utm.UTMPoint = utm.fromMsg(geopoint) + pose_stamp = PoseStamped() + pose_stamp.pose.position = point.toPoint() + zone, band = point.gridZone() + pose_stamp.header.frame_id = "utm" + self._node.get_logger().info(f"Utmpoint: {point}") + self._node.get_logger().info(f"UTM Zone: {zone}{band}, Easting: {point.easting:.1f}, Northing: {point.northing:.1f}") + + #Add yaw + quaternion_values = tf_transformations.quaternion_from_euler(0,0,yaw) + pose_stamp.pose.orientation.x = quaternion_values[0] + pose_stamp.pose.orientation.y = quaternion_values[1] + pose_stamp.pose.orientation.z = quaternion_values[2] + pose_stamp.pose.orientation.w = quaternion_values[3] + + t = self._tf_buffer.lookup_transform( + target_frame=self.frame_id, + source_frame=pose_stamp.header.frame_id, + time=Time(seconds=0), + timeout=Duration(seconds=1), + ) + return do_transform_pose_stamped(pose_stamp, t) + + def robot_odom_callback(self, msg: Odometry): + if msg.header.frame_id == self.frame_id: + self.robot_position = PoseStamped() + self.robot_position.header = msg.header + self.robot_position.pose = msg.pose.pose + else: + pose_in_odom_frame = PoseStamped() + pose_in_odom_frame.header = msg.header + pose_in_odom_frame.pose = msg.pose.pose + + try: + t = self._tf_buffer.lookup_transform( + target_frame=self.frame_id, + source_frame=msg.header.frame_id, + time=Time(seconds=0), + timeout=Duration(seconds=1), + ) + self.robot_position = do_transform_pose_stamped(pose_in_odom_frame, t) + + except Exception as e: + self._node.get_logger().error(f"Could not transform robot position: {e}") + return + + self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) + + orientation_q = self.robot_position.pose.orientation + orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] + (_, _, self.current_yaw) = euler_from_quaternion(orientation_list) + + if not hasattr(self, '_odom_log_counter'): + self._odom_log_counter = 0 + + self._odom_log_counter += 1 + if self._odom_log_counter % 50 == 0: + self._node.get_logger().info( + f"Robot local position: ({self.robot_position.pose.position.x:.2f}, " + f"{self.robot_position.pose.position.y:.2f}), yaw: {math.degrees(self.current_yaw):.1f}°" + ) + + # Path visualization + path_msg = Path() + path_msg.header.frame_id = self.frame_id + path_msg.header.stamp = self._node.get_clock().now().to_msg() + self.poses_history.append(self.robot_position) + path_msg.poses = self.poses_history + self.path_pub.publish(path_msg) + +def main(): + rclpy.init() + node = Node("evolo_move_path_action_server") + + action_client = EvoloMovePath(node, "move_path") + + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + node.get_logger().info("Shutting down evolo move path acation server") + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/behaviours/evolo/evolo_move_path/setup.py b/behaviours/evolo/evolo_move_path/setup.py index e78082d74..6b512de04 100644 --- a/behaviours/evolo/evolo_move_path/setup.py +++ b/behaviours/evolo/evolo_move_path/setup.py @@ -23,7 +23,11 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'move_path_server = evolo_move_path.evolo_move_path_server:main', + 'move_path_server_potential_field = evolo_move_path.evolo_move_path_server_potential_field:main', + 'move_path_client = evolo_move_path.client:main', + 'sim_minimal = evolo_move_path.sim_minimal: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_curve:main', ], }, ) From 159325d750c34dcb7af4cb0f3ef5d922e83638d8 Mon Sep 17 00:00:00 2001 From: Baptiste Date: Mon, 23 Feb 2026 17:19:54 +0100 Subject: [PATCH 2/4] added the requested parts in the comments --- .../evolo_move_path/evolo_move_path/client.py | 19 +- .../evolo_move_path/dubins_algorithm.py | 117 ++++ .../evolo_move_path/evolo_move_path_server.py | 269 -------- .../evolo_move_path_server_dubins_curves.py | 612 ++++++++++++++++++ .../evolo_move_path_server_potential_field.py | 287 ++++---- behaviours/evolo/evolo_move_path/setup.py | 26 +- .../smarc_bringups/scripts/evolo_bringup.sh | 30 +- 7 files changed, 907 insertions(+), 453 deletions(-) create mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py delete mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server.py create mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py mode change 100755 => 100644 scripts/smarc_bringups/scripts/evolo_bringup.sh 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 fc60ac044..a353814bb 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/client.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/client.py @@ -11,7 +11,7 @@ class EvoloMovePathClient(Node): def __init__(self): super().__init__('evolo_move_path_client') self._action_client = ActionClient(self, BaseAction, 'move_path') - self.marker_pub = self.create_publisher(MarkerArray, '/waypoints_viz', 10) + self.marker_pub = self.create_publisher(MarkerArray, 'waypoints_viz', 10) def publish_waypoints(self, waypoint_list): marker_array = MarkerArray() @@ -45,23 +45,18 @@ def send_goal(self): payload = { 'speed': 'high', 'waypoints': [ - {'latitude': 58.8397422670, 'longitude': 17.6534623045, 'tolerance': 5.0}, - {'latitude': 58.8400922670, 'longitude': 17.6540122932, 'tolerance': 5.0}, - {'latitude': 58.8403922670, 'longitude': 17.6533123075, 'tolerance': 5.0}, - {'latitude': 58.8398922670, 'longitude': 17.6528123177, 'tolerance': 5.0}, - {'latitude': 58.8397922670, 'longitude': 17.6543122871, 'tolerance': 5.0} - ], - 'obstacles': [ - # {'latitude': 58.8399222670, 'longitude': 17.6537422987, 'radius': 8.0}, - # {'latitude': 58.8402422670, 'longitude': 17.6536623004, 'radius': 10.0}, - # {'latitude': 58.8399422670, 'longitude': 17.6530123137, 'radius': 6.0} + {'latitude': 58.8397422670, '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.8397922670, 'longitude': 17.6543122871, 'tolerance': 3.0} ] } goal_msg.goal = String() goal_msg.goal.data = json.dumps(payload) - self.get_logger().info("Send mission with obstacles...") + self.get_logger().info("Send mission...") self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py b/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py new file mode 100644 index 000000000..94ea8f81a --- /dev/null +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py @@ -0,0 +1,117 @@ +import math + +# ───────────────────────────────────────────────────────────────────────────── +# Dubins Algorithm +# ───────────────────────────────────────────────────────────────────────────── + +def _mod2pi(x: float) -> float: + return x % (2.0 * math.pi) + +def _dubins_LSL(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + tmp0 = d + sa - sb + p_sq = 2.0 + d*d - 2.0*math.cos(alpha - beta) + 2.0*d*(sa - sb) + if p_sq < 0: return None + tmp1 = math.atan2(cb - ca, tmp0) + return _mod2pi(-alpha + tmp1), math.sqrt(p_sq), _mod2pi(beta - tmp1), 'LSL' + +def _dubins_RSR(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + tmp0 = d - sa + sb + p_sq = 2.0 + d*d - 2.0*math.cos(alpha - beta) + 2.0*d*(sb - sa) + if p_sq < 0: return None + tmp1 = math.atan2(ca - cb, tmp0) + return _mod2pi(alpha - tmp1), math.sqrt(p_sq), _mod2pi(-beta + tmp1), 'RSR' + +def _dubins_LSR(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + p_sq = -2.0 + d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(sa + sb) + if p_sq < 0: return None + p = math.sqrt(p_sq) + tmp1 = math.atan2(-ca - cb, d + sa + sb) - math.atan2(-2.0, p) + return _mod2pi(-alpha + tmp1), p, _mod2pi(-_mod2pi(beta) + tmp1), 'LSR' + +def _dubins_RSL(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + p_sq = -2.0 + d*d + 2.0*math.cos(alpha - beta) - 2.0*d*(sa + sb) + if p_sq < 0: return None + p = math.sqrt(p_sq) + tmp1 = math.atan2(ca + cb, d - sa - sb) - math.atan2(2.0, p) + return _mod2pi(alpha - tmp1), p, _mod2pi(beta - tmp1), 'RSL' + +def _dubins_RLR(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + tmp0 = (6.0 - d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(sa - sb)) / 8.0 + if abs(tmp0) > 1.0: return None + p = _mod2pi(2.0*math.pi - math.acos(tmp0)) + t = _mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + _mod2pi(p / 2.0)) + return t, p, _mod2pi(alpha - beta - t + _mod2pi(p)), 'RLR' + +def _dubins_LRL(alpha, beta, d): + sa, ca = math.sin(alpha), math.cos(alpha) + sb, cb = math.sin(beta), math.cos(beta) + tmp0 = (6.0 - d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(-sa + sb)) / 8.0 + if abs(tmp0) > 1.0: return None + p = _mod2pi(2.0*math.pi - math.acos(tmp0)) + t = _mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) + return t, p, _mod2pi(_mod2pi(beta) - alpha - t + _mod2pi(p)), 'LRL' + +def _sample_segment(x0, y0, yaw0, seg_type, length, radius, step): + pts, dist, x, y, yaw = [], 0.0, x0, y0, yaw0 + while dist < length * radius: + pts.append((x, y, yaw)) + if seg_type == 'S': + x += step * math.cos(yaw); y += step * math.sin(yaw) + elif seg_type == 'L': + dy = step / radius + x += radius * (math.sin(yaw + dy) - math.sin(yaw)) + y += radius * (-math.cos(yaw + dy) + math.cos(yaw)) + yaw += dy + elif seg_type == 'R': + dy = step / radius + x += radius * (-math.sin(yaw - dy) + math.sin(yaw)) + y += radius * (math.cos(yaw - dy) - math.cos(yaw)) + yaw -= dy + dist += step + return pts + +def dubins_path(q0, q1, radius, step): + x0, y0, yaw0 = q0 + x1, y1, yaw1 = q1 + dx, dy = x1 - x0, y1 - y0 + D = math.hypot(dx, dy) + d = D / radius + theta = _mod2pi(math.atan2(dy, dx)) + alpha = _mod2pi(yaw0 - theta) + beta = _mod2pi(yaw1 - theta) + + best, best_len = None, float('inf') + for fn in [_dubins_LSL, _dubins_RSR, _dubins_LSR, + _dubins_RSL, _dubins_RLR, _dubins_LRL]: + c = fn(alpha, beta, d) + if c is None: continue + t, p, q, mode = c + if t + p + q < best_len: + best_len = t + p + q + best = c + + if best is None: + n = max(2, int(D / step)) + return [(x0 + i/n*dx, y0 + i/n*dy, math.atan2(dy, dx)) for i in range(n+1)] + + t, p, q, mode = best + path_pts, cx, cy, cyaw = [], x0, y0, yaw0 + for seg_type, length in zip(list(mode), [t, p, q]): + if length < 1e-6: continue + seg = _sample_segment(cx, cy, cyaw, seg_type, length, radius, step) + if seg: + path_pts.extend(seg) + cx, cy, cyaw = seg[-1] + path_pts.append((x1, y1, yaw1)) + return path_pts + diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server.py deleted file mode 100644 index d453f31fa..000000000 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server.py +++ /dev/null @@ -1,269 +0,0 @@ -import rclpy - -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 PoseStamped -from std_msgs.msg import Float32, Empty -from std_msgs.msg import String -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 - -import tf_transformations - -from enum import Enum - -class EvoloMovePath(): - - class WP: - def __init__(self, p : PoseStamped, tol : float, speed : str): - self.p = p - self.tol = tol - self.speed = speed - - def __init__(self, - node: Node, - action_name: str): - self._node = node - - # Initialize the action server with the node and action name - # Give it all the necessary callbacks - self._as = GentlerActionServer( - node, - action_name, - self._on_goal_received, - self._on_cancel_received, - self._prepare_loop, - self._loop_inner, - self._give_feedback, - loop_frequency=2 - ) - - # Initialize any necessary state for your specific action - # These have nothing to do with the action server itself - - # Tf listener - self._tf_buffer = Buffer() - self._tf_listener = TransformListener( - self._tf_buffer, self._node, spin_thread=True - ) - - # State variables. gets updated from topic callbacks - self.robot_position = PoseStamped() #robot positon [geometry_msgs/msg/Pose] - self.robot_position_time = None #robot position time to be compared with current time - - self.distance_to_target = None - - self.target_index = None - self.target_list = None #self.WP - - #Target frame - #self.frame_id = 'map_gt' - self.frame_id = 'evolo/odom' - - #Settings etc - self.timeout = 1800.0 - - - #Time of action start to check for timeout - self.action_started_time = None - - #Callback groups - self.publisher_callback_group = ReentrantCallbackGroup() - self.subscriber_callback_group = ReentrantCallbackGroup() - - # Publishers - self.evolo_pub = self._node.create_publisher(Float32, controlTopics.CONTROL_YAW_TOPIC,10, callback_group=self.publisher_callback_group) - # Subscribers - self.robot_sub = self._node.create_subscription(Odometry, smarcTopics.ODOM_TOPIC, self.robot_odom_callback,10, callback_group=self.subscriber_callback_group) - self._node.get_logger().info("Action server started") - - def _on_goal_received(self, goal_request: dict) -> bool: - self._node.get_logger().info(f"Received goal request: {goal_request}") - # Here you would typically validate the goal request - # Return True to accept the goal, False to reject it - #params = json.loads(goal_request['json-params']) - - speed = goal_request['speed'] - waypoints = goal_request['waypoints'] - self.timeout = 600 - - if len(waypoints) == 0: - self._node.get_logger().info(f"Waypoint list was empty") - return False - - #Reset target list varaibles - self.target_index = 0 - self.target_list = [] - - - for wp in waypoints: - self._node.get_logger().info(f"WP: {wp}") - wp_params = wp - - self._node.get_logger().info(f"wp params: {wp_params}") - - #compute target position from lat lon - lat = float(wp_params['latitude']) - lon = float(wp_params['longitude']) - self._node.get_logger().info(f"lat lon sent to function: {lat}, {lon}") - target_position = self.latlon_to_local_frame([lat,lon]) - target_speed = speed - target_tol = float(max(10,wp_params['tolerance'])) - - self.target_list.append(self.WP(p=target_position, speed = target_speed, tol = target_tol)) - - - return True - - def _on_cancel_received(self) -> bool: - self._node.get_logger().info("Received cancel request") - # Here you would typically handle the cancel request - # Return True to accept the cancel, False to reject it - #TODO send speed=stop - return True - - def _prepare_loop(self) -> None: - self._node.get_logger().info("Preparing loop for action execution") - # Here you would typically set up any necessary state or resources - # This is run once before the loop starts, after you accept the goal - self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) - - def _loop_inner(self) -> bool | None: - # Here you would typically perform the main logic of the action - # Return True to indicate success, False for failure, or None to continue - # This is run after _prepare_loop call at "loop_frequency" Hz - - #Check for timeout - time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) - runtime = (time_now - self.action_started_time) - if(runtime > self.timeout): - return False # Failure - - if(self.robot_position is None or (time_now - self.robot_position_time) > 10): - self._node.get_logger().error("ERROR no robot position") - return False - - #Calculate distance to our current loiter target and change target if we are close enough to switch to the next one - - target_position = self.target_list[self.target_index].p - self.distance_to_target = self.calculate_distance(self.robot_position, target_position) - if(self.distance_to_target < self.target_list[self.target_index].tol): - self.target_index += 1 - - if(self.target_index >= len(self.target_list)): - return True - - targetYaw = Float32() - dx = target_position.pose.position.x - self.robot_position.pose.position.x - dy = target_position.pose.position.y - self.robot_position.pose.position.y - targetYaw.data = math.atan2(dy,dx) # yaw in ENU - - self.evolo_pub.publish(targetYaw) - - return None - - def _give_feedback(self) -> str: - time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) - runtime = time_now - self.action_started_time - - feedback = f"Action runtime: {runtime}. DTT: {self.distance_to_target}" - self._node.get_logger().info(feedback) - # Here you would typically generate feedback for the action - # This is run after each _loop_inner call - return feedback - - def calculate_distance(self, pose1:PoseStamped, pose2:PoseStamped) -> float: - dx = pose1.pose.position.x - pose2.pose.position.x - dy = pose1.pose.position.y - pose2.pose.position.y - return math.sqrt(dx*dx + dy*dy) - - - def latlon_to_local_frame(self, point_list: list) -> PoseStamped: - - geopoint = GeoPoint() - geopoint.latitude = point_list[0] - geopoint.longitude = point_list[1] - geopoint.altitude = 0.0 - yaw = math.radians(point_list[2]) if len(point_list) > 2 else 0.0 - - - point: utm.UTMPoint = utm.fromMsg(geopoint) - pose_stamp = PoseStamped() - pose_stamp.pose.position = point.toPoint() - zone, band = point.gridZone() - pose_stamp.header.frame_id = f"utm_{zone}_{band}" - - self._node.get_logger().info(f"Utmpoint: {point}") - - #Add yaw - quaternion_values = tf_transformations.quaternion_from_euler(0,0,yaw) - pose_stamp.pose.orientation.x = quaternion_values[0] - pose_stamp.pose.orientation.y = quaternion_values[1] - pose_stamp.pose.orientation.z = quaternion_values[2] - pose_stamp.pose.orientation.w = quaternion_values[3] - - t = self._tf_buffer.lookup_transform( - target_frame=self.frame_id, - source_frame=pose_stamp.header.frame_id, - time=Time(seconds=0), - timeout=Duration(seconds=1), - ) - return do_transform_pose_stamped(pose_stamp, t) - - #Subscriber callback functions - def robot_odom_callback(self,msg : Odometry): - #self._node.get_logger().info("robot position updated.") - self.robot_position = PoseStamped() - self.robot_position.header = msg.header - self.robot_position.pose = msg.pose.pose - self.robot_position_time = int(self._node.get_clock().now().nanoseconds * 1e-9) - #self._node.get_logger().info("" + str(msg.header.frame_id)) - - def testcase(self): - pass - - -def main(): - rclpy.init() - node = Node("evolo_move_path_action_server") - - action_client = EvoloMovePath(node, "move_path") - - #action_client.testcase() - - - executor = MultiThreadedExecutor() - executor.add_node(node) - try: - executor.spin() - except KeyboardInterrupt: - node.get_logger().info("Shutting down evolo move path acation server") - finally: - executor.shutdown() - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() 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 new file mode 100644 index 000000000..6d058fc3b --- /dev/null +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_dubins_curves.py @@ -0,0 +1,612 @@ +import rclpy + +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 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 + +from enum import Enum + +from .dubins_algorithm import dubins_path + + + +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: float = 15.0, + Ld_gain: float = 0.5, + omega_max: float = 16.0, + ff_gain: float = 0.2, + dubins_step: float = 0.5): + 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]: + + Ld = self.Ld_base + self.Ld_gain * robot_v + + lookahead_idx = cursor + 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] + + 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) + + omega_deg = max(-self.omega_max, min(self.omega_max, omega_deg)) + + return omega_deg, lookahead_idx + + +# ───────────────────────────────────────────────────────────────────────────── +# Action +# ───────────────────────────────────────────────────────────────────────────── +class EvoloMovePath(): + + class WP: + def __init__(self, p, tol, speed_kn): + self.p = p + self.tol = tol + self.speed_kn = speed_kn + + def __init__(self, node: Node, action_name: str): + self._node = node + + self._node.declare_parameters( + namespace='', + parameters=[ + ('v_min', 8.0), + ('v_max', 14.0), + ('omega_max', 16.0), + ('err_large_deg', 45.0), + ('ld_base', 15.0), + ('ld_gain', 0.5), + ('ff_gain', 0.2), + ('min_turning_radius', 30.0), + ('dubins_step', 0.5), + ('timeout', 600.0), + ('speed_map_slow', 8.0), + ('speed_map_medium', 11.0), + ('speed_map_high', 14.0), + ('frame_id', 'evolo/odom') + ] + ) + 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.ERR_LARGE_DEG = self._node.get_parameter('err_large_deg').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.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 + ) + + self._as = GentlerActionServer( + node, action_name, + self._on_goal_received, + self._on_cancel_received, + self._prepare_loop, + self._loop_inner, + self._give_feedback, + loop_frequency=50, + ) + + self._tf_buffer = Buffer() + 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.current_linear_speed = 0.0 + self.current_angular_speed = 0.0 + + self.target_index = None + self.target_list = None + self.poses_history = [] + + self.dubins_path = None + self.wp_end_indices = None + self.path_cursor = 0 + + self.action_started_time = None + + self.publisher_callback_group = ReentrantCallbackGroup() + self.subscriber_callback_group = ReentrantCallbackGroup() + + self.evolo_pub = self._node.create_publisher(Float32, controlTopics.CONTROL_YAW_TOPIC, 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.path_pub = self._node.create_publisher(Path, 'visual_path', 10, callback_group=self.publisher_callback_group) + self.viz_pub = self._node.create_publisher(MarkerArray, 'visualisation', 10, callback_group=self.publisher_callback_group) + self.dubins_path_pub = self._node.create_publisher(Path, 'dubins_path', 10, callback_group=self.publisher_callback_group) + self.attractor_pub = self._node.create_publisher(Marker, 'attractor_marker', 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._node.get_logger().info("EvoloMovePath started") + + # ───────────────────────────────────────────────────────────────────────── + 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'] + + 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 + + for wp_params in waypoints: + lat = float(wp_params['latitude']) + lon = float(wp_params['longitude']) + tol = float(wp_params['tolerance']) + 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._node.get_logger().info( + f" WP{len(self.target_list)}: " + f"({pose.pose.position.x:.1f}, {pose.pose.position.y:.1f}) tol={tol}m" + ) + + self.publish_waypoints_markers() + return True + + def _on_cancel_received(self) -> bool: + self._send_stop() + 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 + + + def _get_local_curvature(self, path: list, cursor: int) -> float: + + if cursor + 4 >= len(path): + return 0.0 + + x1, y1, yaw1 = path[cursor] + x2, y2, yaw2 = path[cursor + 2] + x3, y3, yaw3 = path[cursor + 4] + + 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) / 4.0 + kappa = avg_dyaw / self.DUBINS_STEP + + return kappa + + + + # ───────────────────────────────────────────────────────────────────────── + 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: + 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 + + # Planification + 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) + self.path_cursor = max(self.path_cursor, candidate) + self.path_cursor = min(self.path_cursor, len(path) - 1) + + 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._publish_attractor(ax, 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 + + if abs_err_deg > self.ERR_LARGE_DEG: + omega = math.copysign(self.OMEGA_MAX, angle_error) + v = self.V_MIN + mode = "TURN" + + else: + omega, lookahead_idx = 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, + path = path, + cursor = self.path_cursor, + ) + slow_factor = max(0.0, 1.0 - abs_err_deg / self.ERR_LARGE_DEG) + v = self.V_MIN + slow_factor * (v - self.V_MIN) + mode = "PP" + + # Publication + cmd = TwistStamped() + cmd.header.stamp = self._node.get_clock().now().to_msg() + cmd.header.frame_id = self.frame_id + cmd.twist.linear.x = v + cmd.twist.angular.z = omega + self.speed_pub.publish(cmd) + + yaw_msg = Float32() + yaw_msg.data = float(desired_angle) + self.evolo_pub.publish(yaw_msg) + + if not hasattr(self, '_log_counter'): + self._log_counter = 0 + self._log_counter += 1 + if self._log_counter % 50 == 0: + self._node.get_logger().info( + f"[{mode}] err={abs_err_deg:.1f}° v={v:.2f} ω={omega:.2f}°/s | " + f"cursor={self.path_cursor}/{len(path)} DTT={dist_to_wp:.1f}m" + ) + + return None + + # ───────────────────────────────────────────────────────────────────────── + 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" + ) + + 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: + seg = dubins_path(q_prev, q_next, + radius=self.MIN_TURNING_RADIUS, step=self.DUBINS_STEP) + full_path.extend(seg) + wp_ends.append(len(full_path) - 1) + self._node.get_logger().info( + f" Seg{i+1}: {len(seg)} pts → idx={wp_ends[-1]}") + except Exception as e: + self._node.get_logger().error(f"Dubins seg{i+1} failed: {e}") + return False + q_prev = q_next + + 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_pub.publish(self._path_msg(full_path)) + return True + + # ───────────────────────────────────────────────────────────────────────── + 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') + 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) + if score < best_score: + best_score = score + best_idx = i + return best_idx + + def _send_stop(self): + cmd = TwistStamped() + cmd.header.stamp = self._node.get_clock().now().to_msg() + cmd.twist.linear.x = 0.0 + cmd.twist.angular.z = 0.0 + self.speed_pub.publish(cmd) + + 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() + for x, y, yaw in configurations: + ps = PoseStamped() + ps.header = msg.header + ps.pose.position.x = x + ps.pose.position.y = y + q = tf_transformations.quaternion_from_euler(0, 0, yaw) + ps.pose.orientation.x = q[0] + ps.pose.orientation.y = q[1] + ps.pose.orientation.z = q[2] + ps.pose.orientation.w = q[3] + msg.poses.append(ps) + return msg + + def _publish_attractor(self, ax, ay): + m = Marker() + m.header.frame_id = self.frame_id + m.header.stamp = self._node.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) + + 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 "?" + return f"Runtime: {runtime}s | WP: {self.target_index}/{n} | DTT: {dtt}m" + + def latlon_to_local_frame(self, point_list): + geopoint = GeoPoint() + geopoint.latitude = point_list[0] + geopoint.longitude = point_list[1] + geopoint.altitude = 0.0 + utm_pt = georef_utils.convert_latlon_to_utm(geopoint) + ps = PoseStamped() + ps.header = utm_pt.header + ps.pose.position = utm_pt.point + yaw = math.radians(point_list[2]) if len(point_list) > 2 else 0.0 + q = tf_transformations.quaternion_from_euler(0, 0, yaw) + ps.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + try: + t = self._tf_buffer.lookup_transform( + target_frame=self.frame_id, + source_frame=ps.header.frame_id, + time=Time(seconds=0), + timeout=Duration(seconds=1), + ) + return do_transform_pose_stamped(ps, t) + except Exception as e: + self._node.get_logger().error(f"TF failed: {e}") + 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 + self.robot_position.pose = msg.pose.pose + else: + raw = PoseStamped() + raw.header = msg.header + raw.pose = msg.pose.pose + try: + t = self._tf_buffer.lookup_transform( + target_frame=self.frame_id, + source_frame=msg.header.frame_id, + time=Time(seconds=0), + 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}") + return + + 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_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 % 50 == 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}°" + ) + + path_msg = Path() + path_msg.header.frame_id = self.frame_id + path_msg.header.stamp = self._node.get_clock().now().to_msg() + self.poses_history.append(self.robot_position) + path_msg.poses = self.poses_history + self.path_pub.publish(path_msg) + + def publish_waypoints_markers(self): + ma = MarkerArray() + for i, wp in enumerate(self.target_list): + m = Marker() + m.header.frame_id = self.frame_id + m.header.stamp = self._node.get_clock().now().to_msg() + m.ns = "waypoints"; m.id = i + m.type = Marker.SPHERE; m.action = Marker.ADD + m.pose.position.x = wp.p.pose.position.x + m.pose.position.y = wp.p.pose.position.y + m.pose.position.z = 0.5 + m.scale.x = wp.tol * 2; m.scale.y = wp.tol * 2; m.scale.z = 1.0 + m.color.g = 1.0; m.color.a = 0.3 + ma.markers.append(m) + t = Marker() + t.header = m.header + t.ns = "waypoint_labels"; t.id = i + 1000 + t.type = Marker.TEXT_VIEW_FACING; t.action = Marker.ADD + t.pose.position.x = wp.p.pose.position.x + t.pose.position.y = wp.p.pose.position.y + t.pose.position.z = 2.0 + t.scale.z = 2.0 + 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}" + ma.markers.append(t) + self.viz_pub.publish(ma) + + +# ───────────────────────────────────────────────────────────────────────────── +def main(): + rclpy.init() + node = Node("move_path_server_dubins_pp") + EvoloMovePath(node, "move_path") + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + node.get_logger().info("Shutting down") + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py index a46ed7690..21323a211 100644 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py +++ b/behaviours/evolo/evolo_move_path/evolo_move_path/evolo_move_path_server_potential_field.py @@ -32,6 +32,8 @@ import time import math import json +from geometry_msgs.msg import Quaternion +from smarc_utilities import georef_utils import tf_transformations @@ -45,12 +47,6 @@ def __init__(self, x, w): self.w = w -class Repulsor: - def __init__(self, x, w): - self.x = np.array(x, dtype=float) - self.w = w - - class EvoloMovePath(): @@ -76,7 +72,7 @@ def __init__(self, self._prepare_loop, self._loop_inner, self._give_feedback, - loop_frequency=2 + loop_frequency=1 ) # Initialize any necessary state for your specific action @@ -98,7 +94,7 @@ def __init__(self, self.target_list = None #self.WP self.poses_history = [] # for the path - self.obstacles = [] + #Target frame #self.frame_id = 'map_gt' @@ -118,13 +114,13 @@ def __init__(self, # Publishers self.evolo_pub = self._node.create_publisher(Float32, controlTopics.CONTROL_YAW_TOPIC,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.path_pub = self._node.create_publisher(Path, '/evolo/visual_path', 10, callback_group=self.publisher_callback_group) - self.marker_pub = self._node.create_publisher(Marker, '/evolo/force_markers', 10, callback_group=self.publisher_callback_group) - self.viz_markers_pub = self._node.create_publisher(MarkerArray, '/evolo/visualisation', 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.path_pub = self._node.create_publisher(Path, 'visual_path', 10, callback_group=self.publisher_callback_group) + self.marker_pub = self._node.create_publisher(Marker, 'force_markers', 10, callback_group=self.publisher_callback_group) + self.viz_markers_pub = self._node.create_publisher(MarkerArray, 'visualisation', 10, callback_group=self.publisher_callback_group) # Subscribers - self.robot_sub = self._node.create_subscription(Odometry, '/evolo/smarc/odom', 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) self._node.get_logger().info("Action server started") @@ -136,7 +132,6 @@ def _on_goal_received(self, goal_request: dict) -> bool: speed = goal_request['speed'] waypoints = goal_request['waypoints'] - obstacles = goal_request['obstacles'] self.timeout = 600 if len(waypoints) == 0: @@ -162,22 +157,6 @@ def _on_goal_received(self, goal_request: dict) -> bool: target_tol = float(wp_params['tolerance']) self.target_list.append(self.WP(p=target_position, speed = target_speed, tol = target_tol)) - # Obstacles - self.obstacles = [] - if 'obstacles' in goal_request: - for obs in goal_request['obstacles']: - self._node.get_logger().info(f"Obstacle: {obs}") - lat = float(obs['latitude']) - lon = float(obs['longitude']) - radius = float(obs.get('radius', 5.0)) - - obs_position = self.latlon_to_local_frame([lat, lon]) - obs_x = obs_position.pose.position.x - obs_y = obs_position.pose.position.y - - self.obstacles.append(Repulsor(x=[obs_x, obs_y], w=radius)) - self._node.get_logger().info(f"Obstacle added at ({obs_x:.1f}, {obs_y:.1f})") - # Publish markers for visualization self.publish_waypoints_markers() @@ -191,7 +170,7 @@ def _prepare_loop(self) -> None: self._node.get_logger().info("Preparing loop for action execution") self.action_started_time = int(self._node.get_clock().now().nanoseconds * 1e-9) - def compute_force(self, q, s, r): + def compute_force(self, q, s): dq = np.zeros(2) # Attractors @@ -201,23 +180,6 @@ def compute_force(self, q, s, r): if d > 0.01: dq += a.w * (diff / d) - # Repulsors - for rep in r: - diff = q - rep.x - d = np.linalg.norm(diff) - - rho_0 = rep.w * 3.0 - - if d < rho_0 and d > 0.01: - strength_factor = 400.0 - - repulsion_magnitude = strength_factor * (1.0/d - 1.0/rho_0) * (1.0/(d**2)) - dq += repulsion_magnitude * (diff / d) - - self._node.get_logger().info( - f"Repulsion: dist={d:.1f}m, force_mag={repulsion_magnitude:.2f}" - ) - return dq def publish_waypoints_markers(self): @@ -271,53 +233,6 @@ def publish_waypoints_markers(self): marker_array.markers.append(text_marker) - # OBSTACLES - for i, obs in enumerate(self.obstacles): - marker = Marker() - marker.header.frame_id = self.frame_id - marker.header.stamp = self._node.get_clock().now().to_msg() - marker.ns = "obstacles" - marker.id = i + 2000 - marker.type = Marker.CYLINDER - marker.action = Marker.ADD - - marker.pose.position.x = obs.x[0] - marker.pose.position.y = obs.x[1] - marker.pose.position.z = 0.5 - - marker.scale.x = obs.w * 2 - marker.scale.y = obs.w * 2 - marker.scale.z = 1.0 - - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 0.5 - - marker_array.markers.append(marker) - - text_marker = Marker() - text_marker.header.frame_id = self.frame_id - text_marker.header.stamp = self._node.get_clock().now().to_msg() - text_marker.ns = "obstacle_labels" - text_marker.id = i + 3000 - text_marker.type = Marker.TEXT_VIEW_FACING - text_marker.action = Marker.ADD - - text_marker.pose.position.x = obs.x[0] - text_marker.pose.position.y = obs.x[1] - text_marker.pose.position.z = 2.0 - - text_marker.scale.z = 1.5 - - text_marker.color.r = 1.0 - text_marker.color.g = 0.0 - text_marker.color.b = 0.0 - text_marker.color.a = 1.0 - - text_marker.text = f"OBS{i+1}\nR={obs.w:.1f}m" - - marker_array.markers.append(text_marker) self._node.get_logger().info(f"Publishing {len(marker_array.markers)} markers") self.viz_markers_pub.publish(marker_array) @@ -386,6 +301,9 @@ def publish_force_marker(self, robot_pos, force): self.marker_pub.publish(marker) + + + def _loop_inner(self) -> bool | None: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) runtime = (time_now - self.action_started_time) @@ -411,105 +329,171 @@ def _loop_inner(self) -> bool | None: if self.distance_to_target < current_wp.tol: self.target_index += 1 self._node.get_logger().info(f"Waypoint {self.target_index} reached") + + # ✅ Réinitialiser la mémoire du contrôle PD + if hasattr(self, 'previous_angle_error'): + self.previous_angle_error = 0.0 + self.previous_time = None + return None if self.target_index < len(self.target_list) else True self.publish_current_target_marker() + # ============================================================ + # CHAMP DE POTENTIEL AVEC ANTICIPATION (LOOK-AHEAD) + # ============================================================ q = np.array([robot_pos.x, robot_pos.y]) + # ATTRACTION VERS LE WAYPOINT ACTUEL attractors = [Attractor( x=[target_pos.x, target_pos.y], - w=20.0, + w=20.0, )] - # Potential field - force = self.compute_force(q, attractors, self.obstacles) + # ANTICIPATION : Ajouter une force vers le prochain waypoint si proche + LOOK_AHEAD_DISTANCE = 15.0 # Distance à partir de laquelle on anticipe (ajustable) + NEXT_WP_WEIGHT_FACTOR = 0.4 # Poids relatif du prochain waypoint (0-1) + + if self.distance_to_target < LOOK_AHEAD_DISTANCE and self.target_index + 1 < len(self.target_list): + next_wp = self.target_list[self.target_index + 1] + next_pos = next_wp.p.pose.position + + # Calculer le poids progressif basé sur la distance au waypoint actuel + # Plus on est proche, plus le prochain waypoint a d'influence + blend_factor = 1.0 - (self.distance_to_target / LOOK_AHEAD_DISTANCE) + next_weight = 20.0 * NEXT_WP_WEIGHT_FACTOR * blend_factor + + attractors.append(Attractor( + x=[next_pos.x, next_pos.y], + w=next_weight, + )) + + self._node.get_logger().info( + f"Look-ahead active: DTT={self.distance_to_target:.1f}m, " + f"next_weight={next_weight:.1f}, blend={blend_factor:.2f}" + ) + + # Calculer la force totale (attraction) + force = self.compute_force(q, attractors) force_magnitude = np.linalg.norm(force) + # Direction désirée = direction de la force if force_magnitude > 0.01: desired_angle = math.atan2(force[1], force[0]) else: + # Fallback : direction directe vers la cible dx = target_pos.x - robot_pos.x dy = target_pos.y - robot_pos.y desired_angle = math.atan2(dy, dx) + # ============================================================ + # CALCUL DE L'ERREUR ANGULAIRE + # ============================================================ if not hasattr(self, 'current_yaw'): - self.current_yaw = 0.0 + self.current_yaw = 0.0 angle_error = math.atan2(math.sin(desired_angle - self.current_yaw), - math.cos(desired_angle - self.current_yaw)) + math.cos(desired_angle - self.current_yaw)) + + abs_err_deg = abs(math.degrees(angle_error)) + # ============================================================ + # CONTRÔLE PD (Proportionnel + Dérivé) POUR ÉVITER OSCILLATIONS + # ============================================================ + + # Initialiser les variables de mémoire si nécessaire + if not hasattr(self, 'previous_angle_error'): + self.previous_angle_error = 0.0 + self.previous_time = None + + # Calcul de la dérivée de l'erreur + if self.previous_time is not None: + dt = time_now - self.previous_time + if dt > 0: + angle_error_derivative = (angle_error - self.previous_angle_error) / dt + else: + angle_error_derivative = 0.0 + else: + angle_error_derivative = 0.0 + + # Mémoriser pour la prochaine itération + self.previous_angle_error = angle_error + self.previous_time = time_now - # Angular Error - angle_error = math.atan2(math.sin(desired_angle - self.current_yaw), - math.cos(desired_angle - self.current_yaw)) - angle_error_deg = abs(math.degrees(angle_error)) - abs_err_deg = abs(angle_error_deg) + # ============================================================ + # COMMANDES DE VITESSE + # ============================================================ cmd_speed = TwistStamped() - # Parameters V_MIN = 8.0 V_MAX = 14.0 OMEGA_MAX = 16.0 - min_obstacle_dist = float('inf') - for obs in self.obstacles: - dist_to_obs = np.linalg.norm(q - obs.x) - min_obstacle_dist = min(min_obstacle_dist, dist_to_obs) - - safety_factor = 1.0 - if min_obstacle_dist < 10.0: - safety_factor = max(0.4, min_obstacle_dist / 10.0) + # Dans _loop_inner, section commandes de vitesse + + # Paramètres cohérents + V_MIN = 8.0 + V_MAX = 14.0 + OMEGA_MAX = 16.0 + OMEGA_MIN = 8.0 + # Normaliser l'erreur angulaire abs_err_deg = abs(math.degrees(angle_error)) - # Movement if abs_err_deg > 45: + # Grande erreur : rotation prioritaire omega = OMEGA_MAX if angle_error > 0 else -OMEGA_MAX v = V_MIN elif abs_err_deg > 10: - ratio = (abs_err_deg - 10) / 35.0 + # Erreur moyenne : transition douce + # Interpolation non-linéaire (cosinus) pour des transitions plus douces + ratio = (abs_err_deg - 10) / 35.0 # Entre 0 et 1 + smooth_ratio = (1 - math.cos(ratio * math.pi)) / 2 # Courbe en S - omega_magnitude = 8.0 + ratio * (OMEGA_MAX - 8.0) - omega = omega_magnitude if angle_error > 0 else -omega_magnitude - - v = V_MAX - ratio * (V_MAX - V_MIN) + omega = (OMEGA_MIN + smooth_ratio * (OMEGA_MAX - OMEGA_MIN)) * (1 if angle_error > 0 else -1) + v = V_MAX - smooth_ratio * (V_MAX - V_MIN) else: - Kp = 1.2 - omega = Kp * angle_error - omega = max(-8.0, min(8.0, omega)) + # Petite erreur : contrôle PD complet + Kp = 1.5 + Kd = 0.3 + + # FORMULE PD COMPLÈTE + omega = Kp * angle_error + Kd * angle_error_derivative + omega = max(-OMEGA_MIN, min(OMEGA_MIN, omega)) # Saturation v = V_MAX - cmd_speed.twist.linear.x = v * safety_factor + # Appliquer le facteur de sécurité + cmd_speed.twist.linear.x = v + + # Réduire omega proportionnellement si on ralentit beaucoup cmd_speed.twist.angular.z = omega - cmd_speed.header.frame_id = 'evolo/base_link' - - if abs(cmd_speed.twist.angular.z) > OMEGA_MAX: - cmd_speed.twist.angular.z = OMEGA_MAX if cmd_speed.twist.angular.z > 0 else -OMEGA_MAX self.speed_pub.publish(cmd_speed) self.publish_force_marker(robot_pos, force) + # Logs détaillés self._node.get_logger().info( f"DTT: {self.distance_to_target:.1f}m | " f"Force: [{force[0]:.1f}, {force[1]:.1f}] (mag: {force_magnitude:.1f}) | " - f"Desired: {math.degrees(desired_angle):.1f}° | " - f"Err: {angle_error_deg:.1f}° | " + f"Err: {abs_err_deg:.1f}° | dErr: {math.degrees(angle_error_derivative):.1f}°/s | " f"Cmd: v={cmd_speed.twist.linear.x:.2f}, ω={cmd_speed.twist.angular.z:.2f}" ) - # Publish target yaw for monitoring ty_msg = Float32() ty_msg.data = desired_angle self.evolo_pub.publish(ty_msg) return None + + + + def _give_feedback(self) -> str: time_now = int(self._node.get_clock().now().nanoseconds * 1e-9) runtime = time_now - self.action_started_time @@ -529,31 +513,35 @@ def latlon_to_local_frame(self, point_list: list) -> PoseStamped: geopoint.latitude = point_list[0] geopoint.longitude = point_list[1] geopoint.altitude = 0.0 - yaw = math.radians(point_list[2]) if len(point_list) > 2 else 0.0 - point: utm.UTMPoint = utm.fromMsg(geopoint) + utm_point_stamped = georef_utils.convert_latlon_to_utm(geopoint) + pose_stamp = PoseStamped() - pose_stamp.pose.position = point.toPoint() - zone, band = point.gridZone() - pose_stamp.header.frame_id = "utm" - self._node.get_logger().info(f"Utmpoint: {point}") - self._node.get_logger().info(f"UTM Zone: {zone}{band}, Easting: {point.easting:.1f}, Northing: {point.northing:.1f}") - - #Add yaw - quaternion_values = tf_transformations.quaternion_from_euler(0,0,yaw) - pose_stamp.pose.orientation.x = quaternion_values[0] - pose_stamp.pose.orientation.y = quaternion_values[1] - pose_stamp.pose.orientation.z = quaternion_values[2] - pose_stamp.pose.orientation.w = quaternion_values[3] - - t = self._tf_buffer.lookup_transform( + pose_stamp.header = utm_point_stamped.header + pose_stamp.pose.position = utm_point_stamped.point + + yaw = math.radians(point_list[2]) if len(point_list) > 2 else 0.0 + quaternion_values = tf_transformations.quaternion_from_euler(0, 0, yaw) + pose_stamp.pose.orientation = Quaternion( + x=quaternion_values[0], + y=quaternion_values[1], + z=quaternion_values[2], + w=quaternion_values[3] + ) + + try: + t = self._tf_buffer.lookup_transform( target_frame=self.frame_id, source_frame=pose_stamp.header.frame_id, time=Time(seconds=0), timeout=Duration(seconds=1), ) - return do_transform_pose_stamped(pose_stamp, t) - + transformed_pose = do_transform_pose_stamped(pose_stamp, t) + return transformed_pose + except Exception as e: + self._node.get_logger().error(f"Failed to transform pose: {e}") + return None + def robot_odom_callback(self, msg: Odometry): if msg.header.frame_id == self.frame_id: self.robot_position = PoseStamped() @@ -601,6 +589,7 @@ def robot_odom_callback(self, msg: Odometry): path_msg.poses = self.poses_history self.path_pub.publish(path_msg) + def main(): rclpy.init() node = Node("evolo_move_path_action_server") diff --git a/behaviours/evolo/evolo_move_path/setup.py b/behaviours/evolo/evolo_move_path/setup.py index 6b512de04..07d6825ac 100644 --- a/behaviours/evolo/evolo_move_path/setup.py +++ b/behaviours/evolo/evolo_move_path/setup.py @@ -1,5 +1,6 @@ from setuptools import find_packages, setup -import glob, os +import os +from glob import glob package_name = 'evolo_move_path' @@ -11,23 +12,32 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'config'), glob.glob('config/*')), - (os.path.join('share', package_name, 'launch'), glob.glob('launch/*')), + + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), ], install_requires=['setuptools'], zip_safe=True, maintainer='niklas', maintainer_email='nrol@kth.se', - description='TODO: Package description', + description='Evolo path following with potential fields and action server', license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ + # 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_client = evolo_move_path.client:main', - 'sim_minimal = evolo_move_path.sim_minimal: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_curve: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', ], }, -) +) \ No newline at end of file diff --git a/scripts/smarc_bringups/scripts/evolo_bringup.sh b/scripts/smarc_bringups/scripts/evolo_bringup.sh old mode 100755 new mode 100644 index 97892ebb9..b4340b366 --- a/scripts/smarc_bringups/scripts/evolo_bringup.sh +++ b/scripts/smarc_bringups/scripts/evolo_bringup.sh @@ -11,16 +11,16 @@ BT_LOG_MODE=compact # can be 'compact' or 'verbose' #Simulation -SIM = False -if ["$SIM" == "True"]; then +SIM=True +if [ "$SIM" = "True" ]; then REALSIM=simulation - ROBOT_NAME=evolo_v1 + ROBOT_NAME=evolo USE_SIM_TIME=True else REALSIM=real #USE_SIM_TIME=False - USE_SIM_TIME=False #Useful for rosbags - LOCATION_SOURCE=MQTT #[SBG MQTT SERIAL] + USE_SIM_TIME=True #Useful for rosbags + LOCATION_SOURCE=SBG #[SBG MQTT SERIAL] fi #Low controllers @@ -46,23 +46,23 @@ tmux select-layout -t $SESSION:2 tiled tmux select-pane -t $SESSION:2.0 tmux send-keys "sleep 4; ros2 run evolo_move_to move_to_server --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m tmux select-pane -t $SESSION:2.1 -tmux send-keys "sleep 4; ros2 run evolo_move_path move_path_server --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m +tmux send-keys "sleep 4; ros2 run evolo_move_path move_path_server_dubins_curves --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m #tmux select-pane -t $SESSION:2.2 #tmux send-keys "sleep 4; ros2 run lolo_emergency_action server --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m #tmux select-pane -t $SESSION:2.3 #tmux send-keys "sleep 4; ros2 run lolo_loiter server --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m # Mqtt bridge. -tmux new-window -t $SESSION:3 -n 'mqtt_bridge' +tmux new-window -t $SESSION:3 -n 'mqtt' tmux select-window -t $SESSION:3 # To connect to smarc MQTT broker if [ "$REALSIM" = "real" ]; then tmux new-window -t $SESSION:3 -n 'mqtt' tmux select-window -t $SESSION:3 - tmux send-keys "sleep 7; ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=20.240.40.232 broker_port:=1884 robot_name:=$ROBOT_NAME domain:=$AGENT_TYPE realsim:=$REALSIM use_sim_time:=$USE_SIM_TIME context:=$CONTEXT" + tmux send-keys "sleep 7; ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=20.240.40.232 broker_port:=1884 robot_name:=$ROBOT_NAME domain:=$AGENT_TYPE realsim:=$REALSIM use_sim_time:=$USE_SIM_TIME context:=$CONTEXT" C-m else - tmux send-keys "sleep 7; ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=127.0.0.1 broker_port:=1883 robot_name:=$ROBOT_NAME domain:=$AGENT_TYPE realsim:=$REALSIM use_sim_time:=$USE_SIM_TIME context:=$CONTEXT" + tmux send-keys "sleep 7; ros2 launch str_json_mqtt_bridge waraps_bridge.launch broker_addr:=20.240.40.232 broker_port:=1884 robot_name:=$ROBOT_NAME domain:=$AGENT_TYPE realsim:=$REALSIM use_sim_time:=$USE_SIM_TIME context:=$CONTEXT" C-m fi @@ -72,7 +72,7 @@ if [ "$REALSIM" = "real" ]; then #Connection to evolo captain tmux new-window -t $SESSION:4 -n 'Evolo captain' tmux select-window -t $SESSION:4 - tmux send-keys "ros2 launch evolo_mqtt_bridge evolo_mqtt_launch.py" C-m + tmux send-keys "ros2 launch evolo_mqtt_bridge evolo_mqtt_launch.py" #tmux send-keys "ros2 launch evolo_serial_bridge evolo_serial_launch.py" #SBG driver @@ -81,7 +81,7 @@ if [ "$REALSIM" = "real" ]; then tmux send-keys "TODO: lanuch SBG driver" C-m #Odom - tmux new-window -t $SESSION:6 -n 'SBG to Odom' + tmux new-window -t $SESSION:6 -n 'Localization to Odom' tmux select-window -t $SESSION:6 tmux split-window -h -t $SESSION:6.0 @@ -98,11 +98,11 @@ if [ "$REALSIM" = "real" ]; then if [ "$LOCATION_SOURCE" = "MQTT" ] || [ "$LOCATION_SOURCE" = "SERIAL" ]; then #Odom initializer tmux select-pane -t $SESSION:6.0 - tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_initializer_launch.py" C-m + tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_initializer_launch.py use_sim_time:=$USE_SIM_TIME" C-m #sbg to odom node tmux select-pane -t $SESSION:6.1 - tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_launch.py" C-m + tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_launch.py use_sim_time:=$USE_SIM_TIME" C-m fi @@ -128,7 +128,7 @@ if [ "$REALSIM" = "real" ]; then else #Sim tmux new-window -t $SESSION:4 -n 'tcp-endpoint' tmux select-window -t $SESSION:4 - tmux send-keys "ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=127.0.0.1" + tmux send-keys "ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=127.0.0.1" C-m fi if [ "$REALSIM" = "real" ]; then @@ -171,7 +171,7 @@ tmux split-window -h -t $SESSION:13.0 #Pointcloud preprocessing tmux select-pane -t $SESSION:13.0 -tmux send-keys "ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_boat.py" C-m +tmux send-keys "ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_evolo.py use_sim_time:=$USE_SIM_TIME" C-m #occupancy grid tmux select-pane -t $SESSION:13.1 #tmux send-keys "ros2 run clustering_segmentation clustering_segmentation --ros-args -p use_sim_time:=$USE_SIM_TIME" C-m From c5219a963f190681c9c8959918730e243056cd2b Mon Sep 17 00:00:00 2001 From: Baptiste Date: Mon, 23 Feb 2026 17:53:40 +0100 Subject: [PATCH 3/4] added the requested parts in the comments and rosparams file --- .../evolo_move_path/config/evolo_params.yaml | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 behaviours/evolo/evolo_move_path/config/evolo_params.yaml diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml new file mode 100644 index 000000000..0b1f780c0 --- /dev/null +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -0,0 +1,22 @@ +evolo_move_path_action_server: + ros__parameters: + frame_id: "evolo/odom" + timeout: 600.0 + + # Speed and limits + v_min: 8.0 + v_max: 14.0 + omega_max: 16.0 + err_large_deg: 45.0 + + # Pure Pursuit & Dubins + ld_base: 15.0 + ld_gain: 0.5 + ff_gain: 0.2 + min_turning_radius: 30.0 + dubins_step: 0.5 + + # Speed Map + speed_map_slow: 8.0 + speed_map_medium: 11.0 + speed_map_high: 14.0 \ No newline at end of file From 65ee18a66108470d2a46f73d2bbf902f20f6de2f Mon Sep 17 00:00:00 2001 From: Baptiste Date: Tue, 24 Feb 2026 15:18:25 +0100 Subject: [PATCH 4/4] using previous dubins algorithm --- .../evolo_move_path/config/evolo_params.yaml | 6 +- .../evolo_move_path/dubins_algorithm.py | 117 ------------------ .../evolo_move_path_server_dubins_curves.py | 98 +++++++-------- 3 files changed, 51 insertions(+), 170 deletions(-) delete mode 100644 behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py diff --git a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml index 0b1f780c0..503169731 100644 --- a/behaviours/evolo/evolo_move_path/config/evolo_params.yaml +++ b/behaviours/evolo/evolo_move_path/config/evolo_params.yaml @@ -10,10 +10,10 @@ evolo_move_path_action_server: err_large_deg: 45.0 # Pure Pursuit & Dubins - ld_base: 15.0 + ld_base: 20.0 ld_gain: 0.5 - ff_gain: 0.2 - min_turning_radius: 30.0 + ff_gain: 0.1 + min_turning_radius: 45.0 dubins_step: 0.5 # Speed Map diff --git a/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py b/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py deleted file mode 100644 index 94ea8f81a..000000000 --- a/behaviours/evolo/evolo_move_path/evolo_move_path/dubins_algorithm.py +++ /dev/null @@ -1,117 +0,0 @@ -import math - -# ───────────────────────────────────────────────────────────────────────────── -# Dubins Algorithm -# ───────────────────────────────────────────────────────────────────────────── - -def _mod2pi(x: float) -> float: - return x % (2.0 * math.pi) - -def _dubins_LSL(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - tmp0 = d + sa - sb - p_sq = 2.0 + d*d - 2.0*math.cos(alpha - beta) + 2.0*d*(sa - sb) - if p_sq < 0: return None - tmp1 = math.atan2(cb - ca, tmp0) - return _mod2pi(-alpha + tmp1), math.sqrt(p_sq), _mod2pi(beta - tmp1), 'LSL' - -def _dubins_RSR(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - tmp0 = d - sa + sb - p_sq = 2.0 + d*d - 2.0*math.cos(alpha - beta) + 2.0*d*(sb - sa) - if p_sq < 0: return None - tmp1 = math.atan2(ca - cb, tmp0) - return _mod2pi(alpha - tmp1), math.sqrt(p_sq), _mod2pi(-beta + tmp1), 'RSR' - -def _dubins_LSR(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - p_sq = -2.0 + d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(sa + sb) - if p_sq < 0: return None - p = math.sqrt(p_sq) - tmp1 = math.atan2(-ca - cb, d + sa + sb) - math.atan2(-2.0, p) - return _mod2pi(-alpha + tmp1), p, _mod2pi(-_mod2pi(beta) + tmp1), 'LSR' - -def _dubins_RSL(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - p_sq = -2.0 + d*d + 2.0*math.cos(alpha - beta) - 2.0*d*(sa + sb) - if p_sq < 0: return None - p = math.sqrt(p_sq) - tmp1 = math.atan2(ca + cb, d - sa - sb) - math.atan2(2.0, p) - return _mod2pi(alpha - tmp1), p, _mod2pi(beta - tmp1), 'RSL' - -def _dubins_RLR(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - tmp0 = (6.0 - d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(sa - sb)) / 8.0 - if abs(tmp0) > 1.0: return None - p = _mod2pi(2.0*math.pi - math.acos(tmp0)) - t = _mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + _mod2pi(p / 2.0)) - return t, p, _mod2pi(alpha - beta - t + _mod2pi(p)), 'RLR' - -def _dubins_LRL(alpha, beta, d): - sa, ca = math.sin(alpha), math.cos(alpha) - sb, cb = math.sin(beta), math.cos(beta) - tmp0 = (6.0 - d*d + 2.0*math.cos(alpha - beta) + 2.0*d*(-sa + sb)) / 8.0 - if abs(tmp0) > 1.0: return None - p = _mod2pi(2.0*math.pi - math.acos(tmp0)) - t = _mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) - return t, p, _mod2pi(_mod2pi(beta) - alpha - t + _mod2pi(p)), 'LRL' - -def _sample_segment(x0, y0, yaw0, seg_type, length, radius, step): - pts, dist, x, y, yaw = [], 0.0, x0, y0, yaw0 - while dist < length * radius: - pts.append((x, y, yaw)) - if seg_type == 'S': - x += step * math.cos(yaw); y += step * math.sin(yaw) - elif seg_type == 'L': - dy = step / radius - x += radius * (math.sin(yaw + dy) - math.sin(yaw)) - y += radius * (-math.cos(yaw + dy) + math.cos(yaw)) - yaw += dy - elif seg_type == 'R': - dy = step / radius - x += radius * (-math.sin(yaw - dy) + math.sin(yaw)) - y += radius * (math.cos(yaw - dy) - math.cos(yaw)) - yaw -= dy - dist += step - return pts - -def dubins_path(q0, q1, radius, step): - x0, y0, yaw0 = q0 - x1, y1, yaw1 = q1 - dx, dy = x1 - x0, y1 - y0 - D = math.hypot(dx, dy) - d = D / radius - theta = _mod2pi(math.atan2(dy, dx)) - alpha = _mod2pi(yaw0 - theta) - beta = _mod2pi(yaw1 - theta) - - best, best_len = None, float('inf') - for fn in [_dubins_LSL, _dubins_RSR, _dubins_LSR, - _dubins_RSL, _dubins_RLR, _dubins_LRL]: - c = fn(alpha, beta, d) - if c is None: continue - t, p, q, mode = c - if t + p + q < best_len: - best_len = t + p + q - best = c - - if best is None: - n = max(2, int(D / step)) - return [(x0 + i/n*dx, y0 + i/n*dy, math.atan2(dy, dx)) for i in range(n+1)] - - t, p, q, mode = best - path_pts, cx, cy, cyaw = [], x0, y0, yaw0 - for seg_type, length in zip(list(mode), [t, p, q]): - if length < 1e-6: continue - seg = _sample_segment(cx, cy, cyaw, seg_type, length, radius, step) - if seg: - path_pts.extend(seg) - cx, cy, cyaw = seg[-1] - path_pts.append((x1, y1, yaw1)) - return path_pts - 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 6d058fc3b..018c8bcc3 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 @@ -39,7 +39,7 @@ from enum import Enum -from .dubins_algorithm import dubins_path +from dubins_planner.dubins import Waypoint, calc_dubins_path, dubins_traj @@ -53,11 +53,11 @@ class PurePursuitController: """ def __init__(self, - Ld_base: float = 15.0, - Ld_gain: float = 0.5, - omega_max: float = 16.0, - ff_gain: float = 0.2, - dubins_step: float = 0.5): + 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 @@ -132,20 +132,20 @@ def __init__(self, node: Node, action_name: str): self._node.declare_parameters( namespace='', parameters=[ - ('v_min', 8.0), - ('v_max', 14.0), - ('omega_max', 16.0), - ('err_large_deg', 45.0), - ('ld_base', 15.0), - ('ld_gain', 0.5), - ('ff_gain', 0.2), - ('min_turning_radius', 30.0), - ('dubins_step', 0.5), - ('timeout', 600.0), - ('speed_map_slow', 8.0), - ('speed_map_medium', 11.0), - ('speed_map_high', 14.0), - ('frame_id', 'evolo/odom') + ('v_min', rclpy.Parameter.Type.DOUBLE), + ('v_max', rclpy.Parameter.Type.DOUBLE), + ('omega_max', rclpy.Parameter.Type.DOUBLE), + ('err_large_deg', 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 @@ -205,14 +205,14 @@ def __init__(self, node: Node, action_name: str): self.evolo_pub = self._node.create_publisher(Float32, controlTopics.CONTROL_YAW_TOPIC, 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.path_pub = self._node.create_publisher(Path, 'visual_path', 10, callback_group=self.publisher_callback_group) self.viz_pub = self._node.create_publisher(MarkerArray, 'visualisation', 10, callback_group=self.publisher_callback_group) self.dubins_path_pub = self._node.create_publisher(Path, 'dubins_path', 10, callback_group=self.publisher_callback_group) self.attractor_pub = self._node.create_publisher(Marker, 'attractor_marker', 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, 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) self._node.get_logger().info("EvoloMovePath started") @@ -265,17 +265,17 @@ def _prepare_loop(self) -> None: def _get_local_curvature(self, path: list, cursor: int) -> float: - if cursor + 4 >= len(path): + if cursor + 2 >= len(path): return 0.0 x1, y1, yaw1 = path[cursor] - x2, y2, yaw2 = path[cursor + 2] - x3, y3, yaw3 = path[cursor + 4] + 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) / 4.0 + avg_dyaw = (dyaw1 + dyaw2) / 2.0 kappa = avg_dyaw / self.DUBINS_STEP return kappa @@ -347,23 +347,17 @@ def _loop_inner(self) -> bool | None: # Control v = current_wp.speed_kn - if abs_err_deg > self.ERR_LARGE_DEG: - omega = math.copysign(self.OMEGA_MAX, angle_error) - v = self.V_MIN - mode = "TURN" - - else: - omega, lookahead_idx = 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, - path = path, - cursor = self.path_cursor, - ) - slow_factor = max(0.0, 1.0 - abs_err_deg / self.ERR_LARGE_DEG) - v = self.V_MIN + slow_factor * (v - self.V_MIN) - mode = "PP" + omega, lookahead_idx = 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, + path = path, + cursor = self.path_cursor, + ) + slow_factor = max(0.0, 1.0 - abs_err_deg / self.ERR_LARGE_DEG) + v = self.V_MIN + slow_factor * (v - self.V_MIN) + mode = "PP" # Publication cmd = TwistStamped() @@ -414,14 +408,18 @@ def _plan_global_dubins(self) -> bool: wp_ori.z, wp_ori.w])[2]) q_next = (wp_pos.x, wp_pos.y, target_yaw) try: - seg = dubins_path(q_prev, q_next, - radius=self.MIN_TURNING_RADIUS, step=self.DUBINS_STEP) + 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) wp_ends.append(len(full_path) - 1) - self._node.get_logger().info( - f" Seg{i+1}: {len(seg)} pts → idx={wp_ends[-1]}") + except Exception as e: - self._node.get_logger().error(f"Dubins seg{i+1} failed: {e}") + self._node.get_logger().error(f"Dubins failed: {e}") return False q_prev = q_next @@ -595,7 +593,7 @@ def publish_waypoints_markers(self): # ───────────────────────────────────────────────────────────────────────────── def main(): rclpy.init() - node = Node("move_path_server_dubins_pp") + node = Node("evolo_move_path_action_server") EvoloMovePath(node, "move_path") executor = MultiThreadedExecutor() executor.add_node(node)