From 84a50051f173941241b28ff1570c51322236935b Mon Sep 17 00:00:00 2001 From: Elna Date: Fri, 24 Oct 2025 12:11:06 +0200 Subject: [PATCH 1/2] Add obstacle.py to utils --- src/svea_core/svea_core/utils/obstacle.py | 151 ++++++++++++++++++++++ 1 file changed, 151 insertions(+) create mode 100644 src/svea_core/svea_core/utils/obstacle.py diff --git a/src/svea_core/svea_core/utils/obstacle.py b/src/svea_core/svea_core/utils/obstacle.py new file mode 100644 index 00000000..2dd29776 --- /dev/null +++ b/src/svea_core/svea_core/utils/obstacle.py @@ -0,0 +1,151 @@ +from .. import rosonic as rx +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +from visualization_msgs.msg import Marker, MarkerArray +#from svea_msgs.msg import Circle, Circle_array +import tf2_ros + +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy +import rx + +qos_subber = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, # Reliable + history=QoSHistoryPolicy.KEEP_LAST, # Keep the last N messages + durability=QoSDurabilityPolicy.VOLATILE, # Volatile + depth=10, # Size of the queue +) + +qos_pubber = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, +) + + +class LaserScanToCircle(rx.Field): + + radius = rx.Parameter(0.5) # Default radius: 0.5 + min_elements = rx.Parameter( 5) # Default min_elements: 5 + sparsity = rx.Parameter( False) + + marker_pub = rx.Publisher(MarkerArray, "vis_circles", qos_pubber) + #obstacles_publisher = rx.Publisher(Circle_array, "/obstacles",qos_pubber) + + @rx.Subscriber(LaserScan, "/scan" , qos_subber) + def scan_callback(self, scan_msg): + points = self.convert_scan_to_points(scan_msg) + + circles = self.calculate_circles(points) + + if self.sparsity: + circles = self.enforce_sparsity(circles) + + transformed_circles = self.transform_circles(circles, "base_link", "map") + + self.publish_circles(transformed_circles) # Visualization + # self.publish_obstacles(transformed_circles) # For the MPC + + def convert_scan_to_points(self, scan_msg): + """Convert LaserScan message to (x, y) points.""" + angles = np.linspace(scan_msg.angle_min, scan_msg.angle_max, len(scan_msg.ranges)) + ranges = np.array(scan_msg.ranges) + + valid_mask = np.isfinite(ranges) & (ranges > 0) + ranges = ranges[valid_mask] + angles = angles[valid_mask] + + x = ranges * np.cos(angles) + 0.3 + y = ranges * np.sin(angles) + + return np.stack((x, y), axis=-1) + + def calculate_circles(self, points): + """Group points into circles, assuming points are in order left to right.""" + if len(points) == 0: + return [] + + circles = [] + current_center = points[0] + current_count = 1 + + for i in range(1, len(points)): + distance = np.linalg.norm(points[i] - current_center) + if distance <= self.radius: + current_count += 1 + else: + # Add the current circle if it meets the minimum elements condition + if current_count >= self.min_elements: + circles.append((current_center[0], current_center[1])) + # Start a new circle + current_center = points[i] + current_count = 1 + + # Add the last circle if it meets the condition + if current_count >= self.min_elements: + circles.append((current_center[0], current_center[1])) + + return circles + + + def transform_circles(self, circles, from_frame, to_frame): + try: + transform = self.tf_buffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0)) + + translation = np.array([transform.transform.translation.x, + transform.transform.translation.y]) + + q = transform.transform.rotation + cos_theta = 1 - 2 * (q.y ** 2 + q.z ** 2) + sin_theta = 2 * (q.w * q.z + q.x * q.y) + rotation_matrix_2d = np.array([[cos_theta, -sin_theta], + [sin_theta, cos_theta]]) + + circles_np = np.array(circles) + if not len(circles) == 0: + transformed_circles = circles_np @ rotation_matrix_2d.T + translation + else: + transformed_circles = circles_np + return transformed_circles.tolist() + + except tf2_ros.TransformException as ex: + rospy.logerr(f"Transform error: {ex}") + return circles # Return unchanged circles on failure + + def publish_circles(self, circles): + """Publish the circles as markers for visualization.""" + marker_array = MarkerArray() + for i, (center_x, center_y) in enumerate(circles): + marker = Marker() + marker.header.frame_id = "map" # Adjust frame ID as needed + marker.header.stamp = rospy.Time.now() + marker.ns = "vis_circles" + marker.id = i + marker.type = Marker.CYLINDER + marker.action = Marker.ADD + marker.pose.position.x = center_x + marker.pose.position.y = center_y + marker.pose.position.z = 0 # Flat on the ground + marker.pose.orientation.w = 1.0 + marker.scale.x = self.radius * 2 # Diameter in x + marker.scale.y = self.radius * 2 # Diameter in y + marker.scale.z = 0.01 # Small height for visualization + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.5 # Semi-transparent + marker.lifetime = rospy.Duration(0.1) # Short lifetime to refresh with each scan + + marker_array.markers.append(marker) + + self.marker_pub.publish(marker_array) + + def enforce_sparsity(self, circles, epsilon=0.001): + """Filter circles to enforce sparsity by removing overlapping circles.""" + sparse_circles = [] + for circle in circles: + if all(np.linalg.norm(np.array(circle) - np.array(existing)) > self.radius * 2 + epsilon + for existing in sparse_circles): + sparse_circles.append(circle) + return sparse_circles From 77889c0d993045a3ee9f8301e0be9e24e1a73d05 Mon Sep 17 00:00:00 2001 From: Elna Date: Fri, 24 Oct 2025 12:18:52 +0200 Subject: [PATCH 2/2] Add obstacle.py to utils --- src/svea_core/svea_core/utils/obstacle.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/svea_core/svea_core/utils/obstacle.py b/src/svea_core/svea_core/utils/obstacle.py index 2dd29776..8f04676d 100644 --- a/src/svea_core/svea_core/utils/obstacle.py +++ b/src/svea_core/svea_core/utils/obstacle.py @@ -6,6 +6,7 @@ #from svea_msgs.msg import Circle, Circle_array import tf2_ros + from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy import rx