jetson/ros2_ws/src/saltybot_lidar_avoidance/config/lidar_avoidance_params.yaml

@@ -1,31 +0,0 @@
-# LIDAR Avoidance Configuration for SaltyBot
-# 360° obstacle detection with RPLIDAR A1M8
-
-lidar_avoidance:
-  ros__parameters:
-    # Emergency stop distance threshold (meters)
-    # Robot will trigger hard stop if obstacle closer than this
-    emergency_stop_distance: 0.5
-
-    # Reference speed for safety zone calculation (m/s)
-    # 5.56 m/s = 20 km/h
-    max_speed_reference: 5.56
-
-    # Safety zone distance at maximum reference speed (meters)
-    # At 20 km/h, robot maintains 3m clearance before reducing speed
-    safety_zone_at_max_speed: 3.0
-
-    # Minimum safety zone distance (meters)
-    # At zero speed, robot maintains this clearance
-    # Must be >= emergency_stop_distance for smooth operation
-    min_safety_zone: 0.6
-
-    # Forward scanning window (degrees)
-    # ±30° forward cone = 60° total forward scan window
-    # RPLIDAR A1M8 provides full 360° data, but we focus on forward obstacles
-    angle_window_degrees: 60
-
-    # Debounce frames for obstacle detection
-    # Number of consecutive scans with obstacle before triggering alert
-    # Reduces false positives from noise/reflections
-    debounce_frames: 2

jetson/ros2_ws/src/saltybot_lidar_avoidance/launch/lidar_avoidance.launch.py

@@ -1,33 +0,0 @@
-"""Launch file for LIDAR avoidance node."""
-
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
-import os
-from ament_index_python.packages import get_package_share_directory
-
-
-def generate_launch_description():
-    """Generate launch description for LIDAR avoidance."""
-    pkg_dir = get_package_share_directory("saltybot_lidar_avoidance")
-    config_file = os.path.join(
-        pkg_dir, "config", "lidar_avoidance_params.yaml"
-    )
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                "config_file",
-                default_value=config_file,
-                description="Path to configuration YAML file",
-            ),
-            Node(
-                package="saltybot_lidar_avoidance",
-                executable="lidar_avoidance_node",
-                name="lidar_avoidance",
-                output="screen",
-                parameters=[LaunchConfiguration("config_file")],
-            ),
-        ]
-    )

jetson/ros2_ws/src/saltybot_lidar_avoidance/package.xml

@@ -1,29 +0,0 @@
-<?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_lidar_avoidance</name>
-  <version>0.1.0</version>
-  <description>
-    360° LIDAR obstacle avoidance for SaltyBot using RPLIDAR A1M8.
-    Publishes local costmap, obstacle alerts, and filtered cmd_vel with emergency stop.
-  </description>
-  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
-  <license>MIT</license>
-
-  <depend>rclpy</depend>
-  <depend>geometry_msgs</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>nav_msgs</depend>
-
-  <buildtool_depend>ament_python</buildtool_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>

jetson/ros2_ws/src/saltybot_lidar_avoidance/saltybot_lidar_avoidance/__init__.py

@@ -1 +0,0 @@
-"""SaltyBot LIDAR obstacle avoidance package."""

jetson/ros2_ws/src/saltybot_lidar_avoidance/saltybot_lidar_avoidance/lidar_avoidance_node.py

