diff --git a/blocks/actuators/leds/ws2812_effects/README.md b/blocks/actuators/leds/ws2812_effects/README.md index f923672..cd7bad6 100644 --- a/blocks/actuators/leds/ws2812_effects/README.md +++ b/blocks/actuators/leds/ws2812_effects/README.md @@ -103,6 +103,8 @@ The node includes a built-in library `xpi_actuators.lib.led_effects`. * [x] **Pac-Man** * [x] **Conveyor Belt** * [x] **Marquee** - Theater border effect. +* [x] **Candy Cane** - Rotating red/white stripes. +* [x] **Juggle** - 8 balls weaving. ### ✨ Group 4: Sparkles & Weather * [x] **Sparkle** - Random white flashes. @@ -116,19 +118,21 @@ The node includes a built-in library `xpi_actuators.lib.led_effects`. * [x] **Storm** * [x] **Snowfall** * [ ] **Drizzle** -* [ ] **Confetti** +* [x] **Confetti** - Random colored speckles. * [ ] **Popcorn** * [ ] **Explosion** * [ ] **Flicker (Candle)** ### 🔥 Group 5: Physics & Fluids * [x] **Fire** - Burning fire simulation. +* [x] **Fire 2012** - Realistic 1D fire algorithm. * [x] **Blue Fire** - Blue flame. * [x] **Ice Fire** - White/Cyan flame. * [x] **Lava** * [x] **Water** - Flowing sine waves. * [ ] **Ripple** * [x] **Plasma** - Morphing color blobs. +* [x] **Fire Plasma** - Green/Purple fire. * [x] **Bubble** - Rising bubbles. * [ ] **Bouncing Balls** * [ ] **Multi-Ball** @@ -140,6 +144,7 @@ The node includes a built-in library `xpi_actuators.lib.led_effects`. * [x] **Calm Ocean** - Slow morphing blues. * [x] **Sunny Forest** - Foliage with sunbeams. * [x] **Aurora Borealis** - Dancing polar lights. +* [x] **Aurora Fast** - Dynamic solar storm. * [x] **Zen Pulse** - Ultra-slow breathing. * [x] **Morning Mist** - Drifting fog. * [x] **Autumn Leaves** - Golden drift on red. diff --git a/blocks/sensors/navigation/gps_rtk/README.md b/blocks/sensors/navigation/gps_rtk/README.md index 53469a2..a2aa887 100644 --- a/blocks/sensors/navigation/gps_rtk/README.md +++ b/blocks/sensors/navigation/gps_rtk/README.md @@ -36,6 +36,7 @@ ros2 launch xpi_sensors gps_rtk.launch.py ntrip_mount:=YOUR_MOUNTPOINT | :--- | :--- | :--- | :--- | | `port` | string | `/dev/ttyUSB0` | Serial port path | | `baudrate` | int | `38400` | Baud rate (ZED-F9P default is 38400) | +| `frequency` | int | `5` | Update Rate in Hz (set via UBX-CFG-RATE) | ### ntrip_client_node | Parameter | Type | Default | Description | @@ -47,7 +48,8 @@ ros2 launch xpi_sensors gps_rtk.launch.py ntrip_mount:=YOUR_MOUNTPOINT ## 📡 ROS2 Interface ### Publishers -* `~/fix` (`sensor_msgs/NavSatFix`): High-precision position data. +* `~/fix` (`sensor_msgs/NavSatFix`): High-precision position data (with covariance). +* `~/vel` (`geometry_msgs/TwistWithCovarianceStamped`): Ground velocity and heading. * `~/rtk_status` (`std_msgs/Int32`): 0=No Fix, 1=3D, 2=Float RTK, 3=Fixed RTK. ### Subscribers diff --git a/src/xpi_actuators/xpi_actuators/lib/led_effects/__init__.py b/src/xpi_actuators/xpi_actuators/lib/led_effects/__init__.py index 0e28f5a..6a3e101 100644 --- a/src/xpi_actuators/xpi_actuators/lib/led_effects/__init__.py +++ b/src/xpi_actuators/xpi_actuators/lib/led_effects/__init__.py @@ -12,8 +12,9 @@ from .palettes import PaletteEffectsMixin from .special import SpecialEffectsMixin from .indication import IndicationEffectsMixin +from .extra import ExtraEffectsMixin -class LedEffects(EffectBase, BasicEffectsMixin, RainbowEffectsMixin, ChaseEffectsMixin, SparkleEffectsMixin, PhysicsEffectsMixin, MeditativeEffectsMixin, RhythmicEffectsMixin, ReactiveEffectsMixin, UtilityEffectsMixin, PaletteEffectsMixin, SpecialEffectsMixin, IndicationEffectsMixin): +class LedEffects(EffectBase, BasicEffectsMixin, RainbowEffectsMixin, ChaseEffectsMixin, SparkleEffectsMixin, PhysicsEffectsMixin, MeditativeEffectsMixin, RhythmicEffectsMixin, ReactiveEffectsMixin, UtilityEffectsMixin, PaletteEffectsMixin, SpecialEffectsMixin, IndicationEffectsMixin, ExtraEffectsMixin): def __init__(self, num_pixels): super().__init__(num_pixels) diff --git a/src/xpi_actuators/xpi_actuators/lib/led_effects/extra.py b/src/xpi_actuators/xpi_actuators/lib/led_effects/extra.py new file mode 100644 index 0000000..b236644 --- /dev/null +++ b/src/xpi_actuators/xpi_actuators/lib/led_effects/extra.py @@ -0,0 +1,137 @@ +import random +import time +import math +from .base import hsv_to_rgb + +class ExtraEffectsMixin: + def effect_police(self, speed=5.0): + # Red and Blue strobe + # Pattern: R R R - B B B - R R R - B B B + t = int(time.time() * 10 * speed) + phase = t % 4 + + self.clear() + if phase == 0: + for i in range(0, self.num_pixels // 2): + self.set_pixel(i, (255, 0, 0)) + elif phase == 2: + for i in range(self.num_pixels // 2, self.num_pixels): + self.set_pixel(i, (0, 0, 255)) + # Phases 1 and 3 are black (pause) + + def effect_fire_2012(self, cooling=55, sparking=120, speed=15.0): + # Classic Fire 2012 algorithm tailored for 1D strip + # Requires persistent state for heat + if not hasattr(self, '_heat'): + self._heat = [0] * self.num_pixels + + # 1. Cool down every cell a little + for i in range(self.num_pixels): + cooldown = random.randint(0, int(((cooling * 10) / self.num_pixels) + 2)) + self._heat[i] = max(0, self._heat[i] - cooldown) + + # 2. Heat from each cell drifts 'up' and diffuses a little + for i in range(self.num_pixels - 1, 2, -1): + self._heat[i] = (self._heat[i - 1] + self._heat[i - 2] + self._heat[i - 2]) // 3 + + # 3. Randomly ignite new 'sparks' near the bottom + if random.randint(0, 255) < sparking: + y = random.randint(0, 7) + if y < self.num_pixels: + self._heat[y] = min(255, self._heat[y] + random.randint(160, 255)) + + # 4. Convert heat to color + for i in range(self.num_pixels): + self.set_pixel(i, self._heat_to_color(self._heat[i])) + + def _heat_to_color(self, temperature): + # Heat is 0-255 + # 0 -> Black + # 255 -> White + # Scale 'Heat' down from 0-255 to 0-191 + t192 = int((temperature / 255.0) * 191) + + # Calculate ramp up from + heatramp = t192 & 0x3F # 0..63 + heatramp <<= 2 # scale up to 0..252 + + if t192 > 0x80: # Hottest + return (255, 255, heatramp) + elif t192 > 0x40: # Middle + return (255, heatramp, 0) + else: # Coolest + return (heatramp, 0, 0) + + def effect_candy_cane(self, speed=2.0): + # Red and White stripes moving + self.step += speed + offset = int(self.step) + for i in range(self.num_pixels): + if (i + offset) % 10 < 5: + self.set_pixel(i, (255, 0, 0)) # Red + else: + self.set_pixel(i, (200, 200, 200)) # White (dimmed) + + def effect_confetti(self, speed=1.0): + # Random colored speckles that blink in and fade smoothly + self.fade_to_black(10) + if random.random() < (0.05 * speed): + idx = random.randint(0, self.num_pixels - 1) + # Random HSV color + rgb = hsv_to_rgb(random.random(), 1.0, 1.0) + self.set_pixel(idx, rgb) + + def effect_juggle(self, speed=1.0): + # Eight colored dots, weaving in and out of sync with each other + self.fade_to_black(20) + curr_time = time.time() * speed + for i in range(8): + # i/8.0 moves the wave phase + pos = int((self.num_pixels - 1) * ((math.sin(curr_time + i/2.0) + 1.0) / 2.0)) + # Color cycling + rgb = hsv_to_rgb((curr_time * 0.1 + i/8.0) % 1.0, 1.0, 1.0) + + # Blend with existing + # Simple overwrite or add? Add is better for crossing + r, g, b = self.pixels[pos] + nr, ng, nb = rgb + self.set_pixel(pos, (min(255, r+nr), min(255, g+ng), min(255, b+nb))) + + def effect_fire_blue(self, cooling=55, sparking=120, speed=15.0): + """Blue (Gas) Fire""" + self.effect_fire_2012(cooling, sparking, speed) + # Remap colors from Red/Yellow to Blue/Cyan + # Fire 2012 produces (Heat, Heat, 0) mostly. + # We want (0, Heat, Heat) or similar. + for i in range(self.num_pixels): + r, g, b = self.pixels[i] + # Swap channels: R->B, G->G, B->R (but B is usually 0) + # Standard fire: R=High, G=Med, B=Low + # Blue fire: R=Low, G=Med, B=High + self.set_pixel(i, (b, g, r)) + + def effect_fire_plasma(self, cooling=50, sparking=120, speed=15.0): + """Green/Purple Plasma Fire""" + self.effect_fire_2012(cooling, sparking, speed) + for i in range(self.num_pixels): + r, g, b = self.pixels[i] + # Map heat to Green/Purple + # R (Heat) -> G + # G (Heat/2) -> B + # B -> R (Purple tint) + self.set_pixel(i, (g, r, g)) + + def effect_aurora_fast(self, speed=0.8): + """Dynamic Solar Storm Aurora""" + t = time.time() * speed + for i in range(self.num_pixels): + # Faster, more turbulent waves + w1 = math.sin(i * 0.15 + t) + w2 = math.cos(i * 0.3 - t * 1.2) + mix = (w1 + w2) / 2.0 + + # Shift towards Red/Pink/Purple (Active Aurora) + hue = 0.8 + mix * 0.2 # Purple to Red range + sat = 0.9 + val = 0.5 + mix * 0.5 + self.set_pixel(i, hsv_to_rgb(hue, sat, val)) diff --git a/src/xpi_sensors/launch/gps_rtk.launch.py b/src/xpi_sensors/launch/gps_rtk.launch.py index 92c52fa..cd203cd 100644 --- a/src/xpi_sensors/launch/gps_rtk.launch.py +++ b/src/xpi_sensors/launch/gps_rtk.launch.py @@ -7,6 +7,7 @@ def generate_launch_description(): return LaunchDescription([ # Arguments DeclareLaunchArgument('port', default_value='/dev/ttyUSB0'), + DeclareLaunchArgument('frequency', default_value='5', description='GPS Update Rate (Hz)'), DeclareLaunchArgument('ntrip_mount', default_value=''), # 1. RTK GPS Node @@ -16,6 +17,7 @@ def generate_launch_description(): name='gps_rtk', parameters=[{ 'port': LaunchConfiguration('port'), + 'frequency': LaunchConfiguration('frequency'), 'baudrate': 38400 }], output='screen' diff --git a/src/xpi_sensors/xpi_sensors/gps_rtk_node.py b/src/xpi_sensors/xpi_sensors/gps_rtk_node.py index beaf9d4..d3aa2ca 100644 --- a/src/xpi_sensors/xpi_sensors/gps_rtk_node.py +++ b/src/xpi_sensors/xpi_sensors/gps_rtk_node.py @@ -2,15 +2,17 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import NavSatFix, NavSatStatus +from geometry_msgs.msg import TwistWithCovarianceStamped from std_msgs.msg import String, Int32 -from pyubx2 import UBXReader +from pyubx2 import UBXReader, UBXMessage import serial import threading +import math class GpsRtkNode(Node): """ ROS2 Driver for RTK-capable GPS modules (e.g. u-blox ZED-F9P). - Handles high-precision positioning and RTCM correction injection. + Handles high-precision positioning, RTCM correction injection, and velocity. """ def __init__(self): super().__init__('gps_rtk_node') @@ -19,13 +21,16 @@ def __init__(self): self.declare_parameter('port', '/dev/ttyUSB0') self.declare_parameter('baudrate', 38400) self.declare_parameter('frame_id', 'gps_link') + self.declare_parameter('frequency', 5) # Desired update rate in Hz self.port = self.get_parameter('port').value self.baud = self.get_parameter('baudrate').value self.frame_id = self.get_parameter('frame_id').value + self.freq = self.get_parameter('frequency').value # 2. Publishers self.fix_pub = self.create_publisher(NavSatFix, '~/fix', 10) + self.vel_pub = self.create_publisher(TwistWithCovarianceStamped, '~/vel', 10) self.status_pub = self.create_publisher(Int32, '~/rtk_status', 10) # 0=No, 1=3D, 2=Float, 3=Fixed # 3. Subscribers (For RTCM corrections) @@ -36,6 +41,10 @@ def __init__(self): self.ser = serial.Serial(self.port, self.baud, timeout=0.1) self.ubr = UBXReader(self.ser) self.get_logger().info(f"Connected to RTK GPS on {self.port} at {self.baud}") + + # Configure Rate + self.configure_rate(self.freq) + except Exception as e: self.get_logger().error(f"Failed to open serial port: {e}") return @@ -44,15 +53,29 @@ def __init__(self): self.thread = threading.Thread(target=self.read_loop, daemon=True) self.thread.start() + def configure_rate(self, rate_hz): + """Sends UBX-CFG-RATE to set the update frequency.""" + if rate_hz <= 0: return + meas_rate_ms = int(1000 / rate_hz) + + # CFG-RATE: msgClass=0x06, msgID=0x08 + msg = UBXMessage( + "CFG", + "CFG-RATE", + measRate=meas_rate_ms, + navRate=1, + timeRef=1 # 1 = GPS Time + ) + self.ser.write(msg.serialize()) + self.get_logger().info(f"Configured GPS update rate to {rate_hz}Hz ({meas_rate_ms}ms)") + def correction_callback(self, msg): """Injects RTCM corrections from topic into the hardware serial port.""" if hasattr(self, 'ser') and self.ser.is_open: - # Assuming payload is hex or raw bytes. For simplicity, we expect raw bytes string. try: data = bytes.fromhex(msg.data) self.ser.write(data) except ValueError: - # If not hex, try raw self.ser.write(msg.data.encode('utf-8')) def read_loop(self): @@ -64,11 +87,12 @@ def read_loop(self): # Look for UBX-NAV-PVT (Position, Velocity, Time) if parsed_data.identity == "NAV-PVT": self.publish_fix(parsed_data) + self.publish_vel(parsed_data) except Exception as e: self.get_logger().warn(f"Read error: {e}") def publish_fix(self, data): - """Converts UBX NAV-PVT data to ROS2 NavSatFix.""" + """Converts UBX NAV-PVT data to ROS2 NavSatFix with Covariance.""" msg = NavSatFix() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = self.frame_id @@ -78,9 +102,20 @@ def publish_fix(self, data): msg.longitude = data.lon * 1e-7 msg.altitude = float(data.hMSL * 1e-3) # Height above Mean Sea Level in meters + # Covariance + # hAcc and vAcc are in mm + h_acc_m = data.hAcc / 1000.0 + v_acc_m = data.vAcc / 1000.0 + + # Diagonal covariance matrix [E, N, U] -> [Lat, Lon, Alt] approximation + msg.position_covariance = [ + h_acc_m**2, 0.0, 0.0, + 0.0, h_acc_m**2, 0.0, + 0.0, 0.0, v_acc_m**2 + ] + msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN + # RTK Status Mapping - # carrSoln: 0 = no carrier phase, 1 = float, 2 = fixed - # fixType: 3 = 3D fix rtk_flag = data.carrSoln if rtk_flag == 2: # FIXED @@ -96,6 +131,35 @@ def publish_fix(self, data): self.fix_pub.publish(msg) self.status_pub.publish(Int32(data=status_val)) + def publish_vel(self, data): + """Publishes velocity and heading.""" + msg = TwistWithCovarianceStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.frame_id + + # Velocity in ENU frame? No, typically body frame for Twist, but GPS gives global track. + # gSpeed is ground speed (2D). headMot is heading of motion. + + speed_m_s = data.gSpeed / 1000.0 + heading_rad = math.radians(data.headMot * 1e-5) # headMot is 1e-5 deg + + # Convert polar (speed, heading) to Cartesian (x, y) relative to North? + # Standard Twist message usually implies body frame velocity for robots, + # but for GPS 'vel' topic it often means global velocity vector. + # Let's populate linear.x/y as East/North components. + + msg.twist.twist.linear.x = speed_m_s * math.sin(heading_rad) # East + msg.twist.twist.linear.y = speed_m_s * math.cos(heading_rad) # North + msg.twist.twist.linear.z = - (data.velD / 1000.0) # Down velocity to Up + + # Accuracy + s_acc_m_s = data.sAcc / 1000.0 + msg.twist.covariance[0] = s_acc_m_s**2 + msg.twist.covariance[7] = s_acc_m_s**2 + msg.twist.covariance[14] = s_acc_m_s**2 # Approximation + + self.vel_pub.publish(msg) + def main(args=None): rclpy.init(args=args) node = GpsRtkNode()