diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py index 25436cd..6c383d3 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py @@ -5,21 +5,10 @@ from launch.actions import DeclareLaunchArgument import os from ament_index_python.packages import get_package_share_directory - def generate_launch_description(): pkg_dir = get_package_share_directory("saltybot_wheel_slip_detector") config_file = os.path.join(pkg_dir, "config", "wheel_slip_config.yaml") return LaunchDescription([ - DeclareLaunchArgument( - "config_file", - default_value=config_file, - description="Path to configuration YAML file", - ), - Node( - package="saltybot_wheel_slip_detector", - executable="wheel_slip_detector_node", - name="wheel_slip_detector", - output="screen", - parameters=[LaunchConfiguration("config_file")], - ), + DeclareLaunchArgument("config_file", default_value=config_file, description="Path to configuration YAML file"), + Node(package="saltybot_wheel_slip_detector", executable="wheel_slip_detector_node", name="wheel_slip_detector", output="screen", parameters=[LaunchConfiguration("config_file")]), ]) diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml new file mode 100644 index 0000000..8dc2903 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml @@ -0,0 +1,18 @@ + + + + saltybot_wheel_slip_detector + 0.1.0 + Wheel slip detection by comparing commanded vs actual velocity. + Seb + Apache-2.0 + ament_python + rclpy + geometry_msgs + std_msgs + nav_msgs + pytest + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/resource/saltybot_wheel_slip_detector b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/resource/saltybot_wheel_slip_detector new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__init__.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py index 856a758..a967d7b 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py @@ -1,13 +1,6 @@ #!/usr/bin/env python3 -"""Wheel slip detector for SaltyBot. - -Detects wheel slip by comparing commanded velocity vs actual encoder velocity. -Publishes Bool when slip is detected for >0.5s, enabling speed reduction response. -""" - from typing import Optional import math - import rclpy from rclpy.node import Node from rclpy.timer import Timer @@ -15,82 +8,60 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from std_msgs.msg import Bool - class WheelSlipDetectorNode(Node): - """ROS2 node for wheel slip detection.""" - def __init__(self): super().__init__("wheel_slip_detector") - self.declare_parameter("frequency", 10) frequency = self.get_parameter("frequency").value self.declare_parameter("slip_threshold", 0.1) self.declare_parameter("slip_timeout", 0.5) - self.slip_threshold = self.get_parameter("slip_threshold").value self.slip_timeout = self.get_parameter("slip_timeout").value self.period = 1.0 / frequency - self.cmd_vel: Optional[Twist] = None self.actual_vel: Optional[Twist] = None self.slip_duration = 0.0 self.slip_detected = False - self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) self.create_subscription(Odometry, "/odom", self._on_odom, 10) self.pub_slip = self.create_publisher(Bool, "/saltybot/wheel_slip_detected", 10) self.timer: Timer = self.create_timer(self.period, self._timer_callback) - - self.get_logger().info( - f"Wheel slip detector initialized at {frequency}Hz. " - f"Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s" - ) + self.get_logger().info(f"Wheel slip detector initialized at {frequency}Hz. Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s") def _on_cmd_vel(self, msg: Twist) -> None: - """Update commanded velocity from subscription.""" self.cmd_vel = msg def _on_odom(self, msg: Odometry) -> None: - """Update actual velocity from odometry subscription.""" self.actual_vel = msg.twist.twist def _timer_callback(self) -> None: - """Detect wheel slip and publish detection flag.""" if self.cmd_vel is None or self.actual_vel is None: slip_detected = False else: slip_detected = self._check_slip() - if slip_detected: self.slip_duration += self.period else: self.slip_duration = 0.0 - is_slip = self.slip_duration > self.slip_timeout - if is_slip != self.slip_detected: self.slip_detected = is_slip if self.slip_detected: self.get_logger().warn(f"WHEEL SLIP DETECTED: {self.slip_duration:.2f}s") else: self.get_logger().info("Wheel slip cleared") - slip_msg = Bool() slip_msg.data = is_slip self.pub_slip.publish(slip_msg) def _check_slip(self) -> bool: - """Check if velocity difference indicates slip.""" cmd_speed = math.sqrt(self.cmd_vel.linear.x**2 + self.cmd_vel.linear.y**2) actual_speed = math.sqrt(self.actual_vel.linear.x**2 + self.actual_vel.linear.y**2) vel_diff = abs(cmd_speed - actual_speed) - if cmd_speed < 0.05 and actual_speed < 0.05: return False - return vel_diff > self.slip_threshold - def main(args=None): rclpy.init(args=args) node = WheelSlipDetectorNode() @@ -102,6 +73,5 @@ def main(args=None): node.destroy_node() rclpy.shutdown() - if __name__ == "__main__": main() diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg new file mode 100644 index 0000000..d479f34 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/saltybot_wheel_slip_detector +[install] +install-scripts=$base/lib/saltybot_wheel_slip_detector diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py index 19fd5d5..2ede727 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py @@ -1,7 +1,5 @@ from setuptools import find_packages, setup - package_name = "saltybot_wheel_slip_detector" - setup( name=package_name, version="0.1.0", @@ -19,9 +17,5 @@ setup( description="Wheel slip detection from velocity command/actual mismatch", license="Apache-2.0", tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main", - ], - }, -) + entry_points={"console_scripts": ["wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main"]}, +) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__init__.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index cdf0f5f..d64246b 100644 --- a/ui/social-bot/src/App.jsx +++ b/ui/social-bot/src/App.jsx @@ -61,9 +61,6 @@ import { NetworkPanel } from './components/NetworkPanel.jsx'; // Waypoint editor (issue #261) import { WaypointEditor } from './components/WaypointEditor.jsx'; -// Status header (issue #269) -import { StatusHeader } from './components/StatusHeader.jsx'; - const TAB_GROUPS = [ { label: 'SOCIAL', diff --git a/ui/social-bot/src/components/WaypointEditor.jsx b/ui/social-bot/src/components/WaypointEditor.jsx index ffbf571..3069d68 100644 --- a/ui/social-bot/src/components/WaypointEditor.jsx +++ b/ui/social-bot/src/components/WaypointEditor.jsx @@ -10,11 +10,13 @@ * - Execute waypoint sequence with automatic progression * - Clear all waypoints button * - Visual feedback for active waypoint (executing) + * - Imports map display from MapViewer for coordinate system */ import { useEffect, useRef, useState } from 'react'; function WaypointEditor({ subscribe, publish, callService }) { + // Waypoint storage const [waypoints, setWaypoints] = useState([]); const [selectedWaypoint, setSelectedWaypoint] = useState(null); const [isDragging, setIsDragging] = useState(false); @@ -22,16 +24,20 @@ function WaypointEditor({ subscribe, publish, callService }) { const [activeWaypoint, setActiveWaypoint] = useState(null); const [executing, setExecuting] = useState(false); + // Map context const [mapData, setMapData] = useState(null); const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 }); + // Canvas reference + const canvasRef = useRef(null); const containerRef = useRef(null); + // Refs for ROS integration const mapDataRef = useRef(null); const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 }); const waypointsRef = useRef([]); - // Subscribe to map data + // Subscribe to map data (for coordinate reference) useEffect(() => { const unsubMap = subscribe( '/map', @@ -54,7 +60,7 @@ function WaypointEditor({ subscribe, publish, callService }) { return unsubMap; }, [subscribe]); - // Subscribe to robot odometry + // Subscribe to robot odometry (for current position reference) useEffect(() => { const unsubOdom = subscribe( '/odom', @@ -79,22 +85,29 @@ function WaypointEditor({ subscribe, publish, callService }) { return unsubOdom; }, [subscribe]); + // Canvas event handlers const handleCanvasClick = (e) => { - if (!mapDataRef.current || !containerRef.current) return; + if (!mapDataRef.current || !canvasRef.current) return; - const rect = containerRef.current.getBoundingClientRect(); + const canvas = canvasRef.current; + const rect = canvas.getBoundingClientRect(); const clickX = e.clientX - rect.left; const clickY = e.clientY - rect.top; + // Convert canvas coordinates to world coordinates + // This assumes the map is centered on the robot + const map = mapDataRef.current; const robot = robotPoseRef.current; - const zoom = 1; + const zoom = 1; // Would need to track zoom if map has zoom controls - const centerX = containerRef.current.clientWidth / 2; - const centerY = containerRef.current.clientHeight / 2; + // Inverse of map rendering calculation + const centerX = canvas.width / 2; + const centerY = canvas.height / 2; const worldX = robot.x + (clickX - centerX) / zoom; const worldY = robot.y - (clickY - centerY) / zoom; + // Create new waypoint const newWaypoint = { id: Date.now(), x: parseFloat(worldX.toFixed(2)), @@ -106,6 +119,12 @@ function WaypointEditor({ subscribe, publish, callService }) { waypointsRef.current = [...waypointsRef.current, newWaypoint]; }; + const handleCanvasContextMenu = (e) => { + e.preventDefault(); + // Right-click handled by waypoint list + }; + + // Waypoint list handlers const handleDeleteWaypoint = (id) => { setWaypoints((prev) => prev.filter((wp) => wp.id !== id)); waypointsRef.current = waypointsRef.current.filter((wp) => wp.id !== id); @@ -139,12 +158,18 @@ function WaypointEditor({ subscribe, publish, callService }) { setDragIndex(null); }; + // Execute waypoints const sendNavGoal = async (waypoint) => { if (!callService) return; try { + // Create quaternion from heading (default to 0 if no heading) const heading = waypoint.theta || 0; const halfHeading = heading / 2; + const qx = 0; + const qy = 0; + const qz = Math.sin(halfHeading); + const qw = Math.cos(halfHeading); const goal = { pose: { @@ -154,14 +179,15 @@ function WaypointEditor({ subscribe, publish, callService }) { z: 0, }, orientation: { - x: 0, - y: 0, - z: Math.sin(halfHeading), - w: Math.cos(halfHeading), + x: qx, + y: qy, + z: qz, + w: qw, }, }, }; + // Send to Nav2 navigate_to_pose action await callService( '/navigate_to_pose', 'nav2_msgs/NavigateToPose', @@ -182,7 +208,11 @@ function WaypointEditor({ subscribe, publish, callService }) { setExecuting(true); for (const waypoint of waypoints) { const success = await sendNavGoal(waypoint); - if (!success) break; + if (!success) { + console.error('Failed to send goal for waypoint:', waypoint); + break; + } + // Wait a bit before sending next goal await new Promise((resolve) => setTimeout(resolve, 500)); } setExecuting(false); @@ -207,16 +237,21 @@ function WaypointEditor({ subscribe, publish, callService }) { return (
- {/* Map area */} + {/* Map area with click handlers */}
e.preventDefault()} + onContextMenu={handleCanvasContextMenu} > - + {/* Virtual map display - waypoints overlaid */} + + {/* Waypoint markers */} {waypoints.map((wp, idx) => { if (!mapDataRef.current) return null; @@ -233,6 +268,7 @@ function WaypointEditor({ subscribe, publish, callService }) { return ( + {/* Waypoint circle */} + {/* Waypoint number */} {idx + 1} + {/* Line to next waypoint */} {idx < waypoints.length - 1 && ( ); })} + + {/* Robot position marker */} {waypoints.length === 0 ? ( -
Click map to add waypoints
+
+ Click map to add waypoints +
) : ( waypoints.map((wp, idx) => (