@@ -1,239 +0,0 @@
-#!/usr/bin/env python3
-"""360° LIDAR obstacle avoidance node for SaltyBot.
-
-Uses RPLIDAR A1M8 for 360° scanning with speed-dependent safety zones.
-Publishes emergency alerts and filtered cmd_vel with obstacle avoidance.
-
-Subscribed topics:
-  /scan (sensor_msgs/LaserScan) - RPLIDAR A1M8 scan data
-  /cmd_vel (geometry_msgs/Twist) - Input velocity command
-
-Published topics:
-  /saltybot/obstacle_alert (std_msgs/Bool) - Obstacle detected alert
-  /cmd_vel_safe (geometry_msgs/Twist) - Filtered velocity (avoidance applied)
-  /saltybot/lidar_avoidance_status (std_msgs/String) - Debug status JSON
-"""
-
-import json
-import math
-from typing import Tuple
-
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import LaserScan
-from geometry_msgs.msg import Twist
-from std_msgs.msg import Bool, String
-
-
-class LidarAvoidanceNode(Node):
-    """360° LIDAR obstacle avoidance with speed-dependent safety zones."""
-
-    def __init__(self):
-        super().__init__("lidar_avoidance")
-
-        # Safety parameters
-        self.declare_parameter("emergency_stop_distance", 0.5)  # m
-        self.declare_parameter("max_speed_reference", 5.56)  # m/s (20 km/h)
-        self.declare_parameter("safety_zone_at_max_speed", 3.0)  # m
-        self.declare_parameter("min_safety_zone", 0.6)  # m (below emergency stop)
-        self.declare_parameter("angle_window_degrees", 60)  # ±30° forward cone
-        self.declare_parameter("debounce_frames", 2)
-
-        self.emergency_stop_distance = self.get_parameter("emergency_stop_distance").value
-        self.max_speed_reference = self.get_parameter("max_speed_reference").value
-        self.safety_zone_at_max_speed = self.get_parameter("safety_zone_at_max_speed").value
-        self.min_safety_zone = self.get_parameter("min_safety_zone").value
-        self.angle_window_degrees = self.get_parameter("angle_window_degrees").value
-        self.debounce_frames = self.get_parameter("debounce_frames").value
-
-        # State tracking
-        self.obstacle_detected = False
-        self.consecutive_obstacles = 0
-        self.current_speed = 0.0
-        self.last_scan_ranges = None
-        self.emergency_stop_triggered = False
-
-        # Subscriptions
-        self.create_subscription(LaserScan, "/scan", self._on_scan, 10)
-        self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)
-
-        # Publishers
-        self.pub_alert = self.create_publisher(Bool, "/saltybot/obstacle_alert", 10)
-        self.pub_safe_vel = self.create_publisher(Twist, "/cmd_vel_safe", 10)
-        self.pub_status = self.create_publisher(
-            String, "/saltybot/lidar_avoidance_status", 10
-        )
-
-        self.get_logger().info(
-            f"LIDAR avoidance initialized:\n"
-            f"  Emergency stop: {self.emergency_stop_distance}m\n"
-            f"  Speed-dependent zone: {self.safety_zone_at_max_speed}m @ {self.max_speed_reference}m/s\n"
-            f"  Forward angle window: ±{self.angle_window_degrees / 2}°\n"
-            f"  Min safety zone: {self.min_safety_zone}m"
-        )
-
-    def _on_scan(self, msg: LaserScan) -> None:
-        """Process LIDAR scan data and check for obstacles."""
-        self.last_scan_ranges = msg.ranges
-
-        # Calculate safety threshold based on current speed
-        safety_distance = self._get_safety_distance(self.current_speed)
-
-        # Get minimum distance in forward cone
-        min_distance, angle_deg = self._get_min_distance_forward(msg)
-
-        # Check for obstacles
-        obstacle_now = min_distance < safety_distance
-        emergency_stop_now = min_distance < self.emergency_stop_distance
-
-        # Debounce obstacle detection
-        if obstacle_now:
-            self.consecutive_obstacles += 1
-        else:
-            self.consecutive_obstacles = 0
-
-        obstacle_detected_debounced = (
-            self.consecutive_obstacles >= self.debounce_frames
-        )
-
-        # Handle state changes
-        if emergency_stop_now and not self.emergency_stop_triggered:
-            self.get_logger().error(
-                f"EMERGENCY STOP! Obstacle at {min_distance:.2f}m, {angle_deg:.1f}°"
-            )
-            self.emergency_stop_triggered = True
-        elif not emergency_stop_now:
-            self.emergency_stop_triggered = False
-
-        if obstacle_detected_debounced != self.obstacle_detected:
-            self.obstacle_detected = obstacle_detected_debounced
-            if self.obstacle_detected:
-                self.get_logger().warn(
-                    f"Obstacle detected: {min_distance:.2f}m @ {angle_deg:.1f}°"
-                )
-            else:
-                self.get_logger().info("Obstacle cleared")
-
-        # Publish alert
-        alert_msg = Bool(data=self.obstacle_detected)
-        self.pub_alert.publish(alert_msg)
-
-        # Publish status
-        status = {
-            "min_distance": round(min_distance, 3),
-            "angle_deg": round(angle_deg, 1),
-            "safety_distance": round(safety_distance, 3),
-            "obstacle_detected": self.obstacle_detected,
-            "emergency_stop": self.emergency_stop_triggered,
-            "current_speed": round(self.current_speed, 3),
-        }
-        status_msg = String(data=json.dumps(status))
-        self.pub_status.publish(status_msg)
-
-    def _on_cmd_vel(self, msg: Twist) -> None:
-        """Process incoming velocity command and apply obstacle avoidance."""
-        self.current_speed = math.sqrt(msg.linear.x**2 + msg.linear.y**2)
-
-        # Apply safety filtering
-        if self.emergency_stop_triggered:
-            # Emergency stop: zero out all motion
-            safe_vel = Twist()
-        elif self.obstacle_detected:
-            # Obstacle in path: reduce speed
-            safe_vel = Twist()
-            safety_distance = self._get_safety_distance(self.current_speed)
-            min_distance, _ = self._get_min_distance_forward(self.last_scan_ranges)
-
-            if self.last_scan_ranges is not None and min_distance > 0:
-                # Linear interpolation of allowed speed based on distance to obstacle
-                if min_distance < safety_distance:
-                    # Scale velocity from 0 to current based on distance
-                    scale_factor = (min_distance - self.emergency_stop_distance) / (
-                        safety_distance - self.emergency_stop_distance
-                    )
-                    scale_factor = max(0.0, min(1.0, scale_factor))
-
-                    safe_vel.linear.x = msg.linear.x * scale_factor
-                    safe_vel.linear.y = msg.linear.y * scale_factor
-                    safe_vel.angular.z = msg.angular.z * scale_factor
-                else:
-                    safe_vel = msg
-            else:
-                safe_vel = msg
-        else:
-            # No obstacle: pass through command
-            safe_vel = msg
-
-        self.pub_safe_vel.publish(safe_vel)
-
-    def _get_safety_distance(self, speed: float) -> float:
-        """Calculate speed-dependent safety zone distance.
-
-        Linear interpolation: 0 m/s → min_safety_zone, max_speed → safety_zone_at_max_speed
-        """
-        if speed <= 0:
-            return self.min_safety_zone
-
-        if speed >= self.max_speed_reference:
-            return self.safety_zone_at_max_speed
-
-        # Linear interpolation
-        ratio = speed / self.max_speed_reference
-        safety = self.min_safety_zone + ratio * (
-            self.safety_zone_at_max_speed - self.min_safety_zone
-        )
-        return safety
-
-    def _get_min_distance_forward(self, scan_data) -> Tuple[float, float]:
-        """Get minimum distance in forward cone."""
-        if isinstance(scan_data, LaserScan):
-            ranges = scan_data.ranges
-            angle_min = scan_data.angle_min
-            angle_increment = scan_data.angle_increment
-        else:
-            # scan_data is a tuple of (ranges, angle_min, angle_increment) or list
-            if not scan_data:
-                return float('inf'), 0.0
-            ranges = scan_data
-            angle_min = -math.pi  # Assume standard LIDAR orientation
-            angle_increment = 2 * math.pi / len(ranges)
-
-        half_window = self.angle_window_degrees / 2.0 * math.pi / 180.0
-        min_distance = float('inf')
-        min_angle = 0.0
-
-        for i, distance in enumerate(ranges):
-            if distance <= 0 or math.isnan(distance) or math.isinf(distance):
-                continue
-
-            angle_rad = angle_min + i * angle_increment
-
-            # Normalize to -π to π
-            while angle_rad > math.pi:
-                angle_rad -= 2 * math.pi
-            while angle_rad < -math.pi:
-                angle_rad += 2 * math.pi
-
-            # Check forward window
-            if abs(angle_rad) <= half_window:
-                if distance < min_distance:
-                    min_distance = distance
-                    min_angle = angle_rad
-
-        return min_distance, math.degrees(min_angle)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = LidarAvoidanceNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

