feat: Add Issue #473 - Event Logger with structured JSON logging
Comprehensive event logging system for SaltyBot with: Features: - MQTT subscription to /saltybot/* state topics (9 event types) - Structured JSON event logging to ~/.saltybot-data/events/YYYY-MM-DD.jsonl - Event types: encounter, voice_command, trick, e-stop, geofence, dock, error, boot, shutdown - Query service (time range + type filter) - Stats publisher (/saltybot/event_stats) with daily summaries - Dashboard live feed (/saltybot/event_feed) - Automatic log rotation: compress >7 days, delete >90 days - CSV export functionality - Thread-safe event processing Components: - EventLogger ROS2 node with background timers - Event dataclass with timestamp/type/source/data - File-based storage (JSONL + compressed archives) - Query API for time/type filtering - CSV export utility - Comprehensive unit tests (15 test cases) Configuration: - Data directory: ~/.saltybot-data/events - Rotation: 7 days compress, 90 days delete - Stats interval: 60 seconds - Rotation check: hourly Launch: - event_logger.launch.py - Standalone launch - Config: event_logger.yaml with topic mappings Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
02e9b15e6f
commit
d421d63c6f
7
jetson/ros2_ws/src/saltybot_curiosity/.gitignore
vendored
Normal file
7
jetson/ros2_ws/src/saltybot_curiosity/.gitignore
vendored
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
build/
|
||||||
|
install/
|
||||||
|
log/
|
||||||
|
*.pyc
|
||||||
|
__pycache__/
|
||||||
|
*.egg-info/
|
||||||
|
dist/
|
||||||
58
jetson/ros2_ws/src/saltybot_curiosity/README.md
Normal file
58
jetson/ros2_ws/src/saltybot_curiosity/README.md
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
# saltybot_curiosity
|
||||||
|
|
||||||
|
Autonomous curiosity behavior for SaltyBot — frontier exploration when idle.
|
||||||
|
|
||||||
|
## Features
|
||||||
|
|
||||||
|
- **Idle Detection**: Activates after >60s without people
|
||||||
|
- **Frontier Exploration**: Random walk toward unexplored areas
|
||||||
|
- **Sound Localization**: Turns toward detected sounds
|
||||||
|
- **Object Interest**: Approaches colorful/moving objects
|
||||||
|
- **Self-Narration**: TTS commentary during exploration
|
||||||
|
- **Safety**: Respects geofence, avoids obstacles
|
||||||
|
- **Auto-Timeout**: Returns home after 10 minutes
|
||||||
|
- **Curiosity Level**: Configurable 0-1.0 activation probability
|
||||||
|
|
||||||
|
## Launch
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch saltybot_curiosity curiosity.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
## Parameters
|
||||||
|
|
||||||
|
- `curiosity_level`: 0.0-1.0 probability of exploring when idle (default: 0.7)
|
||||||
|
- `idle_threshold_sec`: seconds of idle before activation (default: 60)
|
||||||
|
- `exploration_max_duration_sec`: max exploration time in seconds (default: 600)
|
||||||
|
- `exploration_radius_m`: search radius in meters (default: 5.0)
|
||||||
|
- `pan_tilt_step_deg`: pan-tilt sweep step (default: 15)
|
||||||
|
- `min_sound_activity`: sound detection threshold (default: 0.1)
|
||||||
|
|
||||||
|
## Topics
|
||||||
|
|
||||||
|
### Publishes
|
||||||
|
|
||||||
|
- `/saltybot/curiosity_state` (String): State updates
|
||||||
|
- `/saltybot/tts_request` (String): Self-narration text
|
||||||
|
- `/saltybot/pan_tilt_cmd` (String): Pan-tilt commands
|
||||||
|
- `/cmd_vel` (Twist): Velocity commands (if direct movement)
|
||||||
|
|
||||||
|
### Subscribes
|
||||||
|
|
||||||
|
- `/saltybot/audio_direction` (Float32): Sound bearing (degrees)
|
||||||
|
- `/saltybot/audio_activity` (Bool): Sound detected
|
||||||
|
- `/saltybot/person_detections` (Detection2DArray): People nearby
|
||||||
|
- `/saltybot/object_detections` (Detection2DArray): Interesting objects
|
||||||
|
- `/saltybot/battery_percent` (Float32): Battery level
|
||||||
|
- `/saltybot/geofence_status` (String): Geofence boundary info
|
||||||
|
|
||||||
|
## State Machine
|
||||||
|
|
||||||
|
```
|
||||||
|
IDLE → (is_idle && should_explore) → EXPLORING
|
||||||
|
EXPLORING → (timeout || person detected) → RETURNING
|
||||||
|
RETURNING → IDLE
|
||||||
|
```
|
||||||
|
|
||||||
|
## Issue #470
|
||||||
|
Implements autonomous curiosity exploration as specified in Issue #470.
|
||||||
@ -0,0 +1,13 @@
|
|||||||
|
curiosity_node:
|
||||||
|
ros__parameters:
|
||||||
|
# Curiosity activation parameters
|
||||||
|
curiosity_level: 0.7 # 0.0-1.0 — probability of exploring when idle
|
||||||
|
idle_threshold_sec: 60.0 # seconds without people to trigger exploration
|
||||||
|
exploration_max_duration_sec: 600.0 # 10 minutes max exploration
|
||||||
|
exploration_radius_m: 5.0 # exploration search radius in meters
|
||||||
|
|
||||||
|
# Pan-tilt control
|
||||||
|
pan_tilt_step_deg: 15.0 # pan-tilt sweep step size
|
||||||
|
|
||||||
|
# Audio sensitivity
|
||||||
|
min_sound_activity: 0.1 # minimum sound activity threshold (0.0-1.0)
|
||||||
@ -0,0 +1,41 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
import os
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
config_dir = os.path.join(
|
||||||
|
os.path.dirname(__file__),
|
||||||
|
'..',
|
||||||
|
'config'
|
||||||
|
)
|
||||||
|
|
||||||
|
curiosity_config = os.path.join(config_dir, 'curiosity_params.yaml')
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'curiosity_level',
|
||||||
|
default_value='0.7',
|
||||||
|
description='Curiosity activation probability (0.0-1.0)'
|
||||||
|
),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'idle_threshold_sec',
|
||||||
|
default_value='60',
|
||||||
|
description='Seconds of idle before exploration activates'
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package='saltybot_curiosity',
|
||||||
|
executable='curiosity_node',
|
||||||
|
name='curiosity_node',
|
||||||
|
parameters=[
|
||||||
|
curiosity_config,
|
||||||
|
{
|
||||||
|
'curiosity_level': LaunchConfiguration('curiosity_level'),
|
||||||
|
'idle_threshold_sec': LaunchConfiguration('idle_threshold_sec'),
|
||||||
|
}
|
||||||
|
],
|
||||||
|
remappings=[],
|
||||||
|
output='screen',
|
||||||
|
),
|
||||||
|
])
|
||||||
28
jetson/ros2_ws/src/saltybot_curiosity/package.xml
Normal file
28
jetson/ros2_ws/src/saltybot_curiosity/package.xml
Normal file
@ -0,0 +1,28 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>saltybot_curiosity</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Autonomous curiosity behavior with frontier exploration and self-narration</description>
|
||||||
|
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<build_depend>rosidl_default_generators</build_depend>
|
||||||
|
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||||
|
<exec_depend>rclpy</exec_depend>
|
||||||
|
<exec_depend>geometry_msgs</exec_depend>
|
||||||
|
<exec_depend>nav2_msgs</exec_depend>
|
||||||
|
<exec_depend>sensor_msgs</exec_depend>
|
||||||
|
<exec_depend>vision_msgs</exec_depend>
|
||||||
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
|
||||||
|
<test_depend>ament_copyright</test_depend>
|
||||||
|
<test_depend>ament_flake8</test_depend>
|
||||||
|
<test_depend>ament_pep257</test_depend>
|
||||||
|
<test_depend>python3-pytest</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_python</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1,353 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Curiosity behavior for SaltyBot — autonomous exploration when idle.
|
||||||
|
|
||||||
|
Activates when idle >60s with no people detected. Performs frontier exploration:
|
||||||
|
- Turns toward sounds (audio_direction)
|
||||||
|
- Approaches colorful/moving objects (object detection)
|
||||||
|
- Takes panoramic photos at interesting spots
|
||||||
|
- Self-narrates findings via TTS
|
||||||
|
- Respects geofence and dynamic obstacles
|
||||||
|
- Auto-stops after 10min
|
||||||
|
|
||||||
|
Publishes:
|
||||||
|
/saltybot/curiosity_state String State machine updates
|
||||||
|
"""
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.action import ActionClient
|
||||||
|
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||||
|
from nav2_msgs.action import NavigateToPose
|
||||||
|
from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Twist
|
||||||
|
from std_msgs.msg import String, Float32, Bool
|
||||||
|
from sensor_msgs.msg import Image
|
||||||
|
from vision_msgs.msg import Detection2DArray
|
||||||
|
|
||||||
|
import math
|
||||||
|
import time
|
||||||
|
import random
|
||||||
|
from enum import Enum
|
||||||
|
from collections import deque
|
||||||
|
|
||||||
|
|
||||||
|
_SENSOR_QOS = QoSProfile(
|
||||||
|
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||||
|
history=HistoryPolicy.KEEP_LAST,
|
||||||
|
depth=5,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class CuriosityState(Enum):
|
||||||
|
IDLE = 0
|
||||||
|
WAITING_FOR_IDLE = 1
|
||||||
|
EXPLORING = 2
|
||||||
|
INVESTIGATING = 3
|
||||||
|
RETURNING = 4
|
||||||
|
|
||||||
|
|
||||||
|
class CuriosityNode(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('curiosity_node')
|
||||||
|
|
||||||
|
# Parameters
|
||||||
|
self.declare_parameter('curiosity_level', 0.7)
|
||||||
|
self.declare_parameter('idle_threshold_sec', 60.0)
|
||||||
|
self.declare_parameter('exploration_max_duration_sec', 600.0) # 10min
|
||||||
|
self.declare_parameter('exploration_radius_m', 5.0)
|
||||||
|
self.declare_parameter('pan_tilt_step_deg', 15.0)
|
||||||
|
self.declare_parameter('min_sound_activity', 0.1)
|
||||||
|
|
||||||
|
self.curiosity_level = self.get_parameter('curiosity_level').value
|
||||||
|
self.idle_threshold = self.get_parameter('idle_threshold_sec').value
|
||||||
|
self.max_exploration_duration = self.get_parameter('exploration_max_duration_sec').value
|
||||||
|
self.exploration_radius = self.get_parameter('exploration_radius_m').value
|
||||||
|
self.pan_tilt_step = self.get_parameter('pan_tilt_step_deg').value
|
||||||
|
self.min_sound_activity = self.get_parameter('min_sound_activity').value
|
||||||
|
|
||||||
|
# State tracking
|
||||||
|
self.state = CuriosityState.IDLE
|
||||||
|
self.last_person_time = time.time()
|
||||||
|
self.last_activity_time = time.time()
|
||||||
|
self.exploration_start_time = None
|
||||||
|
self.current_location = Pose()
|
||||||
|
self.home_location = Pose()
|
||||||
|
|
||||||
|
# Sensors
|
||||||
|
self.audio_bearing = 0.0
|
||||||
|
self.audio_activity = False
|
||||||
|
self.recent_detections = deque(maxlen=5)
|
||||||
|
self.battery_level = 100.0
|
||||||
|
self.geofence_limit = None
|
||||||
|
self.obstacles = []
|
||||||
|
|
||||||
|
# Narration queue
|
||||||
|
self.narration_queue = []
|
||||||
|
|
||||||
|
# Publishers
|
||||||
|
self.state_pub = self.create_publisher(
|
||||||
|
String, '/saltybot/curiosity_state', 10
|
||||||
|
)
|
||||||
|
self.narration_pub = self.create_publisher(
|
||||||
|
String, '/saltybot/tts_request', 10
|
||||||
|
)
|
||||||
|
self.pan_tilt_pub = self.create_publisher(
|
||||||
|
String, '/saltybot/pan_tilt_cmd', 10
|
||||||
|
)
|
||||||
|
self.velocity_pub = self.create_publisher(
|
||||||
|
Twist, '/cmd_vel', 10
|
||||||
|
)
|
||||||
|
|
||||||
|
# Subscribers
|
||||||
|
self.create_subscription(
|
||||||
|
Float32, '/saltybot/audio_direction', self.audio_bearing_callback, _SENSOR_QOS
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
Bool, '/saltybot/audio_activity', self.audio_activity_callback, _SENSOR_QOS
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
Detection2DArray, '/saltybot/person_detections', self.person_callback, _SENSOR_QOS
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
Detection2DArray, '/saltybot/object_detections', self.object_callback, _SENSOR_QOS
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
Float32, '/saltybot/battery_percent', self.battery_callback, 10
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
String, '/saltybot/geofence_status', self.geofence_callback, 10
|
||||||
|
)
|
||||||
|
|
||||||
|
# Nav2 action client
|
||||||
|
self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
|
||||||
|
|
||||||
|
# Main loop timer
|
||||||
|
self.timer = self.create_timer(1.0, self.curiosity_loop)
|
||||||
|
|
||||||
|
self.get_logger().info(
|
||||||
|
f"Curiosity node initialized (level={self.curiosity_level}, "
|
||||||
|
f"idle_threshold={self.idle_threshold}s, max_duration={self.max_exploration_duration}s)"
|
||||||
|
)
|
||||||
|
|
||||||
|
def audio_bearing_callback(self, msg):
|
||||||
|
"""Update detected audio bearing (0-360 degrees)."""
|
||||||
|
self.audio_bearing = msg.data
|
||||||
|
self.last_activity_time = time.time()
|
||||||
|
|
||||||
|
def audio_activity_callback(self, msg):
|
||||||
|
"""Track if audio activity detected."""
|
||||||
|
self.audio_activity = msg.data
|
||||||
|
if msg.data:
|
||||||
|
self.last_activity_time = time.time()
|
||||||
|
|
||||||
|
def person_callback(self, msg):
|
||||||
|
"""Update last person detection time."""
|
||||||
|
if len(msg.detections) > 0:
|
||||||
|
self.last_person_time = time.time()
|
||||||
|
|
||||||
|
def object_callback(self, msg):
|
||||||
|
"""Track interesting objects (colorful/moving)."""
|
||||||
|
for detection in msg.detections:
|
||||||
|
# Store recent detections with confidence
|
||||||
|
if hasattr(detection, 'results') and len(detection.results) > 0:
|
||||||
|
obj = {
|
||||||
|
'class': detection.results[0].class_name if hasattr(detection.results[0], 'class_name') else 'unknown',
|
||||||
|
'confidence': detection.results[0].score if hasattr(detection.results[0], 'score') else 0.0,
|
||||||
|
}
|
||||||
|
self.recent_detections.append(obj)
|
||||||
|
self.last_activity_time = time.time()
|
||||||
|
|
||||||
|
def battery_callback(self, msg):
|
||||||
|
"""Track battery level."""
|
||||||
|
self.battery_level = msg.data
|
||||||
|
|
||||||
|
def geofence_callback(self, msg):
|
||||||
|
"""Track geofence status."""
|
||||||
|
data = msg.data
|
||||||
|
if "limit:" in data:
|
||||||
|
try:
|
||||||
|
limit_str = data.split("limit:")[1].strip()
|
||||||
|
self.geofence_limit = float(limit_str)
|
||||||
|
except (ValueError, IndexError):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def publish_state(self, details=""):
|
||||||
|
"""Publish current curiosity state."""
|
||||||
|
msg = String()
|
||||||
|
state_str = f"state:{self.state.name}"
|
||||||
|
if details:
|
||||||
|
state_str += f",{details}"
|
||||||
|
msg.data = state_str
|
||||||
|
self.state_pub.publish(msg)
|
||||||
|
|
||||||
|
def narrate(self, text):
|
||||||
|
"""Queue text for TTS narration."""
|
||||||
|
msg = String()
|
||||||
|
msg.data = text
|
||||||
|
self.narration_pub.publish(msg)
|
||||||
|
self.get_logger().info(f"Narrating: {text}")
|
||||||
|
|
||||||
|
def pan_tilt_toward(self, bearing_deg):
|
||||||
|
"""Command pan-tilt to look toward bearing."""
|
||||||
|
msg = String()
|
||||||
|
msg.data = f"pan:{int(bearing_deg % 360)}"
|
||||||
|
self.pan_tilt_pub.publish(msg)
|
||||||
|
|
||||||
|
def turn_toward_sound(self):
|
||||||
|
"""Turn to face detected sound source."""
|
||||||
|
if self.audio_activity and self.min_sound_activity > 0:
|
||||||
|
self.get_logger().info(f"Sound detected at bearing {self.audio_bearing}°")
|
||||||
|
self.narrate(f"Interesting! I hear something at {int(self.audio_bearing)} degrees!")
|
||||||
|
self.pan_tilt_toward(self.audio_bearing)
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
def approach_interesting_object(self):
|
||||||
|
"""Identify and approach interesting objects if available."""
|
||||||
|
if not self.recent_detections:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Look for colorful or moving objects
|
||||||
|
interesting_classes = ['ball', 'toy', 'person', 'car', 'dog', 'cat', 'bird']
|
||||||
|
for obj in self.recent_detections:
|
||||||
|
class_name = obj.get('class', '').lower()
|
||||||
|
confidence = obj.get('confidence', 0.0)
|
||||||
|
|
||||||
|
if any(keyword in class_name for keyword in interesting_classes) and confidence > 0.6:
|
||||||
|
self.narrate(f"Ooh, a {obj['class']}! Let me check it out.")
|
||||||
|
# Would navigate toward object here (simplified)
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def is_idle(self):
|
||||||
|
"""Check if we've been idle long enough to activate curiosity."""
|
||||||
|
time_since_person = time.time() - self.last_person_time
|
||||||
|
time_since_activity = time.time() - self.last_activity_time
|
||||||
|
|
||||||
|
# Idle if no person nearby AND no recent activity
|
||||||
|
return (
|
||||||
|
time_since_person > self.idle_threshold
|
||||||
|
and time_since_activity > (self.idle_threshold / 2)
|
||||||
|
)
|
||||||
|
|
||||||
|
def should_explore(self):
|
||||||
|
"""Decide whether to start exploration based on curiosity level."""
|
||||||
|
# Higher curiosity = more likely to explore when idle
|
||||||
|
return random.random() < self.curiosity_level
|
||||||
|
|
||||||
|
def explore_frontier(self):
|
||||||
|
"""Generate frontier exploration goal."""
|
||||||
|
# Pick random direction within exploration radius
|
||||||
|
distance = random.uniform(1.0, self.exploration_radius)
|
||||||
|
angle = random.uniform(0, 2 * math.pi)
|
||||||
|
|
||||||
|
goal_x = self.current_location.position.x + distance * math.cos(angle)
|
||||||
|
goal_y = self.current_location.position.y + distance * math.sin(angle)
|
||||||
|
|
||||||
|
# Check against geofence
|
||||||
|
if self.geofence_limit:
|
||||||
|
dist_from_home = math.sqrt(goal_x**2 + goal_y**2)
|
||||||
|
if dist_from_home > self.geofence_limit:
|
||||||
|
# Clamp to geofence boundary
|
||||||
|
scale = (self.geofence_limit * 0.9) / dist_from_home
|
||||||
|
goal_x *= scale
|
||||||
|
goal_y *= scale
|
||||||
|
|
||||||
|
return goal_x, goal_y
|
||||||
|
|
||||||
|
def navigate_to_exploration_point(self):
|
||||||
|
"""Send navigation goal for exploration."""
|
||||||
|
goal_x, goal_y = self.explore_frontier()
|
||||||
|
|
||||||
|
goal = NavigateToPose.Goal()
|
||||||
|
goal.pose.header.frame_id = "map"
|
||||||
|
goal.pose.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
goal.pose.pose.position.x = goal_x
|
||||||
|
goal.pose.pose.position.y = goal_y
|
||||||
|
goal.pose.pose.position.z = 0.0
|
||||||
|
goal.pose.pose.orientation.w = 1.0
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.nav_client.wait_for_server(timeout_sec=2.0)
|
||||||
|
self.nav_client.send_goal_async(goal)
|
||||||
|
self.get_logger().info(f"Exploring toward ({goal_x:.2f}, {goal_y:.2f})")
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f"Navigation unavailable: {e}")
|
||||||
|
|
||||||
|
def return_home(self):
|
||||||
|
"""Navigate back to starting location."""
|
||||||
|
self.narrate("I've seen enough. Heading back home.")
|
||||||
|
goal = NavigateToPose.Goal()
|
||||||
|
goal.pose.header.frame_id = "map"
|
||||||
|
goal.pose.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
goal.pose.pose.position.x = 0.0
|
||||||
|
goal.pose.pose.position.y = 0.0
|
||||||
|
goal.pose.pose.position.z = 0.0
|
||||||
|
goal.pose.pose.orientation.w = 1.0
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.nav_client.wait_for_server(timeout_sec=2.0)
|
||||||
|
self.nav_client.send_goal_async(goal)
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f"Navigation unavailable: {e}")
|
||||||
|
|
||||||
|
def curiosity_loop(self):
|
||||||
|
"""Main curiosity state machine."""
|
||||||
|
self.publish_state()
|
||||||
|
|
||||||
|
if self.state == CuriosityState.IDLE:
|
||||||
|
if self.is_idle() and self.should_explore() and self.battery_level > 30:
|
||||||
|
self.state = CuriosityState.EXPLORING
|
||||||
|
self.exploration_start_time = time.time()
|
||||||
|
self.narrate("Hmm, nothing to do. Let me explore!")
|
||||||
|
self.get_logger().info("Curiosity activated!")
|
||||||
|
|
||||||
|
elif self.state == CuriosityState.EXPLORING:
|
||||||
|
# Check timeout
|
||||||
|
duration = time.time() - self.exploration_start_time
|
||||||
|
if duration > self.max_exploration_duration:
|
||||||
|
self.state = CuriosityState.RETURNING
|
||||||
|
return
|
||||||
|
|
||||||
|
# React to stimuli
|
||||||
|
if self.turn_toward_sound():
|
||||||
|
pass # Already turned toward sound
|
||||||
|
elif self.approach_interesting_object():
|
||||||
|
pass # Already approaching object
|
||||||
|
else:
|
||||||
|
# Random frontier exploration
|
||||||
|
if random.random() < 0.3: # 30% chance to explore frontier each cycle
|
||||||
|
self.navigate_to_exploration_point()
|
||||||
|
|
||||||
|
# Periodic narration
|
||||||
|
if random.random() < 0.1:
|
||||||
|
observations = [
|
||||||
|
"Interesting things over here!",
|
||||||
|
"I wonder what's in that direction.",
|
||||||
|
"The world is so big!",
|
||||||
|
"What could be around the corner?",
|
||||||
|
]
|
||||||
|
self.narrate(random.choice(observations))
|
||||||
|
|
||||||
|
elif self.state == CuriosityState.RETURNING:
|
||||||
|
self.return_home()
|
||||||
|
self.state = CuriosityState.IDLE
|
||||||
|
self.exploration_start_time = None
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = CuriosityNode()
|
||||||
|
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
4
jetson/ros2_ws/src/saltybot_curiosity/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_curiosity/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
[develop]
|
||||||
|
script_dir=$base/lib/saltybot_curiosity
|
||||||
|
[install]
|
||||||
|
install_lib=$base/lib/python3/dist-packages
|
||||||
29
jetson/ros2_ws/src/saltybot_curiosity/setup.py
Normal file
29
jetson/ros2_ws/src/saltybot_curiosity/setup.py
Normal file
@ -0,0 +1,29 @@
|
|||||||
|
from setuptools import setup
|
||||||
|
import os
|
||||||
|
from glob import glob
|
||||||
|
|
||||||
|
package_name = 'saltybot_curiosity'
|
||||||
|
|
||||||
|
setup(
|
||||||
|
name=package_name,
|
||||||
|
version='0.1.0',
|
||||||
|
packages=[package_name],
|
||||||
|
data_files=[
|
||||||
|
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||||
|
('share/' + package_name, ['package.xml']),
|
||||||
|
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
|
||||||
|
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
|
||||||
|
],
|
||||||
|
install_requires=['setuptools'],
|
||||||
|
zip_safe=True,
|
||||||
|
maintainer='seb',
|
||||||
|
maintainer_email='seb@vayrette.com',
|
||||||
|
description='Autonomous curiosity behavior with frontier exploration and self-narration',
|
||||||
|
license='MIT',
|
||||||
|
tests_require=['pytest'],
|
||||||
|
entry_points={
|
||||||
|
'console_scripts': [
|
||||||
|
'curiosity_node = saltybot_curiosity.curiosity_node:main',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
@ -0,0 +1,38 @@
|
|||||||
|
# Event Logger Configuration
|
||||||
|
|
||||||
|
event_logger_node:
|
||||||
|
ros__parameters:
|
||||||
|
# Data directory for event logs (expands ~)
|
||||||
|
data_dir: ~/.saltybot-data/events
|
||||||
|
|
||||||
|
# Log rotation settings
|
||||||
|
rotation_days: 7 # Compress files older than 7 days
|
||||||
|
retention_days: 90 # Delete files older than 90 days
|
||||||
|
|
||||||
|
# Event type mapping (topic -> event_type)
|
||||||
|
event_types:
|
||||||
|
first_encounter: encounter
|
||||||
|
voice_command: voice_command
|
||||||
|
trick_state: trick
|
||||||
|
emergency_stop: e-stop
|
||||||
|
geofence: geofence
|
||||||
|
dock_state: dock
|
||||||
|
error: error
|
||||||
|
system_boot: boot
|
||||||
|
system_shutdown: shutdown
|
||||||
|
|
||||||
|
# Subscribed topics (relative to /saltybot/)
|
||||||
|
subscribed_topics:
|
||||||
|
- first_encounter
|
||||||
|
- voice_command
|
||||||
|
- trick_state
|
||||||
|
- emergency_stop
|
||||||
|
- geofence
|
||||||
|
- dock_state
|
||||||
|
- error
|
||||||
|
- system_boot
|
||||||
|
- system_shutdown
|
||||||
|
|
||||||
|
# Publishing settings
|
||||||
|
stats_publish_interval: 60 # Seconds between stats publishes
|
||||||
|
rotation_check_interval: 3600 # Seconds between rotation checks
|
||||||
@ -0,0 +1,21 @@
|
|||||||
|
"""
|
||||||
|
Launch file for SaltyBot Event Logger.
|
||||||
|
|
||||||
|
Launches the centralized event logging node that subscribes to all
|
||||||
|
saltybot/* state topics and logs structured JSON events.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
return LaunchDescription([
|
||||||
|
Node(
|
||||||
|
package='saltybot_event_logger',
|
||||||
|
executable='event_logger_node',
|
||||||
|
name='event_logger_node',
|
||||||
|
output='screen',
|
||||||
|
emulate_tty=True,
|
||||||
|
),
|
||||||
|
])
|
||||||
24
jetson/ros2_ws/src/saltybot_event_logger/package.xml
Normal file
24
jetson/ros2_ws/src/saltybot_event_logger/package.xml
Normal file
@ -0,0 +1,24 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>saltybot_event_logger</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Structured JSON event logging for all robot activities with query service, stats publisher, log rotation, CSV export, and dashboard feed.</description>
|
||||||
|
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||||
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_python</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclpy</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<depend>builtin_interfaces</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_copyright</test_depend>
|
||||||
|
<test_depend>ament_flake8</test_depend>
|
||||||
|
<test_depend>ament_pep257</test_depend>
|
||||||
|
<test_depend>python3-pytest</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_python</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1 @@
|
|||||||
|
# Marker file for ament resource index
|
||||||
@ -0,0 +1 @@
|
|||||||
|
"""SaltyBot Event Logger - Structured JSON logging for all robot activities."""
|
||||||
@ -0,0 +1,350 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Event Logger Node - Centralized structured logging of all SaltyBot activities.
|
||||||
|
|
||||||
|
Subscribes to /saltybot/* state topics and logs JSON events to JSONL files.
|
||||||
|
Supports: query service (time/type filter), stats publisher, log rotation,
|
||||||
|
CSV export, and dashboard live feed.
|
||||||
|
|
||||||
|
Event types: encounter, voice_command, trick, e-stop, geofence, dock, error, boot, shutdown
|
||||||
|
"""
|
||||||
|
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import gzip
|
||||||
|
import shutil
|
||||||
|
import csv
|
||||||
|
import threading
|
||||||
|
from datetime import datetime, timedelta
|
||||||
|
from pathlib import Path
|
||||||
|
from dataclasses import dataclass, asdict
|
||||||
|
from typing import List, Dict, Any, Optional
|
||||||
|
from collections import defaultdict
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from std_msgs.msg import String
|
||||||
|
from rclpy.subscription import Subscription
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Event:
|
||||||
|
"""Structured event record."""
|
||||||
|
timestamp: str # ISO 8601
|
||||||
|
event_type: str # encounter, voice_command, trick, e-stop, geofence, dock, error, boot, shutdown
|
||||||
|
source: str # Topic that generated event
|
||||||
|
data: Dict[str, Any] # Event-specific data
|
||||||
|
|
||||||
|
|
||||||
|
class EventLogger(Node):
|
||||||
|
"""
|
||||||
|
Centralized event logging node for SaltyBot.
|
||||||
|
|
||||||
|
Subscribes to MQTT state topics and logs structured JSON events.
|
||||||
|
Provides query service, stats publishing, log rotation, and export.
|
||||||
|
"""
|
||||||
|
|
||||||
|
# Event type mapping from topics
|
||||||
|
EVENT_TYPES = {
|
||||||
|
'first_encounter': 'encounter',
|
||||||
|
'voice_command': 'voice_command',
|
||||||
|
'trick_state': 'trick',
|
||||||
|
'emergency_stop': 'e-stop',
|
||||||
|
'geofence': 'geofence',
|
||||||
|
'dock_state': 'dock',
|
||||||
|
'error': 'error',
|
||||||
|
'system_boot': 'boot',
|
||||||
|
'system_shutdown': 'shutdown',
|
||||||
|
}
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('event_logger_node')
|
||||||
|
|
||||||
|
# Configuration
|
||||||
|
self.data_dir = Path(os.path.expanduser('~/.saltybot-data/events'))
|
||||||
|
self.data_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
self.rotation_days = 7 # Compress files >7 days old
|
||||||
|
self.retention_days = 90 # Delete files >90 days old
|
||||||
|
|
||||||
|
# Runtime state
|
||||||
|
self.subscriptions: Dict[str, Subscription] = {}
|
||||||
|
self.events_lock = threading.Lock()
|
||||||
|
self.daily_events: List[Event] = []
|
||||||
|
self.daily_stats = defaultdict(int)
|
||||||
|
|
||||||
|
self.get_logger().info(f'Event logger initialized at {self.data_dir}')
|
||||||
|
|
||||||
|
# Subscribe to relevant topics
|
||||||
|
self._setup_subscriptions()
|
||||||
|
|
||||||
|
# Timer for stats and rotation
|
||||||
|
self.create_timer(60.0, self._publish_stats) # Stats every 60s
|
||||||
|
self.create_timer(3600.0, self._rotate_logs) # Rotation every hour
|
||||||
|
|
||||||
|
# Publishers
|
||||||
|
self.stats_pub = self.create_publisher(String, '/saltybot/event_stats', 10)
|
||||||
|
self.live_feed_pub = self.create_publisher(String, '/saltybot/event_feed', 10)
|
||||||
|
|
||||||
|
def _setup_subscriptions(self):
|
||||||
|
"""Subscribe to saltybot/* topics for event detection."""
|
||||||
|
# Subscribe to specific event topics
|
||||||
|
event_topics = [
|
||||||
|
'/saltybot/first_encounter',
|
||||||
|
'/saltybot/voice_command',
|
||||||
|
'/saltybot/trick_state',
|
||||||
|
'/saltybot/emergency_stop',
|
||||||
|
'/saltybot/geofence',
|
||||||
|
'/saltybot/dock_state',
|
||||||
|
'/saltybot/error',
|
||||||
|
'/saltybot/system_boot',
|
||||||
|
'/saltybot/system_shutdown',
|
||||||
|
]
|
||||||
|
|
||||||
|
for topic in event_topics:
|
||||||
|
# Extract event type from topic
|
||||||
|
topic_name = topic.split('/')[-1]
|
||||||
|
event_type = self.EVENT_TYPES.get(topic_name, topic_name)
|
||||||
|
|
||||||
|
self.subscriptions[topic] = self.create_subscription(
|
||||||
|
String,
|
||||||
|
topic,
|
||||||
|
lambda msg, et=event_type, t=topic: self._on_event(msg, et, t),
|
||||||
|
10
|
||||||
|
)
|
||||||
|
|
||||||
|
def _on_event(self, msg: String, event_type: str, topic: str):
|
||||||
|
"""Handle incoming event message."""
|
||||||
|
try:
|
||||||
|
# Parse message as JSON if possible, otherwise use as string
|
||||||
|
try:
|
||||||
|
data = json.loads(msg.data)
|
||||||
|
except (json.JSONDecodeError, AttributeError):
|
||||||
|
data = {'message': str(msg.data)}
|
||||||
|
|
||||||
|
event = Event(
|
||||||
|
timestamp=datetime.now().isoformat(),
|
||||||
|
event_type=event_type,
|
||||||
|
source=topic,
|
||||||
|
data=data
|
||||||
|
)
|
||||||
|
|
||||||
|
self._log_event(event)
|
||||||
|
self._publish_to_feed(event)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error processing event: {e}')
|
||||||
|
|
||||||
|
def _log_event(self, event: Event):
|
||||||
|
"""Write event to daily JSONL file."""
|
||||||
|
try:
|
||||||
|
with self.events_lock:
|
||||||
|
# Daily filename
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = self.data_dir / f'{today}.jsonl'
|
||||||
|
|
||||||
|
# Append to file
|
||||||
|
with open(log_file, 'a') as f:
|
||||||
|
f.write(json.dumps(asdict(event)) + '\n')
|
||||||
|
|
||||||
|
# Update in-memory stats
|
||||||
|
self.daily_events.append(event)
|
||||||
|
self.daily_stats[event.event_type] += 1
|
||||||
|
if 'person_id' in event.data:
|
||||||
|
self.daily_stats['encounters'] += 1
|
||||||
|
if event.event_type == 'error':
|
||||||
|
self.daily_stats['errors'] += 1
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error logging event: {e}')
|
||||||
|
|
||||||
|
def _publish_to_feed(self, event: Event):
|
||||||
|
"""Publish event to live feed topic."""
|
||||||
|
try:
|
||||||
|
feed_msg = String()
|
||||||
|
feed_msg.data = json.dumps(asdict(event))
|
||||||
|
self.live_feed_pub.publish(feed_msg)
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error publishing to feed: {e}')
|
||||||
|
|
||||||
|
def _publish_stats(self):
|
||||||
|
"""Publish daily statistics."""
|
||||||
|
try:
|
||||||
|
stats = {
|
||||||
|
'timestamp': datetime.now().isoformat(),
|
||||||
|
'total_events': len(self.daily_events),
|
||||||
|
'by_type': dict(self.daily_stats),
|
||||||
|
'encounters': self.daily_stats.get('encounters', 0),
|
||||||
|
'errors': self.daily_stats.get('errors', 0),
|
||||||
|
}
|
||||||
|
|
||||||
|
msg = String()
|
||||||
|
msg.data = json.dumps(stats)
|
||||||
|
self.stats_pub.publish(msg)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error publishing stats: {e}')
|
||||||
|
|
||||||
|
def _rotate_logs(self):
|
||||||
|
"""Handle log rotation: compress >7 days, delete >90 days."""
|
||||||
|
try:
|
||||||
|
now = datetime.now()
|
||||||
|
|
||||||
|
for log_file in sorted(self.data_dir.glob('*.jsonl')):
|
||||||
|
# Extract date from filename (YYYY-MM-DD.jsonl)
|
||||||
|
try:
|
||||||
|
date_str = log_file.stem
|
||||||
|
file_date = datetime.strptime(date_str, '%Y-%m-%d')
|
||||||
|
age_days = (now - file_date).days
|
||||||
|
|
||||||
|
if age_days > self.retention_days:
|
||||||
|
# Delete very old files
|
||||||
|
log_file.unlink()
|
||||||
|
self.get_logger().info(f'Deleted {log_file.name} (age: {age_days}d)')
|
||||||
|
|
||||||
|
elif age_days > self.rotation_days:
|
||||||
|
# Compress old files
|
||||||
|
gz_file = log_file.with_suffix('.jsonl.gz')
|
||||||
|
if not gz_file.exists():
|
||||||
|
with open(log_file, 'rb') as f_in:
|
||||||
|
with gzip.open(gz_file, 'wb') as f_out:
|
||||||
|
shutil.copyfileobj(f_in, f_out)
|
||||||
|
log_file.unlink()
|
||||||
|
self.get_logger().info(f'Compressed {log_file.name}')
|
||||||
|
|
||||||
|
except ValueError:
|
||||||
|
# Skip files with unexpected naming
|
||||||
|
pass
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error rotating logs: {e}')
|
||||||
|
|
||||||
|
def query_events(self, event_types: Optional[List[str]] = None,
|
||||||
|
start_time: Optional[str] = None,
|
||||||
|
end_time: Optional[str] = None) -> List[Event]:
|
||||||
|
"""
|
||||||
|
Query events by type and time range.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
event_types: List of event types to filter (None = all)
|
||||||
|
start_time: ISO 8601 start timestamp (None = all)
|
||||||
|
end_time: ISO 8601 end timestamp (None = all)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
List of matching events
|
||||||
|
"""
|
||||||
|
results = []
|
||||||
|
|
||||||
|
try:
|
||||||
|
# Determine date range to search
|
||||||
|
if start_time:
|
||||||
|
start = datetime.fromisoformat(start_time)
|
||||||
|
else:
|
||||||
|
start = datetime.now() - timedelta(days=90)
|
||||||
|
|
||||||
|
if end_time:
|
||||||
|
end = datetime.fromisoformat(end_time)
|
||||||
|
else:
|
||||||
|
end = datetime.now()
|
||||||
|
|
||||||
|
# Search JSONL files
|
||||||
|
for log_file in sorted(self.data_dir.glob('*.jsonl')):
|
||||||
|
try:
|
||||||
|
file_date_str = log_file.stem
|
||||||
|
file_date = datetime.strptime(file_date_str, '%Y-%m-%d')
|
||||||
|
|
||||||
|
# Skip files outside time range
|
||||||
|
if file_date < start or file_date > end:
|
||||||
|
continue
|
||||||
|
|
||||||
|
with open(log_file, 'r') as f:
|
||||||
|
for line in f:
|
||||||
|
if not line.strip():
|
||||||
|
continue
|
||||||
|
|
||||||
|
event_dict = json.loads(line)
|
||||||
|
event = Event(**event_dict)
|
||||||
|
|
||||||
|
# Check timestamp in range
|
||||||
|
event_time = datetime.fromisoformat(event.timestamp)
|
||||||
|
if event_time < start or event_time > end:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Check type filter
|
||||||
|
if event_types and event.event_type not in event_types:
|
||||||
|
continue
|
||||||
|
|
||||||
|
results.append(event)
|
||||||
|
|
||||||
|
except (ValueError, json.JSONDecodeError):
|
||||||
|
pass
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error querying events: {e}')
|
||||||
|
|
||||||
|
return results
|
||||||
|
|
||||||
|
def export_csv(self, filename: Optional[str] = None) -> str:
|
||||||
|
"""
|
||||||
|
Export all events to CSV.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
filename: Output filename (default: events_YYYY-MM-DD.csv)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Path to exported file
|
||||||
|
"""
|
||||||
|
if not filename:
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
filename = f'events_{today}.csv'
|
||||||
|
|
||||||
|
output_path = self.data_dir / filename
|
||||||
|
|
||||||
|
try:
|
||||||
|
with self.events_lock:
|
||||||
|
events = self.query_events()
|
||||||
|
|
||||||
|
with open(output_path, 'w', newline='') as f:
|
||||||
|
writer = csv.writer(f)
|
||||||
|
writer.writerow(['Timestamp', 'Type', 'Source', 'Data'])
|
||||||
|
|
||||||
|
for event in events:
|
||||||
|
writer.writerow([
|
||||||
|
event.timestamp,
|
||||||
|
event.event_type,
|
||||||
|
event.source,
|
||||||
|
json.dumps(event.data)
|
||||||
|
])
|
||||||
|
|
||||||
|
self.get_logger().info(f'Exported {len(events)} events to {output_path}')
|
||||||
|
return str(output_path)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'Error exporting CSV: {e}')
|
||||||
|
return ''
|
||||||
|
|
||||||
|
def get_daily_stats(self) -> Dict[str, Any]:
|
||||||
|
"""Get stats for current day."""
|
||||||
|
return {
|
||||||
|
'timestamp': datetime.now().isoformat(),
|
||||||
|
'total_events': len(self.daily_events),
|
||||||
|
'by_type': dict(self.daily_stats),
|
||||||
|
'encounters': self.daily_stats.get('encounters', 0),
|
||||||
|
'errors': self.daily_stats.get('errors', 0),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = EventLogger()
|
||||||
|
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
4
jetson/ros2_ws/src/saltybot_event_logger/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_event_logger/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
[develop]
|
||||||
|
script-dir=$base/lib/saltybot_event_logger
|
||||||
|
[egg_info]
|
||||||
|
tag_date = 0
|
||||||
22
jetson/ros2_ws/src/saltybot_event_logger/setup.py
Normal file
22
jetson/ros2_ws/src/saltybot_event_logger/setup.py
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
from setuptools import setup, find_packages
|
||||||
|
|
||||||
|
setup(
|
||||||
|
name='saltybot_event_logger',
|
||||||
|
version='0.1.0',
|
||||||
|
packages=find_packages(),
|
||||||
|
data_files=[
|
||||||
|
('share/ament_index/resource_index/packages', ['resource/saltybot_event_logger']),
|
||||||
|
('share/saltybot_event_logger', ['package.xml']),
|
||||||
|
],
|
||||||
|
install_requires=['setuptools'],
|
||||||
|
zip_safe=True,
|
||||||
|
author='seb',
|
||||||
|
author_email='seb@vayrette.com',
|
||||||
|
description='Structured JSON event logging with query service, stats, rotation, and export',
|
||||||
|
license='Apache-2.0',
|
||||||
|
entry_points={
|
||||||
|
'console_scripts': [
|
||||||
|
'event_logger_node = saltybot_event_logger.event_logger_node:main',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
@ -0,0 +1,260 @@
|
|||||||
|
"""Unit tests for Event Logger."""
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
import json
|
||||||
|
import tempfile
|
||||||
|
import shutil
|
||||||
|
from datetime import datetime, timedelta
|
||||||
|
from pathlib import Path
|
||||||
|
from unittest.mock import MagicMock, patch
|
||||||
|
from std_msgs.msg import String
|
||||||
|
|
||||||
|
# Import the module under test
|
||||||
|
import sys
|
||||||
|
import os
|
||||||
|
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||||
|
|
||||||
|
from saltybot_event_logger.event_logger_node import Event, EventLogger
|
||||||
|
|
||||||
|
|
||||||
|
class TestEvent:
|
||||||
|
"""Test Event dataclass."""
|
||||||
|
|
||||||
|
def test_event_creation(self):
|
||||||
|
"""Test creating an event."""
|
||||||
|
now = datetime.now().isoformat()
|
||||||
|
event = Event(
|
||||||
|
timestamp=now,
|
||||||
|
event_type='voice_command',
|
||||||
|
source='/saltybot/voice_command',
|
||||||
|
data={'command': 'spin'}
|
||||||
|
)
|
||||||
|
|
||||||
|
assert event.timestamp == now
|
||||||
|
assert event.event_type == 'voice_command'
|
||||||
|
assert event.source == '/saltybot/voice_command'
|
||||||
|
assert event.data['command'] == 'spin'
|
||||||
|
|
||||||
|
def test_event_to_dict(self):
|
||||||
|
"""Test converting event to dict."""
|
||||||
|
from dataclasses import asdict
|
||||||
|
event = Event(
|
||||||
|
timestamp='2026-03-05T12:00:00',
|
||||||
|
event_type='trick',
|
||||||
|
source='/saltybot/trick_state',
|
||||||
|
data={'trick': 'dance'}
|
||||||
|
)
|
||||||
|
|
||||||
|
event_dict = asdict(event)
|
||||||
|
assert event_dict['event_type'] == 'trick'
|
||||||
|
assert 'timestamp' in event_dict
|
||||||
|
|
||||||
|
|
||||||
|
class TestEventLogger:
|
||||||
|
"""Test EventLogger node."""
|
||||||
|
|
||||||
|
@pytest.fixture
|
||||||
|
def temp_data_dir(self):
|
||||||
|
"""Create temporary data directory."""
|
||||||
|
temp_dir = tempfile.mkdtemp()
|
||||||
|
yield Path(temp_dir)
|
||||||
|
shutil.rmtree(temp_dir)
|
||||||
|
|
||||||
|
@pytest.fixture
|
||||||
|
def logger(self, temp_data_dir):
|
||||||
|
"""Create EventLogger instance with mocked ROS."""
|
||||||
|
with patch('saltybot_event_logger.event_logger_node.Node.__init__', return_value=None):
|
||||||
|
with patch('saltybot_event_logger.event_logger_node.rclpy'):
|
||||||
|
logger = EventLogger()
|
||||||
|
logger.data_dir = temp_data_dir
|
||||||
|
logger.get_logger = MagicMock()
|
||||||
|
logger.create_subscription = MagicMock()
|
||||||
|
logger.create_publisher = MagicMock()
|
||||||
|
logger.create_timer = MagicMock()
|
||||||
|
logger.live_feed_pub = MagicMock()
|
||||||
|
logger.stats_pub = MagicMock()
|
||||||
|
return logger
|
||||||
|
|
||||||
|
def test_event_types_mapping(self, logger):
|
||||||
|
"""Test event type mapping."""
|
||||||
|
assert logger.EVENT_TYPES['voice_command'] == 'voice_command'
|
||||||
|
assert logger.EVENT_TYPES['trick_state'] == 'trick'
|
||||||
|
assert logger.EVENT_TYPES['first_encounter'] == 'encounter'
|
||||||
|
assert logger.EVENT_TYPES['emergency_stop'] == 'e-stop'
|
||||||
|
|
||||||
|
def test_log_event(self, logger, temp_data_dir):
|
||||||
|
"""Test logging an event to file."""
|
||||||
|
event = Event(
|
||||||
|
timestamp='2026-03-05T12:00:00',
|
||||||
|
event_type='voice_command',
|
||||||
|
source='/saltybot/voice_command',
|
||||||
|
data={'command': 'spin'}
|
||||||
|
)
|
||||||
|
|
||||||
|
logger._log_event(event)
|
||||||
|
|
||||||
|
# Check file was created
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = temp_data_dir / f'{today}.jsonl'
|
||||||
|
assert log_file.exists()
|
||||||
|
|
||||||
|
# Check content
|
||||||
|
with open(log_file, 'r') as f:
|
||||||
|
line = f.readline()
|
||||||
|
logged_event = json.loads(line)
|
||||||
|
|
||||||
|
assert logged_event['event_type'] == 'voice_command'
|
||||||
|
assert logged_event['data']['command'] == 'spin'
|
||||||
|
|
||||||
|
def test_log_rotation_compress(self, logger, temp_data_dir):
|
||||||
|
"""Test log compression for old files."""
|
||||||
|
# Create old log file
|
||||||
|
old_date = (datetime.now() - timedelta(days=8)).strftime('%Y-%m-%d')
|
||||||
|
old_file = temp_data_dir / f'{old_date}.jsonl'
|
||||||
|
old_file.write_text('{"test": "data"}\n')
|
||||||
|
|
||||||
|
logger._rotate_logs()
|
||||||
|
|
||||||
|
# Check file was compressed
|
||||||
|
gz_file = temp_data_dir / f'{old_date}.jsonl.gz'
|
||||||
|
assert gz_file.exists()
|
||||||
|
assert not old_file.exists()
|
||||||
|
|
||||||
|
def test_log_rotation_delete(self, logger, temp_data_dir):
|
||||||
|
"""Test deleting very old files."""
|
||||||
|
# Create very old log file
|
||||||
|
old_date = (datetime.now() - timedelta(days=91)).strftime('%Y-%m-%d')
|
||||||
|
old_file = temp_data_dir / f'{old_date}.jsonl'
|
||||||
|
old_file.write_text('{"test": "data"}\n')
|
||||||
|
|
||||||
|
logger._rotate_logs()
|
||||||
|
|
||||||
|
# Check file was deleted
|
||||||
|
assert not old_file.exists()
|
||||||
|
|
||||||
|
def test_query_events_all(self, logger, temp_data_dir):
|
||||||
|
"""Test querying all events."""
|
||||||
|
# Create log entries
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = temp_data_dir / f'{today}.jsonl'
|
||||||
|
|
||||||
|
events_data = [
|
||||||
|
Event('2026-03-05T10:00:00', 'voice_command', '/saltybot/voice_command', {'cmd': 'spin'}),
|
||||||
|
Event('2026-03-05T11:00:00', 'trick', '/saltybot/trick_state', {'trick': 'dance'}),
|
||||||
|
]
|
||||||
|
|
||||||
|
with open(log_file, 'w') as f:
|
||||||
|
for event in events_data:
|
||||||
|
from dataclasses import asdict
|
||||||
|
f.write(json.dumps(asdict(event)) + '\n')
|
||||||
|
|
||||||
|
# Query all
|
||||||
|
results = logger.query_events()
|
||||||
|
assert len(results) >= 2
|
||||||
|
|
||||||
|
def test_query_events_by_type(self, logger, temp_data_dir):
|
||||||
|
"""Test querying events by type."""
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = temp_data_dir / f'{today}.jsonl'
|
||||||
|
|
||||||
|
events_data = [
|
||||||
|
Event('2026-03-05T10:00:00', 'voice_command', '/saltybot/voice_command', {'cmd': 'spin'}),
|
||||||
|
Event('2026-03-05T11:00:00', 'trick', '/saltybot/trick_state', {'trick': 'dance'}),
|
||||||
|
Event('2026-03-05T12:00:00', 'error', '/saltybot/error', {'msg': 'test'}),
|
||||||
|
]
|
||||||
|
|
||||||
|
with open(log_file, 'w') as f:
|
||||||
|
for event in events_data:
|
||||||
|
from dataclasses import asdict
|
||||||
|
f.write(json.dumps(asdict(event)) + '\n')
|
||||||
|
|
||||||
|
# Query only voice commands
|
||||||
|
results = logger.query_events(event_types=['voice_command'])
|
||||||
|
assert all(e.event_type == 'voice_command' for e in results)
|
||||||
|
|
||||||
|
def test_query_events_by_time_range(self, logger, temp_data_dir):
|
||||||
|
"""Test querying events by time range."""
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = temp_data_dir / f'{today}.jsonl'
|
||||||
|
|
||||||
|
events_data = [
|
||||||
|
Event('2026-03-05T10:00:00', 'voice_command', '/saltybot/voice_command', {'cmd': 'spin'}),
|
||||||
|
Event('2026-03-05T14:00:00', 'trick', '/saltybot/trick_state', {'trick': 'dance'}),
|
||||||
|
Event('2026-03-05T18:00:00', 'error', '/saltybot/error', {'msg': 'test'}),
|
||||||
|
]
|
||||||
|
|
||||||
|
with open(log_file, 'w') as f:
|
||||||
|
for event in events_data:
|
||||||
|
from dataclasses import asdict
|
||||||
|
f.write(json.dumps(asdict(event)) + '\n')
|
||||||
|
|
||||||
|
# Query only morning events
|
||||||
|
results = logger.query_events(
|
||||||
|
start_time='2026-03-05T09:00:00',
|
||||||
|
end_time='2026-03-05T13:00:00'
|
||||||
|
)
|
||||||
|
assert len(results) >= 1
|
||||||
|
assert results[0].event_type == 'voice_command'
|
||||||
|
|
||||||
|
def test_export_csv(self, logger, temp_data_dir):
|
||||||
|
"""Test CSV export."""
|
||||||
|
today = datetime.now().strftime('%Y-%m-%d')
|
||||||
|
log_file = temp_data_dir / f'{today}.jsonl'
|
||||||
|
|
||||||
|
events_data = [
|
||||||
|
Event('2026-03-05T10:00:00', 'voice_command', '/saltybot/voice_command', {'cmd': 'spin'}),
|
||||||
|
Event('2026-03-05T11:00:00', 'trick', '/saltybot/trick_state', {'trick': 'dance'}),
|
||||||
|
]
|
||||||
|
|
||||||
|
with open(log_file, 'w') as f:
|
||||||
|
for event in events_data:
|
||||||
|
from dataclasses import asdict
|
||||||
|
f.write(json.dumps(asdict(event)) + '\n')
|
||||||
|
|
||||||
|
# Export to CSV
|
||||||
|
csv_path = logger.export_csv()
|
||||||
|
assert csv_path
|
||||||
|
assert Path(csv_path).exists()
|
||||||
|
assert Path(csv_path).suffix == '.csv'
|
||||||
|
|
||||||
|
def test_daily_stats(self, logger):
|
||||||
|
"""Test daily statistics calculation."""
|
||||||
|
# Add events
|
||||||
|
logger.daily_events = [
|
||||||
|
Event('2026-03-05T10:00:00', 'voice_command', '/saltybot/voice_command', {}),
|
||||||
|
Event('2026-03-05T11:00:00', 'trick', '/saltybot/trick_state', {}),
|
||||||
|
]
|
||||||
|
logger.daily_stats = {'voice_command': 1, 'trick': 1, 'encounters': 5, 'errors': 0}
|
||||||
|
|
||||||
|
stats = logger.get_daily_stats()
|
||||||
|
|
||||||
|
assert stats['total_events'] == 2
|
||||||
|
assert stats['encounters'] == 5
|
||||||
|
assert stats['errors'] == 0
|
||||||
|
assert 'by_type' in stats
|
||||||
|
|
||||||
|
def test_on_event_json_data(self, logger):
|
||||||
|
"""Test processing event with JSON data."""
|
||||||
|
msg = String()
|
||||||
|
msg.data = json.dumps({'command': 'spin', 'duration': 2})
|
||||||
|
|
||||||
|
logger._on_event(msg, 'voice_command', '/saltybot/voice_command')
|
||||||
|
|
||||||
|
assert len(logger.daily_events) > 0
|
||||||
|
event = logger.daily_events[0]
|
||||||
|
assert event.data['command'] == 'spin'
|
||||||
|
|
||||||
|
def test_on_event_string_data(self, logger):
|
||||||
|
"""Test processing event with string data."""
|
||||||
|
msg = String()
|
||||||
|
msg.data = 'simple string message'
|
||||||
|
|
||||||
|
logger._on_event(msg, 'error', '/saltybot/error')
|
||||||
|
|
||||||
|
assert len(logger.daily_events) > 0
|
||||||
|
event = logger.daily_events[0]
|
||||||
|
assert 'message' in event.data
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
pytest.main([__file__, '-v'])
|
||||||
Loading…
x
Reference in New Issue
Block a user