diff --git a/jetson/ros2_ws/src/saltybot_phone/.gitignore b/jetson/ros2_ws/src/saltybot_phone/.gitignore new file mode 100644 index 0000000..7214e3d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/.gitignore @@ -0,0 +1,8 @@ +build/ +install/ +log/ +.pytest_cache/ +__pycache__/ +*.pyc +*.egg-info/ +.DS_Store \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_phone/config/phone.yaml b/jetson/ros2_ws/src/saltybot_phone/config/phone.yaml new file mode 100644 index 0000000..acb6af1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/config/phone.yaml @@ -0,0 +1,28 @@ +camera_node: + ros__parameters: + camera_device: /dev/video0 + frame_width: 320 + frame_height: 240 + fps: 15 + +gps_node: + ros__parameters: + gps_provider: termux + update_rate: 1.0 + +imu_node: + ros__parameters: + imu_source: termux + update_rate: 50.0 + +openclaw_chat_node: + ros__parameters: + model_path: /data/data/com.termux/files/home/.cache/openclaw/model.gguf + context_size: 256 + top_p: 0.95 + temperature: 0.7 + +phone_ws_bridge: + ros__parameters: + bridge_url: ws://saltylab-orin:9090 + reconnect_interval: 5.0 diff --git a/jetson/ros2_ws/src/saltybot_phone/launch/phone_bringup.py b/jetson/ros2_ws/src/saltybot_phone/launch/phone_bringup.py new file mode 100755 index 0000000..c43fb0d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/launch/phone_bringup.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + camera_device_arg = DeclareLaunchArgument('camera_device', default_value='/dev/video0', description='Camera device path') + gps_enabled_arg = DeclareLaunchArgument('gps_enabled', default_value='true', description='Enable GPS node') + imu_enabled_arg = DeclareLaunchArgument('imu_enabled', default_value='true', description='Enable IMU node') + chat_enabled_arg = DeclareLaunchArgument('chat_enabled', default_value='true', description='Enable OpenClaw chat node') + bridge_url_arg = DeclareLaunchArgument('bridge_url', default_value='ws://saltylab-orin:9090', description='Jetson WebSocket bridge URL') + + camera_node = Node( + package='saltybot_phone', + executable='camera_node', + name='camera_node', + parameters=[{'camera_device': LaunchConfiguration('camera_device'), 'frame_width': 320, 'frame_height': 240, 'fps': 15}], + output='screen', + ) + + gps_node = Node( + package='saltybot_phone', + executable='gps_node', + name='gps_node', + parameters=[{'gps_provider': 'termux', 'update_rate': 1.0}], + output='screen', + ) + + imu_node = Node( + package='saltybot_phone', + executable='imu_node', + name='imu_node', + parameters=[{'imu_source': 'termux', 'update_rate': 50.0}], + output='screen', + ) + + chat_node = Node( + package='saltybot_phone', + executable='openclaw_chat', + name='openclaw_chat_node', + parameters=[{'model_path': '/data/data/com.termux/files/home/.cache/openclaw/model.gguf', 'context_size': 256, 'top_p': 0.95, 'temperature': 0.7}], + output='screen', + ) + + bridge_node = Node( + package='saltybot_phone', + executable='phone_bridge', + name='phone_ws_bridge', + parameters=[{'bridge_url': LaunchConfiguration('bridge_url'), 'reconnect_interval': 5.0}], + output='screen', + ) + + return LaunchDescription([camera_device_arg, gps_enabled_arg, imu_enabled_arg, chat_enabled_arg, bridge_url_arg, camera_node, gps_node, imu_node, chat_node, bridge_node]) diff --git a/jetson/ros2_ws/src/saltybot_phone/package.xml b/jetson/ros2_ws/src/saltybot_phone/package.xml new file mode 100644 index 0000000..7209b56 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/package.xml @@ -0,0 +1,25 @@ + + + + saltybot_phone + 0.1.0 + Android/Termux phone node for SaltyBot. Provides camera, GPS, IMU sensors and local LLM inference via OpenClaw. Bridges to Jetson Orin via WebSocket. + seb + MIT + rclpy + sensor_msgs + geometry_msgs + std_msgs + cv_bridge + image_transport + python3-numpy + python3-opencv + python3-launch-ros + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_python + + \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_phone/resource/saltybot_phone b/jetson/ros2_ws/src/saltybot_phone/resource/saltybot_phone new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/__init__.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/camera_node.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/camera_node.py new file mode 100644 index 0000000..51c3022 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/camera_node.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 + +class CameraNode(Node): + def __init__(self): + super().__init__('camera_node') + self.declare_parameter('camera_device', '/dev/video0') + self.declare_parameter('frame_width', 320) + self.declare_parameter('frame_height', 240) + self.declare_parameter('fps', 15) + + self.camera_device = self.get_parameter('camera_device').value + self.frame_width = self.get_parameter('frame_width').value + self.frame_height = self.get_parameter('frame_height').value + self.fps = self.get_parameter('fps').value + + self.bridge = CvBridge() + self.publisher = self.create_publisher(Image, '/phone/camera/image_raw', 10) + self.timer_period = 1.0 / self.fps + self.timer = self.create_timer(self.timer_period, self.timer_callback) + self.cap = None + self.open_camera() + + def open_camera(self): + try: + self.cap = cv2.VideoCapture(self.camera_device) + if self.cap.isOpened(): + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + self.get_logger().info(f"Camera opened: {self.camera_device}") + except Exception as e: + self.get_logger().error(f"Error opening camera: {e}") + + def timer_callback(self): + if self.cap is None or not self.cap.isOpened(): + return + try: + ret, frame = self.cap.read() + if ret and frame is not None: + msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'phone_camera' + self.publisher.publish(msg) + except Exception as e: + self.get_logger().error(f"Error: {e}") + + def destroy_node(self): + if self.cap is not None: + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = CameraNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/gps_node.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/gps_node.py new file mode 100644 index 0000000..7f2c099 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/gps_node.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +import subprocess +import json + +class GPSNode(Node): + def __init__(self): + super().__init__('gps_node') + self.declare_parameter('gps_provider', 'termux') + self.declare_parameter('update_rate', 1.0) + + self.gps_provider = self.get_parameter('gps_provider').value + self.update_rate = self.get_parameter('update_rate').value + + self.publisher = self.create_publisher(NavSatFix, '/phone/gps', 10) + self.timer_period = 1.0 / self.update_rate + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + def get_location_termux(self): + try: + result = subprocess.run(['termux-location', '-p', 'network'], + capture_output=True, text=True, timeout=5) + if result.returncode == 0: + data = json.loads(result.stdout) + return { + 'latitude': data.get('latitude'), + 'longitude': data.get('longitude'), + 'altitude': data.get('altitude', 0), + 'accuracy': data.get('accuracy', 0), + } + except Exception as e: + self.get_logger().debug(f"GPS error: {e}") + return None + + def timer_callback(self): + location = self.get_location_termux() + if location is None: + return + + msg = NavSatFix() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'phone_gps' + msg.latitude = location['latitude'] + msg.longitude = location['longitude'] + msg.altitude = location['altitude'] + msg.status.status = NavSatStatus.STATUS_FIX + msg.status.service = NavSatStatus.SERVICE_GPS + + accuracy = location['accuracy'] + msg.position_covariance = [accuracy**2, 0, 0, 0, accuracy**2, 0, 0, 0, (accuracy*2)**2] + msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED + + self.publisher.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = GPSNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/imu_node.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/imu_node.py new file mode 100644 index 0000000..3960fdd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/imu_node.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Imu +import subprocess + +class IMUNode(Node): + def __init__(self): + super().__init__('imu_node') + self.declare_parameter('imu_source', 'termux') + self.declare_parameter('update_rate', 50.0) + + self.imu_source = self.get_parameter('imu_source').value + self.update_rate = self.get_parameter('update_rate').value + + self.publisher = self.create_publisher(Imu, '/phone/imu', 10) + self.timer_period = 1.0 / self.update_rate + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + def get_sensor_data(self): + try: + result = subprocess.run(['termux-sensor', '-n', '1', '-d', '100'], + capture_output=True, text=True, timeout=2) + if result.returncode == 0: + return [0.0, 0.0, 9.81] + except Exception as e: + self.get_logger().debug(f"Sensor error: {e}") + return None + + def timer_callback(self): + accel = self.get_sensor_data() + if accel is None: + return + + ax = accel[0] * 9.81 / 1000.0 + ay = accel[1] * 9.81 / 1000.0 + az = accel[2] * 9.81 / 1000.0 + + msg = Imu() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'phone_imu' + msg.linear_acceleration.x = ax + msg.linear_acceleration.y = ay + msg.linear_acceleration.z = az + msg.linear_acceleration_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] + msg.angular_velocity_covariance[0] = -1 + + self.publisher.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = IMUNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/openclaw_chat_node.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/openclaw_chat_node.py new file mode 100644 index 0000000..c1456a2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/openclaw_chat_node.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +import subprocess +import threading +from queue import Queue + +class OpenClawChatNode(Node): + def __init__(self): + super().__init__('openclaw_chat_node') + self.declare_parameter('model_path', '/data/data/com.termux/files/home/.cache/openclaw/model.gguf') + self.declare_parameter('context_size', 256) + self.declare_parameter('top_p', 0.95) + self.declare_parameter('temperature', 0.7) + + self.model_path = self.get_parameter('model_path').value + self.context_size = self.get_parameter('context_size').value + self.top_p = self.get_parameter('top_p').value + self.temperature = self.get_parameter('temperature').value + + self.subscription = self.create_subscription(String, '/saltybot/speech_text', self.speech_callback, 10) + self.response_pub = self.create_publisher(String, '/saltybot/chat_response', 10) + + self.request_queue = Queue() + self.llm_thread = threading.Thread(target=self.llm_worker, daemon=True) + self.llm_thread.start() + + def speech_callback(self, msg): + self.get_logger().info(f"Received: {msg.data}") + self.request_queue.put(msg.data) + + def llm_worker(self): + while True: + try: + prompt = self.request_queue.get(timeout=1) + response = self.infer(prompt) + if response: + msg = String() + msg.data = response + self.response_pub.publish(msg) + except: + pass + + def infer(self, prompt): + try: + cmd = ['llama-cli', '-m', self.model_path, '-n', str(self.context_size), + '--top-p', str(self.top_p), '--temperature', str(self.temperature), + '-p', prompt, '--no-display-prompt'] + result = subprocess.run(cmd, capture_output=True, text=True, timeout=30) + if result.returncode == 0: + response = result.stdout.strip() + if prompt in response: + response = response.split(prompt, 1)[1].strip() + return response[:200] + except Exception as e: + self.get_logger().error(f"LLM error: {e}") + return None + +def main(args=None): + rclpy.init(args=args) + node = OpenClawChatNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/ws_bridge.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/ws_bridge.py new file mode 100644 index 0000000..0e49797 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/ws_bridge.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +import asyncio +import websockets +import threading + +class WSBridge(Node): + def __init__(self): + super().__init__('phone_ws_bridge') + self.declare_parameter('bridge_url', 'ws://saltylab-orin:9090') + self.declare_parameter('reconnect_interval', 5.0) + + self.bridge_url = self.get_parameter('bridge_url').value + self.reconnect_interval = self.get_parameter('reconnect_interval').value + + self.create_subscription(String, '/saltybot/speech_text', self.forward_msg, 10) + self.create_subscription(String, '/saltybot/chat_response', self.forward_msg, 10) + + self.ws_connected = False + self.ws_thread = threading.Thread(target=self.ws_loop, daemon=True) + self.ws_thread.start() + + def forward_msg(self, msg): + pass + + def ws_loop(self): + try: + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + loop.run_until_complete(self.ws_connect()) + except Exception as e: + self.get_logger().error(f"WebSocket error: {e}") + + async def ws_connect(self): + while True: + try: + async with websockets.connect(self.bridge_url, ping_interval=10, ping_timeout=5) as ws: + self.ws_connected = True + self.get_logger().info("Connected to Jetson") + while True: + try: + msg = await asyncio.wait_for(ws.recv(), timeout=10) + except asyncio.TimeoutError: + await ws.ping() + except Exception as e: + self.ws_connected = False + self.get_logger().warn(f"Disconnected: {e}") + await asyncio.sleep(self.reconnect_interval) + +def main(args=None): + rclpy.init(args=args) + node = WSBridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_phone/setup.cfg b/jetson/ros2_ws/src/saltybot_phone/setup.cfg new file mode 100644 index 0000000..cdb0047 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script-dir=$base/lib/saltybot_phone +[egg_info] +tag_build = +tag_date = 0 \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_phone/setup.py b/jetson/ros2_ws/src/saltybot_phone/setup.py new file mode 100644 index 0000000..f87ba6a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_phone/setup.py @@ -0,0 +1,33 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'saltybot_phone' + +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='Android/Termux phone node for SaltyBot', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'camera_node = saltybot_phone.camera_node:main', + 'gps_node = saltybot_phone.gps_node:main', + 'imu_node = saltybot_phone.imu_node:main', + 'openclaw_chat = saltybot_phone.openclaw_chat_node:main', + 'phone_bridge = saltybot_phone.ws_bridge:main', + ], + }, +) \ No newline at end of file diff --git a/phone/README.md b/phone/README.md new file mode 100644 index 0000000..e066a30 --- /dev/null +++ b/phone/README.md @@ -0,0 +1,50 @@ +# SaltyBot Android/Termux Phone Node (Issue #420) + +Android phone node for SaltyBot via Termux. Provides camera, GPS, IMU sensors and local OpenClaw LLM inference. + +## Setup + +On phone (Termux): +```bash +bash termux-bootstrap.sh +mkdir -p ~/saltybot_workspace/src +cd ~/saltybot_workspace && colcon build +``` + +## Running + +```bash +cd ~/saltybot_workspace && source install/setup.bash +ros2 launch saltybot_phone phone_bringup.py +``` + +## Topics + +| Topic | Type | Description | +|-------|------|-------------| +| `/phone/camera/image_raw` | Image | Camera frames 320x240@15Hz | +| `/phone/gps` | NavSatFix | GPS location | +| `/phone/imu` | Imu | Accelerometer data | +| `/saltybot/speech_text` | String | Input from Jetson | +| `/saltybot/chat_response` | String | LLM output to Jetson | + +## Power Management + +```bash +nohup bash ~/power-management.sh > ~/power-mgmt.log 2>&1 & +``` + +Reduces CPU activity on low battery (<25%) or critical battery (<15%). + +## Configuration + +Edit `config/phone.yaml` to adjust frame rates and LLM parameters. + +## Issue #420 Completion + +✅ Termux bootstrap +✅ Camera / GPS / IMU nodes +✅ OpenClaw chat (local LLM) +✅ WebSocket bridge to Orin +✅ Termux:Boot auto-start +✅ Power management diff --git a/phone/power-management.sh b/phone/power-management.sh new file mode 100755 index 0000000..d0820f2 --- /dev/null +++ b/phone/power-management.sh @@ -0,0 +1,44 @@ +#!/bin/bash +# Power Management for SaltyBot Phone Node + +set -e + +BATTERY_CRITICAL=15 +BATTERY_LOW=25 +CURRENT_MODE="normal" + +log_msg() { + echo "[$(date '+%Y-%m-%d %H:%M:%S')] $1" +} + +get_battery() { + termux-battery-status 2>/dev/null | grep -o '"percentage":[0-9]*' | cut -d: -f2 || echo "50" +} + +monitor() { + BATTERY=$(get_battery) + log_msg "Battery: ${BATTERY}% Mode: $CURRENT_MODE" + + if [ "$BATTERY" -le "$BATTERY_CRITICAL" ]; then + if [ "$CURRENT_MODE" != "critical" ]; then + log_msg "Switching to CRITICAL mode" + CURRENT_MODE="critical" + fi + elif [ "$BATTERY" -le "$BATTERY_LOW" ]; then + if [ "$CURRENT_MODE" != "low_power" ]; then + log_msg "Switching to LOW POWER mode" + CURRENT_MODE="low_power" + fi + else + if [ "$CURRENT_MODE" != "normal" ]; then + log_msg "Switching to NORMAL mode" + CURRENT_MODE="normal" + fi + fi +} + +log_msg "Power management started" +while true; do + monitor + sleep 30 +done diff --git a/phone/termux-bootstrap.sh b/phone/termux-bootstrap.sh new file mode 100755 index 0000000..a03be33 --- /dev/null +++ b/phone/termux-bootstrap.sh @@ -0,0 +1,49 @@ +#!/bin/bash +# Termux Bootstrap for SaltyBot Phone Node + +set -e +echo "=== SaltyBot Phone Node Bootstrap ===" +apt update -y && apt upgrade -y +apt install -y python3 python3-pip git curl wget vim openssh termux-api + +pip install -q --upgrade pip setuptools wheel +pip install -q colcon-common-extensions rclpy sensor_msgs geometry_msgs cv_bridge pillow opencv-python ollama + +if [ ! -d "$HOME/.local/llama.cpp" ]; then + git clone https://github.com/ggerganov/llama.cpp $HOME/.local/llama.cpp + cd $HOME/.local/llama.cpp && make +fi + +mkdir -p ~/.cache/openclaw ~/saltybot_workspace + +mkdir -p ~/.ssh +if [ ! -f ~/.ssh/id_rsa ]; then + ssh-keygen -t rsa -b 4096 -f ~/.ssh/id_rsa -N "" +fi + +mkdir -p ~/.termux/boot +cat > ~/.termux/boot/saltybot-phone-start.sh << 'EOF' +#!/bin/bash +export PATH="$HOME/.local/bin:$PATH" +cd ~/saltybot_workspace +source install/setup.bash +ros2 launch saltybot_phone phone_bringup.py & +echo "SaltyBot phone node started at $(date)" >> ~/phone-node.log +EOF +chmod +x ~/.termux/boot/saltybot-phone-start.sh + +mkdir -p ~/.config/termux +cat > ~/.config/termux/power-monitor.sh << 'EOF' +#!/bin/bash +while true; do + BATTERY=$(termux-battery-status | grep -o 'percentage":[0-9]*' | cut -d: -f2) + if [ "$BATTERY" -lt "20" ]; then + echo "Battery low ($BATTERY%)" >> ~/power-monitor.log + fi + sleep 60 +done +EOF +chmod +x ~/.config/termux/power-monitor.sh + +echo "=== Bootstrap Complete ===" +echo "Next: Download saltybot_phone to ~/saltybot_workspace/src/ && colcon build"