jetson/ros2_ws/src/saltybot_lidar_avoidance/setup.cfg

@@ -1,5 +0,0 @@
-[develop]
-script_dir=$base/lib/saltybot_lidar_avoidance
-
-[install]
-install_scripts=$base/lib/saltybot_lidar_avoidance

jetson/ros2_ws/src/saltybot_lidar_avoidance/setup.py

@@ -1,27 +0,0 @@
-from setuptools import setup
-
-package_name = "saltybot_lidar_avoidance"
-
-setup(
-    name=package_name,
-    version="0.1.0",
-    packages=[package_name],
-    data_files=[
-        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
-        (f"share/{package_name}", ["package.xml"]),
-        (f"share/{package_name}/launch", ["launch/lidar_avoidance.launch.py"]),
-        (f"share/{package_name}/config", ["config/lidar_avoidance_params.yaml"]),
-    ],
-    install_requires=["setuptools"],
-    zip_safe=True,
-    maintainer="sl-controls",
-    maintainer_email="sl-controls@saltylab.local",
-    description="360° LIDAR obstacle avoidance with emergency stop and speed-dependent safety zones",
-    license="MIT",
-    tests_require=["pytest"],
-    entry_points={
-        "console_scripts": [
-            "lidar_avoidance_node = saltybot_lidar_avoidance.lidar_avoidance_node:main",
-        ],
-    },
-)

