Feature/ws2812 effects

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.

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

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)
 

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))

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'

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()