Compare commits
No commits in common. "e76f0ab95f2780483b7f9c4265b487287818aa20" and "03999e7f91e47293c22e84f8254e9fd6c0210b4e" have entirely different histories.
e76f0ab95f
...
03999e7f91
@ -1,5 +0,0 @@
|
|||||||
wheel_slip_detector:
|
|
||||||
ros__parameters:
|
|
||||||
frequency: 10
|
|
||||||
slip_threshold: 0.1
|
|
||||||
slip_timeout: 0.5
|
|
||||||
@ -1,14 +0,0 @@
|
|||||||
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():
|
|
||||||
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")]),
|
|
||||||
])
|
|
||||||
@ -1,18 +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_wheel_slip_detector</name>
|
|
||||||
<version>0.1.0</version>
|
|
||||||
<description>Wheel slip detection by comparing commanded vs actual velocity.</description>
|
|
||||||
<maintainer email="seb@vayrette.com">Seb</maintainer>
|
|
||||||
<license>Apache-2.0</license>
|
|
||||||
<buildtool_depend>ament_python</buildtool_depend>
|
|
||||||
<depend>rclpy</depend>
|
|
||||||
<depend>geometry_msgs</depend>
|
|
||||||
<depend>std_msgs</depend>
|
|
||||||
<depend>nav_msgs</depend>
|
|
||||||
<test_depend>pytest</test_depend>
|
|
||||||
<export>
|
|
||||||
<build_type>ament_python</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,77 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
from typing import Optional
|
|
||||||
import math
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from rclpy.timer import Timer
|
|
||||||
from geometry_msgs.msg import Twist
|
|
||||||
from nav_msgs.msg import Odometry
|
|
||||||
from std_msgs.msg import Bool
|
|
||||||
|
|
||||||
class WheelSlipDetectorNode(Node):
|
|
||||||
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. Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s")
|
|
||||||
|
|
||||||
def _on_cmd_vel(self, msg: Twist) -> None:
|
|
||||||
self.cmd_vel = msg
|
|
||||||
|
|
||||||
def _on_odom(self, msg: Odometry) -> None:
|
|
||||||
self.actual_vel = msg.twist.twist
|
|
||||||
|
|
||||||
def _timer_callback(self) -> None:
|
|
||||||
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:
|
|
||||||
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()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
[develop]
|
|
||||||
script-dir=$base/lib/saltybot_wheel_slip_detector
|
|
||||||
[install]
|
|
||||||
install-scripts=$base/lib/saltybot_wheel_slip_detector
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
from setuptools import find_packages, setup
|
|
||||||
package_name = "saltybot_wheel_slip_detector"
|
|
||||||
setup(
|
|
||||||
name=package_name,
|
|
||||||
version="0.1.0",
|
|
||||||
packages=find_packages(exclude=["test"]),
|
|
||||||
data_files=[
|
|
||||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
|
||||||
("share/" + package_name, ["package.xml"]),
|
|
||||||
("share/" + package_name + "/launch", ["launch/wheel_slip_detector.launch.py"]),
|
|
||||||
("share/" + package_name + "/config", ["config/wheel_slip_config.yaml"]),
|
|
||||||
],
|
|
||||||
install_requires=["setuptools"],
|
|
||||||
zip_safe=True,
|
|
||||||
maintainer="Seb",
|
|
||||||
maintainer_email="seb@vayrette.com",
|
|
||||||
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"]},
|
|
||||||
)
|
|
||||||
@ -58,9 +58,6 @@ import JoystickTeleop from './components/JoystickTeleop.jsx';
|
|||||||
// Network diagnostics (issue #222)
|
// Network diagnostics (issue #222)
|
||||||
import { NetworkPanel } from './components/NetworkPanel.jsx';
|
import { NetworkPanel } from './components/NetworkPanel.jsx';
|
||||||
|
|
||||||
// Waypoint editor (issue #261)
|
|
||||||
import { WaypointEditor } from './components/WaypointEditor.jsx';
|
|
||||||
|
|
||||||
const TAB_GROUPS = [
|
const TAB_GROUPS = [
|
||||||
{
|
{
|
||||||
label: 'SOCIAL',
|
label: 'SOCIAL',
|
||||||
@ -88,13 +85,6 @@ const TAB_GROUPS = [
|
|||||||
{ id: 'cameras', label: 'Cameras', },
|
{ id: 'cameras', label: 'Cameras', },
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
{
|
|
||||||
label: 'NAVIGATION',
|
|
||||||
color: 'text-teal-600',
|
|
||||||
tabs: [
|
|
||||||
{ id: 'waypoints', label: 'Waypoints' },
|
|
||||||
],
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
label: 'FLEET',
|
label: 'FLEET',
|
||||||
color: 'text-green-600',
|
color: 'text-green-600',
|
||||||
@ -254,10 +244,8 @@ export default function App() {
|
|||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
)}
|
)}
|
||||||
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
|
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
|
||||||
{activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
|
{activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
|
||||||
|
|
||||||
{activeTab === 'waypoints' && <WaypointEditor subscribe={subscribe} publish={publishFn} callService={callService} />}
|
|
||||||
|
|
||||||
{activeTab === 'fleet' && <FleetPanel />}
|
{activeTab === 'fleet' && <FleetPanel />}
|
||||||
{activeTab === 'missions' && <MissionPlanner />}
|
{activeTab === 'missions' && <MissionPlanner />}
|
||||||
|
|||||||
@ -1,449 +0,0 @@
|
|||||||
/**
|
|
||||||
* WaypointEditor.jsx — Interactive waypoint navigation editor with click-to-place and drag-to-reorder
|
|
||||||
*
|
|
||||||
* Features:
|
|
||||||
* - Click on map canvas to place waypoints
|
|
||||||
* - Drag waypoints to reorder navigation sequence
|
|
||||||
* - Right-click to delete waypoints
|
|
||||||
* - Real-time waypoint list with labels and coordinates
|
|
||||||
* - Send Nav2 goal to /navigate_to_pose action
|
|
||||||
* - 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);
|
|
||||||
const [dragIndex, setDragIndex] = useState(null);
|
|
||||||
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 (for coordinate reference)
|
|
||||||
useEffect(() => {
|
|
||||||
const unsubMap = subscribe(
|
|
||||||
'/map',
|
|
||||||
'nav_msgs/OccupancyGrid',
|
|
||||||
(msg) => {
|
|
||||||
try {
|
|
||||||
const mapInfo = {
|
|
||||||
width: msg.info.width,
|
|
||||||
height: msg.info.height,
|
|
||||||
resolution: msg.info.resolution,
|
|
||||||
origin: msg.info.origin,
|
|
||||||
};
|
|
||||||
setMapData(mapInfo);
|
|
||||||
mapDataRef.current = mapInfo;
|
|
||||||
} catch (e) {
|
|
||||||
console.error('Error parsing map data:', e);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
return unsubMap;
|
|
||||||
}, [subscribe]);
|
|
||||||
|
|
||||||
// Subscribe to robot odometry (for current position reference)
|
|
||||||
useEffect(() => {
|
|
||||||
const unsubOdom = subscribe(
|
|
||||||
'/odom',
|
|
||||||
'nav_msgs/Odometry',
|
|
||||||
(msg) => {
|
|
||||||
try {
|
|
||||||
const pos = msg.pose.pose.position;
|
|
||||||
const ori = msg.pose.pose.orientation;
|
|
||||||
|
|
||||||
const siny_cosp = 2 * (ori.w * ori.z + ori.x * ori.y);
|
|
||||||
const cosy_cosp = 1 - 2 * (ori.y * ori.y + ori.z * ori.z);
|
|
||||||
const theta = Math.atan2(siny_cosp, cosy_cosp);
|
|
||||||
|
|
||||||
const newPose = { x: pos.x, y: pos.y, theta };
|
|
||||||
setRobotPose(newPose);
|
|
||||||
robotPoseRef.current = newPose;
|
|
||||||
} catch (e) {
|
|
||||||
console.error('Error parsing odometry data:', e);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
return unsubOdom;
|
|
||||||
}, [subscribe]);
|
|
||||||
|
|
||||||
// Canvas event handlers
|
|
||||||
const handleCanvasClick = (e) => {
|
|
||||||
if (!mapDataRef.current || !canvasRef.current) return;
|
|
||||||
|
|
||||||
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; // Would need to track zoom if map has zoom controls
|
|
||||||
|
|
||||||
// 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)),
|
|
||||||
y: parseFloat(worldY.toFixed(2)),
|
|
||||||
label: `WP-${waypoints.length + 1}`,
|
|
||||||
};
|
|
||||||
|
|
||||||
setWaypoints((prev) => [...prev, newWaypoint]);
|
|
||||||
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);
|
|
||||||
if (selectedWaypoint === id) setSelectedWaypoint(null);
|
|
||||||
};
|
|
||||||
|
|
||||||
const handleWaypointSelect = (id) => {
|
|
||||||
setSelectedWaypoint(selectedWaypoint === id ? null : id);
|
|
||||||
};
|
|
||||||
|
|
||||||
const handleWaypointDragStart = (e, index) => {
|
|
||||||
setIsDragging(true);
|
|
||||||
setDragIndex(index);
|
|
||||||
};
|
|
||||||
|
|
||||||
const handleWaypointDragOver = (e, targetIndex) => {
|
|
||||||
if (!isDragging || dragIndex === null || dragIndex === targetIndex) return;
|
|
||||||
|
|
||||||
const newWaypoints = [...waypoints];
|
|
||||||
const draggedWaypoint = newWaypoints[dragIndex];
|
|
||||||
newWaypoints.splice(dragIndex, 1);
|
|
||||||
newWaypoints.splice(targetIndex, 0, draggedWaypoint);
|
|
||||||
|
|
||||||
setWaypoints(newWaypoints);
|
|
||||||
waypointsRef.current = newWaypoints;
|
|
||||||
setDragIndex(targetIndex);
|
|
||||||
};
|
|
||||||
|
|
||||||
const handleWaypointDragEnd = () => {
|
|
||||||
setIsDragging(false);
|
|
||||||
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: {
|
|
||||||
position: {
|
|
||||||
x: waypoint.x,
|
|
||||||
y: waypoint.y,
|
|
||||||
z: 0,
|
|
||||||
},
|
|
||||||
orientation: {
|
|
||||||
x: qx,
|
|
||||||
y: qy,
|
|
||||||
z: qz,
|
|
||||||
w: qw,
|
|
||||||
},
|
|
||||||
},
|
|
||||||
};
|
|
||||||
|
|
||||||
// Send to Nav2 navigate_to_pose action
|
|
||||||
await callService(
|
|
||||||
'/navigate_to_pose',
|
|
||||||
'nav2_msgs/NavigateToPose',
|
|
||||||
{ pose: goal.pose }
|
|
||||||
);
|
|
||||||
|
|
||||||
setActiveWaypoint(waypoint.id);
|
|
||||||
return true;
|
|
||||||
} catch (e) {
|
|
||||||
console.error('Error sending nav goal:', e);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
const executeWaypoints = async () => {
|
|
||||||
if (waypoints.length === 0) return;
|
|
||||||
|
|
||||||
setExecuting(true);
|
|
||||||
for (const waypoint of waypoints) {
|
|
||||||
const success = await sendNavGoal(waypoint);
|
|
||||||
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);
|
|
||||||
setActiveWaypoint(null);
|
|
||||||
};
|
|
||||||
|
|
||||||
const clearWaypoints = () => {
|
|
||||||
setWaypoints([]);
|
|
||||||
waypointsRef.current = [];
|
|
||||||
setSelectedWaypoint(null);
|
|
||||||
setActiveWaypoint(null);
|
|
||||||
};
|
|
||||||
|
|
||||||
const sendSingleGoal = async () => {
|
|
||||||
if (selectedWaypoint === null) return;
|
|
||||||
|
|
||||||
const wp = waypoints.find((w) => w.id === selectedWaypoint);
|
|
||||||
if (wp) {
|
|
||||||
await sendNavGoal(wp);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
return (
|
|
||||||
<div className="flex h-full gap-3">
|
|
||||||
{/* Map area with click handlers */}
|
|
||||||
<div className="flex-1 flex flex-col space-y-3">
|
|
||||||
<div className="flex-1 bg-gray-900 rounded-lg border border-cyan-950 overflow-hidden relative cursor-crosshair">
|
|
||||||
<div
|
|
||||||
ref={containerRef}
|
|
||||||
className="w-full h-full"
|
|
||||||
onClick={handleCanvasClick}
|
|
||||||
onContextMenu={handleCanvasContextMenu}
|
|
||||||
>
|
|
||||||
{/* Virtual map display - waypoints overlaid */}
|
|
||||||
<svg
|
|
||||||
className="absolute inset-0 w-full h-full pointer-events-none"
|
|
||||||
id="waypoint-overlay"
|
|
||||||
>
|
|
||||||
{/* Waypoint markers */}
|
|
||||||
{waypoints.map((wp, idx) => {
|
|
||||||
if (!mapDataRef.current) return null;
|
|
||||||
|
|
||||||
const robot = robotPoseRef.current;
|
|
||||||
const zoom = 1;
|
|
||||||
const centerX = containerRef.current?.clientWidth / 2 || 400;
|
|
||||||
const centerY = containerRef.current?.clientHeight / 2 || 300;
|
|
||||||
|
|
||||||
const canvasX = centerX + (wp.x - robot.x) * zoom;
|
|
||||||
const canvasY = centerY - (wp.y - robot.y) * zoom;
|
|
||||||
|
|
||||||
const isActive = wp.id === activeWaypoint;
|
|
||||||
const isSelected = wp.id === selectedWaypoint;
|
|
||||||
|
|
||||||
return (
|
|
||||||
<g key={wp.id}>
|
|
||||||
{/* Waypoint circle */}
|
|
||||||
<circle
|
|
||||||
cx={canvasX}
|
|
||||||
cy={canvasY}
|
|
||||||
r="10"
|
|
||||||
fill={isActive ? '#ef4444' : isSelected ? '#fbbf24' : '#06b6d4'}
|
|
||||||
opacity="0.8"
|
|
||||||
/>
|
|
||||||
{/* Waypoint number */}
|
|
||||||
<text
|
|
||||||
x={canvasX}
|
|
||||||
y={canvasY}
|
|
||||||
textAnchor="middle"
|
|
||||||
dominantBaseline="middle"
|
|
||||||
fill="white"
|
|
||||||
fontSize="10"
|
|
||||||
fontWeight="bold"
|
|
||||||
pointerEvents="none"
|
|
||||||
>
|
|
||||||
{idx + 1}
|
|
||||||
</text>
|
|
||||||
{/* Line to next waypoint */}
|
|
||||||
{idx < waypoints.length - 1 && (
|
|
||||||
<line
|
|
||||||
x1={canvasX}
|
|
||||||
y1={canvasY}
|
|
||||||
x2={
|
|
||||||
centerX +
|
|
||||||
(waypoints[idx + 1].x - robot.x) * zoom
|
|
||||||
}
|
|
||||||
y2={
|
|
||||||
centerY -
|
|
||||||
(waypoints[idx + 1].y - robot.y) * zoom
|
|
||||||
}
|
|
||||||
stroke="#10b981"
|
|
||||||
strokeWidth="2"
|
|
||||||
opacity="0.6"
|
|
||||||
/>
|
|
||||||
)}
|
|
||||||
</g>
|
|
||||||
);
|
|
||||||
})}
|
|
||||||
|
|
||||||
{/* Robot position marker */}
|
|
||||||
<circle
|
|
||||||
cx={containerRef.current?.clientWidth / 2 || 400}
|
|
||||||
cy={containerRef.current?.clientHeight / 2 || 300}
|
|
||||||
r="8"
|
|
||||||
fill="#8b5cf6"
|
|
||||||
opacity="1"
|
|
||||||
/>
|
|
||||||
</svg>
|
|
||||||
|
|
||||||
<div className="absolute inset-0 flex items-center justify-center pointer-events-none text-gray-600 text-sm">
|
|
||||||
{waypoints.length === 0 && (
|
|
||||||
<div className="text-center">
|
|
||||||
<div>Click to place waypoints</div>
|
|
||||||
<div className="text-xs text-gray-700">Right-click to delete</div>
|
|
||||||
</div>
|
|
||||||
)}
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Info panel */}
|
|
||||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 text-xs text-gray-600 space-y-1">
|
|
||||||
<div className="flex justify-between">
|
|
||||||
<span>Waypoints:</span>
|
|
||||||
<span className="text-cyan-400">{waypoints.length}</span>
|
|
||||||
</div>
|
|
||||||
<div className="flex justify-between">
|
|
||||||
<span>Robot Position:</span>
|
|
||||||
<span className="text-cyan-400">
|
|
||||||
({robotPose.x.toFixed(2)}, {robotPose.y.toFixed(2)})
|
|
||||||
</span>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Waypoint list sidebar */}
|
|
||||||
<div className="w-64 flex flex-col bg-gray-950 rounded-lg border border-cyan-950 space-y-3 p-3">
|
|
||||||
<div className="flex items-center justify-between">
|
|
||||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">WAYPOINTS</div>
|
|
||||||
<div className="text-gray-600 text-xs">{waypoints.length}</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Waypoint list */}
|
|
||||||
<div className="flex-1 overflow-y-auto space-y-1">
|
|
||||||
{waypoints.length === 0 ? (
|
|
||||||
<div className="text-center text-gray-700 text-xs py-4">
|
|
||||||
Click map to add waypoints
|
|
||||||
</div>
|
|
||||||
) : (
|
|
||||||
waypoints.map((wp, idx) => (
|
|
||||||
<div
|
|
||||||
key={wp.id}
|
|
||||||
draggable
|
|
||||||
onDragStart={(e) => handleWaypointDragStart(e, idx)}
|
|
||||||
onDragOver={(e) => {
|
|
||||||
e.preventDefault();
|
|
||||||
handleWaypointDragOver(e, idx);
|
|
||||||
}}
|
|
||||||
onDragEnd={handleWaypointDragEnd}
|
|
||||||
onClick={() => handleWaypointSelect(wp.id)}
|
|
||||||
onContextMenu={(e) => {
|
|
||||||
e.preventDefault();
|
|
||||||
handleDeleteWaypoint(wp.id);
|
|
||||||
}}
|
|
||||||
className={`p-2 rounded border text-xs cursor-move transition-colors ${
|
|
||||||
wp.id === activeWaypoint
|
|
||||||
? 'bg-red-950 border-red-700 text-red-300'
|
|
||||||
: wp.id === selectedWaypoint
|
|
||||||
? 'bg-amber-950 border-amber-700 text-amber-300'
|
|
||||||
: 'bg-gray-900 border-gray-700 text-gray-400 hover:border-gray-600'
|
|
||||||
}`}
|
|
||||||
>
|
|
||||||
<div className="flex justify-between items-start gap-2">
|
|
||||||
<div className="font-bold">#{idx + 1}</div>
|
|
||||||
<div className="text-right flex-1">
|
|
||||||
<div className="text-gray-500">{wp.label}</div>
|
|
||||||
<div className="text-gray-600">
|
|
||||||
{wp.x.toFixed(2)}, {wp.y.toFixed(2)}
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
))
|
|
||||||
)}
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Control buttons */}
|
|
||||||
<div className="space-y-2 border-t border-gray-800 pt-3">
|
|
||||||
<button
|
|
||||||
onClick={sendSingleGoal}
|
|
||||||
disabled={selectedWaypoint === null || executing}
|
|
||||||
className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-cyan-800 bg-cyan-950 text-cyan-400 hover:bg-cyan-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
|
|
||||||
>
|
|
||||||
SEND GOAL
|
|
||||||
</button>
|
|
||||||
|
|
||||||
<button
|
|
||||||
onClick={executeWaypoints}
|
|
||||||
disabled={waypoints.length === 0 || executing}
|
|
||||||
className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-green-800 bg-green-950 text-green-400 hover:bg-green-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
|
|
||||||
>
|
|
||||||
{executing ? 'EXECUTING...' : 'EXECUTE ALL'}
|
|
||||||
</button>
|
|
||||||
|
|
||||||
<button
|
|
||||||
onClick={clearWaypoints}
|
|
||||||
disabled={waypoints.length === 0}
|
|
||||||
className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-red-800 bg-red-950 text-red-400 hover:bg-red-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
|
|
||||||
>
|
|
||||||
CLEAR ALL
|
|
||||||
</button>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Instructions */}
|
|
||||||
<div className="text-xs text-gray-600 space-y-1 border-t border-gray-800 pt-3">
|
|
||||||
<div className="font-bold text-gray-500">CONTROLS:</div>
|
|
||||||
<div>• Click: Place waypoint</div>
|
|
||||||
<div>• Right-click: Delete waypoint</div>
|
|
||||||
<div>• Drag: Reorder waypoints</div>
|
|
||||||
<div>• Click list: Select waypoint</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{/* Topic info */}
|
|
||||||
<div className="text-xs text-gray-600 border-t border-gray-800 pt-3">
|
|
||||||
<div className="flex justify-between">
|
|
||||||
<span>Service:</span>
|
|
||||||
<span className="text-gray-500">/navigate_to_pose</span>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
export { WaypointEditor };
|
|
||||||
Loading…
x
Reference in New Issue
Block a user