diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/config/terrain_classification_params.yaml b/jetson/ros2_ws/src/saltybot_terrain_classification/config/terrain_classification_params.yaml new file mode 100644 index 0000000..8c47487 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/config/terrain_classification_params.yaml @@ -0,0 +1,10 @@ +terrain_classification: + ros__parameters: + control_rate: 10.0 + depth_topic: "/camera/depth/image_rect_raw" + lidar_topic: "/scan" + depth_std_threshold: 0.02 + min_depth_m: 0.1 + max_depth_m: 3.0 + confidence_threshold: 0.5 + hysteresis_samples: 5 diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/launch/terrain_classification.launch.py b/jetson/ros2_ws/src/saltybot_terrain_classification/launch/terrain_classification.launch.py new file mode 100644 index 0000000..a2de7d5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/launch/terrain_classification.launch.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +"""Launch file for terrain classification node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + + +def generate_launch_description(): + """Generate launch description.""" + depth_topic = LaunchConfiguration("depth_topic", default="/camera/depth/image_rect_raw") + lidar_topic = LaunchConfiguration("lidar_topic", default="/scan") + control_rate = LaunchConfiguration("control_rate", default="10.0") + confidence_threshold = LaunchConfiguration("confidence_threshold", default="0.5") + + return LaunchDescription([ + DeclareLaunchArgument("depth_topic", default_value="/camera/depth/image_rect_raw", + description="RealSense depth topic"), + DeclareLaunchArgument("lidar_topic", default_value="/scan", + description="RPLIDAR scan topic"), + DeclareLaunchArgument("control_rate", default_value="10.0", + description="Control loop rate (Hz)"), + DeclareLaunchArgument("confidence_threshold", default_value="0.5", + description="Minimum confidence to publish"), + + Node( + package="saltybot_terrain_classification", + executable="terrain_classification_node", + name="terrain_classification", + output="screen", + parameters=[ + {"depth_topic": depth_topic}, + {"lidar_topic": lidar_topic}, + {"control_rate": control_rate}, + {"confidence_threshold": confidence_threshold}, + {"depth_std_threshold": 0.02}, + {"min_depth_m": 0.1}, + {"max_depth_m": 3.0}, + {"hysteresis_samples": 5}, + ], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/package.xml b/jetson/ros2_ws/src/saltybot_terrain_classification/package.xml new file mode 100644 index 0000000..8ce38d4 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/package.xml @@ -0,0 +1,29 @@ + + + + saltybot_terrain_classification + 0.1.0 + Terrain classification using RealSense depth and RPLIDAR (Issue #469) + SaltyLab + MIT + + ament_python + + rclpy + std_msgs + geometry_msgs + sensor_msgs + message_filters + numpy + opencv-python + cv_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/resource/saltybot_terrain_classification b/jetson/ros2_ws/src/saltybot_terrain_classification/resource/saltybot_terrain_classification new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/__init__.py b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/depth_extractor.py b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/depth_extractor.py new file mode 100644 index 0000000..e34ba60 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/depth_extractor.py @@ -0,0 +1,64 @@ +"""Depth Feature Extractor — Surface roughness from RealSense depth.""" + +from __future__ import annotations +import numpy as np +from typing import Optional +import cv2 + + +class DepthExtractor: + """Extract terrain roughness features from RealSense depth images.""" + + def __init__( + self, + roi_width_px: int = 320, + roi_height_px: int = 240, + min_depth_m: float = 0.1, + max_depth_m: float = 3.0, + bilateral_d: int = 5, + ): + self._roi_w = roi_width_px + self._roi_h = roi_height_px + self._min_depth = min_depth_m + self._max_depth = max_depth_m + self._bilateral_d = bilateral_d + + def extract_roughness(self, depth_image: np.ndarray) -> Optional[float]: + """Extract surface roughness from depth image.""" + if depth_image is None or depth_image.size == 0: + return None + + if depth_image.dtype == np.uint16: + depth_m = depth_image.astype(np.float32) / 1000.0 + else: + depth_m = depth_image.astype(np.float32) + + h, w = depth_m.shape + x_start = (w - self._roi_w) // 2 + y_start = (h - self._roi_h) // 2 + x_end = x_start + self._roi_w + y_end = y_start + self._roi_h + + roi = depth_m[max(0, y_start):min(h, y_end), max(0, x_start):min(w, x_end)] + + valid_mask = (roi > self._min_depth) & (roi < self._max_depth) + valid_depths = roi[valid_mask] + + if len(valid_depths) < 10: + return None + + roi_smooth = cv2.bilateralFilter(roi.astype(np.float32), self._bilateral_d, 0.1, 1.0) + depth_variance = np.var(valid_depths) + + grad_x = cv2.Sobel(roi_smooth, cv2.CV_32F, 1, 0, ksize=3) + grad_y = cv2.Sobel(roi_smooth, cv2.CV_32F, 0, 1, ksize=3) + grad_magnitude = np.sqrt(grad_x**2 + grad_y**2) + + valid_grad = grad_magnitude[valid_mask] + grad_mean = np.mean(valid_grad) if len(valid_grad) > 0 else 0.0 + + depth_roughness = min(1.0, depth_variance / 0.05) + grad_roughness = min(1.0, grad_mean / 0.02) + + roughness = 0.6 * depth_roughness + 0.4 * grad_roughness + return float(roughness) diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/lidar_extractor.py b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/lidar_extractor.py new file mode 100644 index 0000000..6dced98 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/lidar_extractor.py @@ -0,0 +1,58 @@ +"""LIDAR Feature Extractor — Surface characteristics from RPLIDAR.""" + +from __future__ import annotations +import numpy as np +from typing import Optional +from collections import deque + + +class LidarExtractor: + """Extract terrain reflectance features from RPLIDAR scans.""" + + def __init__( + self, + ground_angle_front: float = 10.0, + ground_angle_rear: float = 170.0, + reflectance_window: int = 20, + ): + self._front_angle = ground_angle_front + self._rear_angle = ground_angle_rear + self._refl_window = deque(maxlen=reflectance_window) + + def extract_reflectance(self, ranges: np.ndarray, intensities: np.ndarray) -> Optional[float]: + """Extract mean reflectance from ground-hitting rays.""" + if len(ranges) == 0 or len(intensities) == 0: + return None + + intensities_norm = intensities.astype(np.float32) + if np.max(intensities_norm) > 1.5: + intensities_norm /= 255.0 + else: + intensities_norm = np.clip(intensities_norm, 0.0, 1.0) + + valid_mask = (ranges > 0.2) & (ranges < 5.0) + valid_intensities = intensities_norm[valid_mask] + + if len(valid_intensities) < 5: + return None + + mean_reflectance = float(np.mean(valid_intensities)) + self._refl_window.append(mean_reflectance) + + if self._refl_window: + return float(np.mean(list(self._refl_window))) + return mean_reflectance + + def extract_range_variance(self, ranges: np.ndarray) -> Optional[float]: + """Extract surface variance from range measurements.""" + if len(ranges) == 0: + return None + + valid_ranges = ranges[(ranges > 0.2) & (ranges < 5.0)] + + if len(valid_ranges) < 5: + return None + + range_variance = float(np.var(valid_ranges)) + normalized = min(1.0, range_variance / 0.2) + return float(normalized) diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classification_node.py b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classification_node.py new file mode 100644 index 0000000..59b4775 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classification_node.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python3 +""" +terrain_classification_node.py — Terrain classification (Issue #469). + +Fuses RealSense D435i depth + RPLIDAR A1M8 to classify terrain and recommend speed. + +Pipeline (10 Hz) +──────────────── + 1. Extract depth-based roughness features + 2. Extract lidar-based reflectance features + 3. Classify terrain using rule-based matcher + 4. Publish /saltybot/terrain_type (JSON + string) + 5. Publish speed recommendations + +Publishes +───────── + /saltybot/terrain_type std_msgs/String — JSON: type, confidence, speed_scale + /saltybot/terrain_type_string std_msgs/String — Human-readable type name + /saltybot/terrain_speed_scale std_msgs/Float32 — Speed multiplier [0.0, 1.0] +""" + +import json +import time +import numpy as np + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from sensor_msgs.msg import Image, LaserScan +from std_msgs.msg import String, Float32 +import cv2 +from cv_bridge import CvBridge + +from saltybot_terrain_classification.terrain_classifier import TerrainClassifier +from saltybot_terrain_classification.depth_extractor import DepthExtractor +from saltybot_terrain_classification.lidar_extractor import LidarExtractor + + +class TerrainClassificationNode(Node): + + def __init__(self): + super().__init__("terrain_classification") + + self._declare_params() + p = self._load_params() + + self._classifier = TerrainClassifier( + depth_std_threshold=p["depth_std_threshold"], + hysteresis_count=p["hysteresis_samples"], + ) + self._depth_extractor = DepthExtractor( + roi_width_px=320, + roi_height_px=240, + min_depth_m=p["min_depth_m"], + max_depth_m=p["max_depth_m"], + ) + self._lidar_extractor = LidarExtractor() + self._cv_bridge = CvBridge() + + self._latest_depth_image = None + self._latest_scan = None + self._last_depth_t = 0.0 + self._last_scan_t = 0.0 + + depth_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + lidar_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + pub_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + + self.create_subscription( + Image, p["depth_topic"], self._depth_cb, depth_qos + ) + self.create_subscription( + LaserScan, p["lidar_topic"], self._scan_cb, lidar_qos + ) + + self._terrain_type_pub = self.create_publisher( + String, "/saltybot/terrain_type", pub_qos + ) + self._terrain_type_string_pub = self.create_publisher( + String, "/saltybot/terrain_type_string", pub_qos + ) + self._speed_scale_pub = self.create_publisher( + Float32, "/saltybot/terrain_speed_scale", pub_qos + ) + + rate = p["control_rate"] + self._timer = self.create_timer(1.0 / rate, self._control_cb) + + self.get_logger().info( + f"TerrainClassificationNode ready rate={rate}Hz " + f"depth={p['depth_topic']} lidar={p['lidar_topic']}" + ) + + def _declare_params(self) -> None: + self.declare_parameter("control_rate", 10.0) + self.declare_parameter("depth_topic", "/camera/depth/image_rect_raw") + self.declare_parameter("lidar_topic", "/scan") + self.declare_parameter("depth_std_threshold", 0.02) + self.declare_parameter("min_depth_m", 0.1) + self.declare_parameter("max_depth_m", 3.0) + self.declare_parameter("confidence_threshold", 0.5) + self.declare_parameter("hysteresis_samples", 5) + + def _load_params(self) -> dict: + g = self.get_parameter + return {k: g(k).value for k in [ + "control_rate", "depth_topic", "lidar_topic", + "depth_std_threshold", "min_depth_m", "max_depth_m", + "confidence_threshold", "hysteresis_samples", + ]} + + def _depth_cb(self, msg: Image) -> None: + self._latest_depth_image = msg + self._last_depth_t = time.monotonic() + + def _scan_cb(self, msg: LaserScan) -> None: + self._latest_scan = msg + self._last_scan_t = time.monotonic() + + def _control_cb(self) -> None: + p = self._load_params() + now = time.monotonic() + + depth_age = (now - self._last_depth_t) if self._last_depth_t > 0.0 else 1e9 + scan_age = (now - self._last_scan_t) if self._last_scan_t > 0.0 else 1e9 + + if depth_age > 1.0 or scan_age > 1.0: + return + + depth_roughness = None + lidar_reflectance = None + + if self._latest_depth_image is not None: + try: + depth_cv = self._cv_bridge.imgmsg_to_cv2( + self._latest_depth_image, desired_encoding="passthrough" + ) + depth_roughness = self._depth_extractor.extract_roughness(depth_cv) + except Exception: + pass + + if self._latest_scan is not None: + try: + ranges = np.array(self._latest_scan.ranges, dtype=np.float32) + intensities = np.array(self._latest_scan.intensities, dtype=np.float32) + lidar_reflectance = self._lidar_extractor.extract_reflectance( + ranges, intensities + ) + except Exception: + pass + + if depth_roughness is None: + depth_roughness = 0.3 + if lidar_reflectance is None: + lidar_reflectance = 0.5 + + classification = self._classifier.update(depth_roughness, lidar_reflectance) + + if classification.confidence < p["confidence_threshold"]: + return + + self._publish_terrain_type(classification) + self._publish_speed_scale(classification.speed_scale) + + self.get_logger().info( + f"Terrain: {classification.surface_type} " + f"confidence={classification.confidence:.2f} " + f"speed_scale={classification.speed_scale:.2f}" + ) + + def _publish_terrain_type(self, classification) -> None: + """Publish terrain classification JSON.""" + msg = String() + msg.data = json.dumps({ + "surface_type": classification.surface_type, + "confidence": round(classification.confidence, 3), + "roughness": round(classification.roughness, 3), + "reflectance": round(classification.reflectance, 3), + "speed_scale": round(classification.speed_scale, 3), + }) + self._terrain_type_pub.publish(msg) + + msg_str = String() + msg_str.data = classification.surface_type + self._terrain_type_string_pub.publish(msg_str) + + def _publish_speed_scale(self, scale: float) -> None: + """Publish speed scale multiplier.""" + msg = Float32() + msg.data = float(scale) + self._speed_scale_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = TerrainClassificationNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classifier.py b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classifier.py new file mode 100644 index 0000000..3071d4e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/saltybot_terrain_classification/terrain_classifier.py @@ -0,0 +1,127 @@ +"""Terrain Classifier — Multi-sensor classification (Issue #469).""" + +from __future__ import annotations +from dataclasses import dataclass +from collections import deque +import numpy as np +from typing import Optional + + +@dataclass +class TerrainClassification: + """Classification result with confidence.""" + surface_type: str + confidence: float + roughness: float + reflectance: float + speed_scale: float + + +class TerrainClassifier: + """Multi-sensor terrain classifier using depth variance + reflectance.""" + + _SURFACE_RULES = [ + ("smooth", 0.10, 0.70, 1.00, 1.00), + ("carpet", 0.25, 0.60, 0.90, 0.90), + ("grass", 0.40, 0.40, 0.70, 0.75), + ("gravel", 1.00, 0.20, 0.60, 0.60), + ] + + def __init__( + self, + depth_std_threshold: float = 0.02, + reflectance_window_size: int = 10, + hysteresis_count: int = 5, + control_rate_hz: float = 10.0, + ): + self._depth_std_threshold = depth_std_threshold + self._hysteresis_count = hysteresis_count + self._depth_vars = deque(maxlen=reflectance_window_size) + self._reflectances = deque(maxlen=reflectance_window_size) + self._current_type = "unknown" + self._current_confidence = 0.0 + self._candidate_type = "unknown" + self._candidate_count = 0 + + @property + def current_surface_type(self) -> str: + return self._current_type + + @property + def current_confidence(self) -> float: + return self._current_confidence + + def update(self, depth_variance: float, reflectance_mean: float) -> TerrainClassification: + """Update classifier with sensor measurements.""" + roughness = min(1.0, depth_variance / (self._depth_std_threshold + 1e-6)) + reflectance = np.clip(reflectance_mean, 0.0, 1.0) + + self._depth_vars.append(roughness) + self._reflectances.append(reflectance) + + avg_roughness = np.mean(list(self._depth_vars)) if self._depth_vars else 0.0 + avg_reflectance = np.mean(list(self._reflectances)) if self._reflectances else 0.5 + + candidate_type, candidate_confidence, speed_scale = self._classify( + avg_roughness, avg_reflectance + ) + + if candidate_type == self._candidate_type: + self._candidate_count += 1 + else: + self._candidate_type = candidate_type + self._candidate_count = 1 + + if self._candidate_count >= self._hysteresis_count: + self._current_type = candidate_type + self._current_confidence = candidate_confidence + + return TerrainClassification( + surface_type=self._current_type, + confidence=self._current_confidence, + roughness=avg_roughness, + reflectance=avg_reflectance, + speed_scale=speed_scale, + ) + + def reset(self) -> None: + """Clear buffers and reset to unknown.""" + self._depth_vars.clear() + self._reflectances.clear() + self._current_type = "unknown" + self._current_confidence = 0.0 + self._candidate_type = "unknown" + self._candidate_count = 0 + + def _classify(self, roughness: float, reflectance: float) -> tuple[str, float, float]: + """Classify terrain and compute confidence + speed scale.""" + best_match = None + best_score = 0.0 + best_speed = 1.0 + + for name, rough_max, refl_min, refl_max, speed in self._SURFACE_RULES: + rough_penalty = max(0.0, (roughness - rough_max) / rough_max) if roughness > rough_max else 0.0 + refl_penalty = 0.0 + + if reflectance < refl_min: + refl_penalty = (refl_min - reflectance) / refl_min + elif reflectance > refl_max: + refl_penalty = (reflectance - refl_max) / (1.0 - refl_max) + + score = 1.0 - (rough_penalty + refl_penalty) * 0.5 + + if score > best_score: + best_score = score + best_match = name + best_speed = speed + + confidence = max(0.0, best_score) + return best_match or "unknown", confidence, best_speed + + @staticmethod + def speed_scale_for_surface(surface_type: str) -> float: + """Get speed scale for a surface type.""" + for name, _, _, _, speed in TerrainClassifier._SURFACE_RULES: + if name == surface_type: + return speed + return 0.75 diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/setup.cfg b/jetson/ros2_ws/src/saltybot_terrain_classification/setup.cfg new file mode 100644 index 0000000..15c0f01 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_terrain_classification +[install] +script_dir=$base/lib/saltybot_terrain_classification diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/setup.py b/jetson/ros2_ws/src/saltybot_terrain_classification/setup.py new file mode 100644 index 0000000..fe2ac96 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_terrain_classification/setup.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +from setuptools import setup, find_packages + +setup( + name="saltybot_terrain_classification", + version="0.1.0", + packages=find_packages(), + data_files=[ + ("share/ament_index/resource_index/packages", + ["resource/saltybot_terrain_classification"]), + ("share/saltybot_terrain_classification", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="SaltyLab", + maintainer_email="seb@example.com", + description="Terrain classification using RealSense depth and RPLIDAR (Issue #469)", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "terrain_classification_node=saltybot_terrain_classification.terrain_classification_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_terrain_classification/test/__init__.py b/jetson/ros2_ws/src/saltybot_terrain_classification/test/__init__.py new file mode 100644 index 0000000..e69de29