|
| 1 | +#!/usr/bin/env python3 |
| 2 | +import rclpy |
| 3 | +from rclpy.node import Node |
| 4 | +import pygame |
| 5 | +import math |
| 6 | +from geometry_msgs.msg import Point |
| 7 | +from coffee_expressions.msg import AffectiveState |
| 8 | + |
| 9 | +class BezierCurve: |
| 10 | + @staticmethod |
| 11 | + def quadratic(p0, p1, p2, t): |
| 12 | + x = (1 - t) * (1 - t) * p0[0] + 2 * (1 - t) * t * p1[0] + t * t * p2[0] |
| 13 | + y = (1 - t) * (1 - t) * p0[1] + 2 * (1 - t) * t * p1[1] + t * t * p2[1] |
| 14 | + return (x, y) |
| 15 | + |
| 16 | +class Eye: |
| 17 | + def __init__(self, center_pos, size): |
| 18 | + self.center = center_pos |
| 19 | + self.size = size |
| 20 | + self.pupil_pos = (0, 0) # Normalized position (-1 to 1) |
| 21 | + self.current_expression = "neutral" |
| 22 | + self.expression_transition = 0.0 |
| 23 | + self.target_control_points = self._get_expression_control_points("neutral") |
| 24 | + self.current_control_points = self._get_expression_control_points("neutral") |
| 25 | + |
| 26 | + def _get_expression_control_points(self, expression): |
| 27 | + """Get Bezier control points for different expressions""" |
| 28 | + if expression == "happy": |
| 29 | + return [ |
| 30 | + [(0, -0.8), (0.5, -1.0), (1.0, -0.8)], # Upper curve |
| 31 | + [(1.0, -0.8), (0.5, -0.4), (0, -0.8)] # Lower curve |
| 32 | + ] |
| 33 | + elif expression == "curious": |
| 34 | + return [ |
| 35 | + [(0, -1.0), (0.5, -1.2), (1.0, -1.0)], # Upper curve - raised |
| 36 | + [(1.0, -1.0), (0.5, -0.6), (0, -1.0)] # Lower curve - normal |
| 37 | + ] |
| 38 | + else: # neutral |
| 39 | + return [ |
| 40 | + [(0, -1.0), (0.5, -1.0), (1.0, -1.0)], # Upper curve |
| 41 | + [(1.0, -1.0), (0.5, -1.0), (0, -1.0)] # Lower curve |
| 42 | + ] |
| 43 | + |
| 44 | + def update_expression(self, expression, dt): |
| 45 | + """Update the eye expression with smooth transition""" |
| 46 | + if expression != self.current_expression: |
| 47 | + self.target_control_points = self._get_expression_control_points(expression) |
| 48 | + self.current_expression = expression |
| 49 | + self.expression_transition = 0.0 |
| 50 | + |
| 51 | + # Update transition |
| 52 | + if self.expression_transition < 1.0: |
| 53 | + self.expression_transition = min(1.0, self.expression_transition + dt * 2.0) |
| 54 | + target = self.target_control_points |
| 55 | + current = self.current_control_points |
| 56 | + |
| 57 | + # Interpolate control points |
| 58 | + for i in range(len(current)): |
| 59 | + for j in range(len(current[i])): |
| 60 | + current[i][j] = ( |
| 61 | + current[i][j][0] + (target[i][j][0] - current[i][j][0]) * self.expression_transition, |
| 62 | + current[i][j][1] + (target[i][j][1] - current[i][j][1]) * self.expression_transition |
| 63 | + ) |
| 64 | + |
| 65 | + def update_gaze(self, target_pos): |
| 66 | + """Update pupil position based on gaze target""" |
| 67 | + self.pupil_pos = ( |
| 68 | + max(-1.0, min(1.0, target_pos[0])), |
| 69 | + max(-1.0, min(1.0, target_pos[1])) |
| 70 | + ) |
| 71 | + |
| 72 | + def draw(self, surface): |
| 73 | + """Draw the eye using Bezier curves""" |
| 74 | + # Draw white background |
| 75 | + pygame.draw.ellipse(surface, (255, 255, 255), |
| 76 | + (self.center[0] - self.size//2, |
| 77 | + self.center[1] - self.size//2, |
| 78 | + self.size, self.size)) |
| 79 | + |
| 80 | + # Draw eye shape using Bezier curves |
| 81 | + points_upper = [] |
| 82 | + points_lower = [] |
| 83 | + steps = 20 |
| 84 | + |
| 85 | + for i in range(steps + 1): |
| 86 | + t = i / steps |
| 87 | + # Scale and position the control points |
| 88 | + cp_upper = [(self.center[0] + p[0] * self.size//2, |
| 89 | + self.center[1] + p[1] * self.size//2) |
| 90 | + for p in self.current_control_points[0]] |
| 91 | + cp_lower = [(self.center[0] + p[0] * self.size//2, |
| 92 | + self.center[1] + p[1] * self.size//2) |
| 93 | + for p in self.current_control_points[1]] |
| 94 | + |
| 95 | + points_upper.append(BezierCurve.quadratic(cp_upper[0], cp_upper[1], cp_upper[2], t)) |
| 96 | + points_lower.append(BezierCurve.quadratic(cp_lower[0], cp_lower[1], cp_lower[2], t)) |
| 97 | + |
| 98 | + # Draw the curves |
| 99 | + if len(points_upper) > 1: |
| 100 | + pygame.draw.lines(surface, (0, 0, 0), False, points_upper, 2) |
| 101 | + if len(points_lower) > 1: |
| 102 | + pygame.draw.lines(surface, (0, 0, 0), False, points_lower, 2) |
| 103 | + |
| 104 | + # Draw pupil |
| 105 | + pupil_x = self.center[0] + self.pupil_pos[0] * self.size//4 |
| 106 | + pupil_y = self.center[1] + self.pupil_pos[1] * self.size//4 |
| 107 | + pupil_size = self.size // 5 |
| 108 | + pygame.draw.circle(surface, (0, 0, 0), |
| 109 | + (int(pupil_x), int(pupil_y)), |
| 110 | + pupil_size) |
| 111 | + |
| 112 | +class ExpressiveEyes(Node): |
| 113 | + def __init__(self): |
| 114 | + super().__init__('expressive_eyes') |
| 115 | + |
| 116 | + # Initialize Pygame |
| 117 | + pygame.init() |
| 118 | + self.screen_width = 800 |
| 119 | + self.screen_height = 400 |
| 120 | + self.screen = pygame.display.set_mode((self.screen_width, self.screen_height)) |
| 121 | + pygame.display.set_caption("Coffee Buddy - Expressive Eyes") |
| 122 | + |
| 123 | + # Create eyes |
| 124 | + eye_size = 200 |
| 125 | + left_center = (self.screen_width//4, self.screen_height//2) |
| 126 | + right_center = (3*self.screen_width//4, self.screen_height//2) |
| 127 | + self.left_eye = Eye(left_center, eye_size) |
| 128 | + self.right_eye = Eye(right_center, eye_size) |
| 129 | + |
| 130 | + # Initialize state |
| 131 | + self.current_expression = "neutral" |
| 132 | + self.last_update = self.get_clock().now() |
| 133 | + |
| 134 | + # Create subscription |
| 135 | + self.subscription = self.create_subscription( |
| 136 | + AffectiveState, |
| 137 | + 'affective_state', |
| 138 | + self.affective_state_callback, |
| 139 | + 10) |
| 140 | + |
| 141 | + # Create timer for animation updates |
| 142 | + self.create_timer(0.016, self.update_animation) # ~60 FPS |
| 143 | + |
| 144 | + def affective_state_callback(self, msg): |
| 145 | + """Handle incoming affective state messages""" |
| 146 | + self.current_expression = msg.expression.lower() |
| 147 | + |
| 148 | + # Convert gaze target to normalized coordinates |
| 149 | + # Assuming gaze_target is in meters, scale appropriately |
| 150 | + scale = 2.0 # Adjust this to control gaze sensitivity |
| 151 | + gaze_x = msg.gaze_target.x * scale |
| 152 | + gaze_y = msg.gaze_target.y * scale |
| 153 | + |
| 154 | + self.left_eye.update_gaze((gaze_x, gaze_y)) |
| 155 | + self.right_eye.update_gaze((gaze_x, gaze_y)) |
| 156 | + |
| 157 | + def update_animation(self): |
| 158 | + """Update animation state and render""" |
| 159 | + # Calculate delta time |
| 160 | + now = self.get_clock().now() |
| 161 | + dt = (now - self.last_update).nanoseconds / 1e9 |
| 162 | + self.last_update = now |
| 163 | + |
| 164 | + # Update expressions |
| 165 | + self.left_eye.update_expression(self.current_expression, dt) |
| 166 | + self.right_eye.update_expression(self.current_expression, dt) |
| 167 | + |
| 168 | + # Clear screen |
| 169 | + self.screen.fill((200, 200, 200)) # Light gray background |
| 170 | + |
| 171 | + # Draw eyes |
| 172 | + self.left_eye.draw(self.screen) |
| 173 | + self.right_eye.draw(self.screen) |
| 174 | + |
| 175 | + # Update display |
| 176 | + pygame.display.flip() |
| 177 | + |
| 178 | + # Handle Pygame events |
| 179 | + for event in pygame.event.get(): |
| 180 | + if event.type == pygame.QUIT: |
| 181 | + self.destroy_node() |
| 182 | + pygame.quit() |
| 183 | + rclpy.shutdown() |
| 184 | + |
| 185 | + def destroy_node(self): |
| 186 | + """Clean up resources""" |
| 187 | + pygame.quit() |
| 188 | + super().destroy_node() |
| 189 | + |
| 190 | +def main(args=None): |
| 191 | + rclpy.init(args=args) |
| 192 | + node = ExpressiveEyes() |
| 193 | + try: |
| 194 | + rclpy.spin(node) |
| 195 | + except KeyboardInterrupt: |
| 196 | + pass |
| 197 | + finally: |
| 198 | + node.destroy_node() |
| 199 | + rclpy.shutdown() |
| 200 | + |
| 201 | +if __name__ == '__main__': |
| 202 | + main() |
0 commit comments