jetson/ros2_ws/src/saltybot_lidar_avoidance/test/test_lidar_avoidance.py

@@ -1,102 +0,0 @@
-"""Unit tests for LIDAR avoidance node."""
-
-import math
-import pytest
-from sensor_msgs.msg import LaserScan
-from geometry_msgs.msg import Twist
-from std_msgs.msg import Bool
-
-
-class MockNode:
-    """Mock ROS2 node for testing."""
-
-    def __init__(self):
-        from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
-        import rclpy
-
-        rclpy.init(allow_reuse=True)
-        self.node = LidarAvoidanceNode()
-
-    def cleanup(self):
-        self.node.destroy_node()
-
-
-def create_scan(
-    ranges, angle_min=-math.pi, angle_increment=2 * math.pi / 360
-):
-    """Create a LaserScan message."""
-    scan = LaserScan()
-    scan.ranges = ranges
-    scan.angle_min = angle_min
-    scan.angle_increment = angle_increment
-    scan.angle_max = angle_min + angle_increment * (len(ranges) - 1)
-    return scan
-
-
-def test_safety_distance_zero_speed():
-    """Test safety distance at zero speed."""
-    import rclpy
-
-    rclpy.init(allow_reuse=True)
-    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
-
-    node = LidarAvoidanceNode()
-    safety = node._get_safety_distance(0.0)
-    assert safety == node.min_safety_zone
-    node.destroy_node()
-
-
-def test_safety_distance_max_speed():
-    """Test safety distance at max speed."""
-    import rclpy
-
-    rclpy.init(allow_reuse=True)
-    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
-
-    node = LidarAvoidanceNode()
-    safety = node._get_safety_distance(node.max_speed_reference)
-    assert abs(safety - node.safety_zone_at_max_speed) < 0.01
-    node.destroy_node()
-
-
-def test_safety_distance_interpolation():
-    """Test safety distance linear interpolation."""
-    import rclpy
-
-    rclpy.init(allow_reuse=True)
-    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
-
-    node = LidarAvoidanceNode()
-    half_speed = node.max_speed_reference / 2.0
-    safety = node._get_safety_distance(half_speed)
-
-    expected = node.min_safety_zone + 0.5 * (
-        node.safety_zone_at_max_speed - node.min_safety_zone
-    )
-    assert abs(safety - expected) < 0.01
-    node.destroy_node()
-
-
-def test_min_distance_forward():
-    """Test forward distance detection."""
-    import rclpy
-
-    rclpy.init(allow_reuse=True)
-    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
-
-    node = LidarAvoidanceNode()
-
-    # Create scan with obstacle at 0° (forward)
-    ranges = [float('inf')] * 360
-    ranges[180] = 1.5  # Obstacle at index 180 (0° in normalized coordinates)
-
-    scan = create_scan(ranges)
-    min_dist, angle = node._get_min_distance_forward(scan)
-
-    assert min_dist == 1.5
-    assert abs(angle) < 10  # Close to forward direction
-    node.destroy_node()
-
-
-if __name__ == "__main__":
-    pytest.main([__file__, "-v"])