Compare commits
2 Commits
479a33a6fa
...
a8e1ea3314
| Author | SHA1 | Date | |
|---|---|---|---|
| a8e1ea3314 | |||
| a9e3082e57 |
354
chassis/cable_management_clips.scad
Normal file
354
chassis/cable_management_clips.scad
Normal file
@ -0,0 +1,354 @@
|
||||
// =============================================================================
|
||||
// SaltyBot — Cable Management Clips
|
||||
// Agent: sl-mechanical | 2026-03-02
|
||||
//
|
||||
// MODULAR SNAP-ON CABLE CLIPS with integrated adhesive base and zip-tie anchors.
|
||||
// Designed to organize power cables, sensor bundles, and signal harnesses on the
|
||||
// chassis. Each clip accommodates a range of cable bundle diameters via elastic
|
||||
// snap jaws.
|
||||
//
|
||||
// HOW IT WORKS
|
||||
// 1. Adhesive base (3M VHB or equivalent) adheres to chassis surface.
|
||||
// 2. Cable bundle pressed upward through snap jaws until it seats with audible click.
|
||||
// 3. Overhanging jaw tabs provide two zip-tie anchor points (one per side).
|
||||
// 4. Vertical ear holes accept M3 threaded inserts for wire or strap attachment.
|
||||
//
|
||||
// CLIP FAMILY
|
||||
// • Clip 5mm: Holds 4–6 mm bundles (small signal cables)
|
||||
// • Clip 8mm: Holds 6–10 mm bundles (mixed power + signal)
|
||||
// • Clip 12mm: Holds 10–14 mm bundles (heavy power)
|
||||
//
|
||||
// PARTS (set RENDER= to export each)
|
||||
// clip_5mm — 3D print × N (RENDER="clip_5mm")
|
||||
// clip_8mm — 3D print × N (RENDER="clip_8mm")
|
||||
// clip_12mm — 3D print × N (RENDER="clip_12mm")
|
||||
// assembly_all — Full preview with ghosts (RENDER="assembly_all")
|
||||
//
|
||||
// MATERIALS
|
||||
// • Body: PETG or ASA (weatherproof, adhesive-friendly)
|
||||
// • Adhesive: 3M VHB 5952F (50 mm × 75 mm pads, rated 5 N/cm²)
|
||||
// • Anchors: M3 threaded inserts (optional, for high-load retention)
|
||||
// • Zip-ties: Standard nylon 3.6 mm × 150 mm (e.g., HellermannTyton)
|
||||
//
|
||||
// INSTALLATION
|
||||
// 1. Clean chassis surface with isopropyl alcohol; let dry.
|
||||
// 2. Peel 3M VHB backing; press clip firmly (30 s hold).
|
||||
// 3. Wait 24 hours for full adhesive cure.
|
||||
// 4. Press cable bundle upward through snap jaws until seated.
|
||||
// 5. Route zip-ties through jaw anchor points; cinch at desired tension.
|
||||
// 6. Optionally thread M3 bolts through ear holes for redundant retention.
|
||||
// =============================================================================
|
||||
|
||||
$fn = 64;
|
||||
|
||||
// =============================================================================
|
||||
// CLIP GEOMETRY — COMMON PARAMETERS
|
||||
// =============================================================================
|
||||
|
||||
// SNAP JAW PROFILE (all clips share same jaw geometry)
|
||||
JAW_THICKNESS = 2.0; // mm thickness of snap arms (thin = flexible)
|
||||
JAW_BEND_R = 0.8; // mm radius at jaw root (stress relief)
|
||||
JAW_CLOSURE = 0.3; // mm interference fit depth when snapped closed
|
||||
SNAP_TRAVEL = 1.2; // mm vertical distance cable travels before seat
|
||||
SNAP_REST_GAP = 0.2; // mm gap when unloaded (keeps jaws sprung apart)
|
||||
|
||||
// ADHESIVE BASE
|
||||
BASE_LENGTH = 50.0; // mm forward-back footprint
|
||||
BASE_WIDTH = 40.0; // mm left-right footprint
|
||||
BASE_THICKNESS = 2.5; // mm base pad thickness
|
||||
BASE_FILLET = 2.0; // mm corner rounding (aids adhesive contact)
|
||||
|
||||
// ZIP-TIE ANCHOR FEATURES
|
||||
ANCHOR_TAB_H = 5.0; // mm height of jaw-tip anchor tab
|
||||
ANCHOR_TAB_T = 1.5; // mm thickness of anchor tab
|
||||
ANCHOR_SLOT_W = 4.0; // mm width of zip-tie slot (3.6 mm ties + 0.4 mm clearance)
|
||||
ANCHOR_SLOT_H = 1.0; // mm height of slot throat
|
||||
|
||||
// WIRE/STRAP ATTACHMENT EARS
|
||||
EAR_D = 4.2; // mm hole diameter (M3 clearance, 2.6 mm nominal)
|
||||
EAR_WALL_T = 3.0; // mm wall thickness around hole
|
||||
EAR_H = 8.0; // mm ear protrusion height from base
|
||||
|
||||
// =============================================================================
|
||||
// CLIP SIZE VARIANTS
|
||||
// =============================================================================
|
||||
|
||||
// For each clip size, define:
|
||||
// CABLE_D — nominal cable bundle diameter
|
||||
// JAW_SPAN — inner span of closed jaws (CABLE_D + JAW_CLOSURE)
|
||||
// CLIP_HEIGHT — overall height of clip body
|
||||
// CLAMP_X — width of clamp section (controls jaw lever arm)
|
||||
|
||||
CLIP_PARAMS = [
|
||||
// [name, cable_d, jaw_span, height, clamp_x]
|
||||
["5mm", 5.0, 5.3, 28.0, 18.0],
|
||||
["8mm", 8.0, 8.3, 35.0, 22.0],
|
||||
["12mm", 12.0, 12.3, 42.0, 26.0],
|
||||
];
|
||||
|
||||
// =============================================================================
|
||||
// RENDER CONTROL
|
||||
// =============================================================================
|
||||
|
||||
// "assembly_all" — all clips in array, with base ghosts
|
||||
// "clip_5mm" — single 5mm clip (ready to export STL)
|
||||
// "clip_8mm" — single 8mm clip
|
||||
// "clip_12mm" — single 12mm clip
|
||||
|
||||
RENDER = "assembly_all";
|
||||
|
||||
// Helper to fetch clip parameters by name
|
||||
function get_clip_params(name) =
|
||||
(name == "5mm") ? CLIP_PARAMS[0] :
|
||||
(name == "8mm") ? CLIP_PARAMS[1] :
|
||||
(name == "12mm") ? CLIP_PARAMS[2] :
|
||||
CLIP_PARAMS[0];
|
||||
|
||||
// =============================================================================
|
||||
// MAIN RENDER DISPATCH
|
||||
// =============================================================================
|
||||
|
||||
if (RENDER == "assembly_all") {
|
||||
assembly_all();
|
||||
} else if (RENDER == "clip_5mm") {
|
||||
clip_body(get_clip_params("5mm"));
|
||||
} else if (RENDER == "clip_8mm") {
|
||||
clip_body(get_clip_params("8mm"));
|
||||
} else if (RENDER == "clip_12mm") {
|
||||
clip_body(get_clip_params("12mm"));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// ASSEMBLY VIEW (all clips in a row, with adhesive pads ghosted)
|
||||
// =============================================================================
|
||||
|
||||
module assembly_all() {
|
||||
for (i = [0 : len(CLIP_PARAMS) - 1]) {
|
||||
p = CLIP_PARAMS[i];
|
||||
x_offset = i * 70; // 70 mm spacing
|
||||
|
||||
translate([x_offset, 0, 0]) {
|
||||
// Clip body
|
||||
color("DodgerBlue", 0.92)
|
||||
clip_body(p);
|
||||
|
||||
// Adhesive base ghost (3M VHB pad)
|
||||
%color("LimeGreen", 0.40)
|
||||
translate([0, 0, -BASE_THICKNESS])
|
||||
rounded_rect([50, 40, 0.2], BASE_FILLET);
|
||||
|
||||
// Label
|
||||
echo(str("Clip ", p[0], " — Cable dia. ", p[1], " mm"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// CLIP BODY MODULE (parametric across all sizes)
|
||||
// =============================================================================
|
||||
//
|
||||
// Structure:
|
||||
// • Base: rounded rectangle, adhesive-mounting surface
|
||||
// • Spine: central vertical structure, extends from base
|
||||
// • Jaws: two snap arms extending upward/outward from spine
|
||||
// • Ears: two lateral holes for M3 attachment (optional)
|
||||
// • Anchors: small tabs on jaw tips for zip-tie routing
|
||||
//
|
||||
|
||||
module clip_body(params) {
|
||||
name = params[0];
|
||||
cable_d = params[1];
|
||||
jaw_span = params[2];
|
||||
clip_h = params[3];
|
||||
clamp_x = params[4];
|
||||
|
||||
spine_thick = 3.5; // mm thickness of central spine
|
||||
jaw_l = clip_h - BASE_THICKNESS; // jaw arm length
|
||||
jaw_root_x = clamp_x / 2; // X position where jaw originates from spine
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── ADHESIVE BASE ──────────────────────────────────────────────
|
||||
translate([0, 0, -BASE_THICKNESS/2])
|
||||
rounded_rect([BASE_LENGTH, BASE_WIDTH, BASE_THICKNESS], BASE_FILLET);
|
||||
|
||||
// ── CENTRAL SPINE (support structure) ───────────────────────────
|
||||
translate([-spine_thick/2, -clamp_x/2, 0])
|
||||
cube([spine_thick, clamp_x, BASE_THICKNESS + jaw_l]);
|
||||
|
||||
// ── LEFT JAW (snap arm with flexible root) ──────────────────────
|
||||
jaw_body(-jaw_root_x, jaw_l, jaw_span);
|
||||
|
||||
// ── RIGHT JAW (snap arm, mirror) ───────────────────────────────
|
||||
jaw_body(jaw_root_x, jaw_l, jaw_span);
|
||||
|
||||
// ── LEFT EAR (M3 attachment hole) ──────────────────────────────
|
||||
translate([-clamp_x/2 - EAR_WALL_T - EAR_D/2, 0, BASE_THICKNESS])
|
||||
ear_boss();
|
||||
|
||||
// ── RIGHT EAR ──────────────────────────────────────────────────
|
||||
translate([clamp_x/2 + EAR_WALL_T + EAR_D/2, 0, BASE_THICKNESS])
|
||||
ear_boss();
|
||||
}
|
||||
|
||||
// ── SUBTRACT: Anchor slot hollows (zip-tie slots in jaw tips) ──────
|
||||
jaw_root_z = BASE_THICKNESS + jaw_l - ANCHOR_TAB_H;
|
||||
|
||||
// Left jaw anchor slot
|
||||
translate([-jaw_root_x - 2, -ANCHOR_SLOT_W/2, jaw_root_z])
|
||||
cube([3, ANCHOR_SLOT_W, ANCHOR_SLOT_H]);
|
||||
|
||||
// Right jaw anchor slot
|
||||
translate([jaw_root_x - 1, -ANCHOR_SLOT_W/2, jaw_root_z])
|
||||
cube([3, ANCHOR_SLOT_W, ANCHOR_SLOT_H]);
|
||||
|
||||
// ── SUBTRACT: Ear attachment holes (M3 clearance) ────────────────
|
||||
// Left ear hole
|
||||
translate([-clamp_x/2 - EAR_WALL_T - EAR_D/2, 0, BASE_THICKNESS + EAR_H/2])
|
||||
cylinder(d=EAR_D, h=EAR_H + 1, center=true);
|
||||
|
||||
// Right ear hole
|
||||
translate([clamp_x/2 + EAR_WALL_T + EAR_D/2, 0, BASE_THICKNESS + EAR_H/2])
|
||||
cylinder(d=EAR_D, h=EAR_H + 1, center=true);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// JAW BODY (single snap arm with cable pocket)
|
||||
// =============================================================================
|
||||
//
|
||||
// A flexible cantilever arm extending from the spine.
|
||||
// Lower section: solid (load-bearing).
|
||||
// Upper section: curved U-channel (grips cable).
|
||||
// Jaw tips: overhanging tabs for zip-tie anchors.
|
||||
//
|
||||
|
||||
module jaw_body(x_root, jaw_length, inner_span) {
|
||||
jaw_span_outer = inner_span + 2 * JAW_THICKNESS;
|
||||
|
||||
// The jaw sweeps from x_root (spine side) along +X, curving to grip.
|
||||
// At the tip, it has a slight outward bow for snap action.
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// Lower jaw arm (solid, structural)
|
||||
translate([x_root, -jaw_span_outer/2, BASE_THICKNESS])
|
||||
cube([jaw_length * 0.65, jaw_span_outer, JAW_THICKNESS * 1.5]);
|
||||
|
||||
// Upper jaw arm (U-channel form)
|
||||
translate([x_root, -inner_span/2 - JAW_THICKNESS, BASE_THICKNESS])
|
||||
cube([jaw_length * 0.85, inner_span + 2*JAW_THICKNESS, JAW_THICKNESS]);
|
||||
|
||||
// Jaw tip anchor tab (for zip-tie slots)
|
||||
tip_x = x_root + jaw_length * 0.8;
|
||||
translate([tip_x, -jaw_span_outer/2 - ANCHOR_TAB_T,
|
||||
BASE_THICKNESS + JAW_THICKNESS])
|
||||
cube([ANCHOR_TAB_H, jaw_span_outer + 2*ANCHOR_TAB_T, ANCHOR_TAB_H]);
|
||||
}
|
||||
|
||||
// Hollow out the U-channel (cable pocket)
|
||||
// Inner cavity: inner_span wide, runs most of jaw length
|
||||
translate([x_root + JAW_THICKNESS * 0.5, -inner_span/2, BASE_THICKNESS])
|
||||
cube([jaw_length * 0.7, inner_span, JAW_THICKNESS + 0.5]);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// EAR BOSS (M3 attachment point)
|
||||
// =============================================================================
|
||||
//
|
||||
// A small raised button with a through-hole, providing optional redundant
|
||||
// attachment for straps or hard-wired retention.
|
||||
//
|
||||
|
||||
module ear_boss() {
|
||||
difference() {
|
||||
cylinder(d=EAR_D + 2*EAR_WALL_T, h=EAR_H);
|
||||
translate([0, 0, -1])
|
||||
cylinder(d=EAR_D, h=EAR_H + 2);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// UTILITY: Rounded Rectangle (for base and ghosts)
|
||||
// =============================================================================
|
||||
//
|
||||
|
||||
module rounded_rect(size, r) {
|
||||
// size = [width, length, height]
|
||||
w = size[0];
|
||||
l = size[1];
|
||||
h = size[2];
|
||||
|
||||
linear_extrude(height=h)
|
||||
offset(r=r)
|
||||
offset(r=-r)
|
||||
square([w, l], center=true);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// EXPORT / PRINT INSTRUCTIONS
|
||||
// =============================================================================
|
||||
//
|
||||
// CLIP 5mm (3D print × N):
|
||||
// openscad cable_management_clips.scad -D 'RENDER="clip_5mm"' -o clip_5mm.stl
|
||||
// Print settings: PETG/ASA, 4 perimeters, 20% infill, 0.2 mm layer
|
||||
// Orientation: base flat on bed (smooth finish for adhesive)
|
||||
//
|
||||
// CLIP 8mm (3D print × N):
|
||||
// openscad cable_management_clips.scad -D 'RENDER="clip_8mm"' -o clip_8mm.stl
|
||||
// Print settings: Same as 5mm
|
||||
//
|
||||
// CLIP 12mm (3D print × N):
|
||||
// openscad cable_management_clips.scad -D 'RENDER="clip_12mm"' -o clip_12mm.stl
|
||||
// Print settings: Same as 5mm
|
||||
//
|
||||
// =============================================================================
|
||||
//
|
||||
// INSTALLATION GUIDE
|
||||
//
|
||||
// 1. SURFACE PREP
|
||||
// • Clean chassis surface with isopropyl alcohol.
|
||||
// • Let dry for 5 minutes; inspect for dust or residue.
|
||||
//
|
||||
// 2. ADHESIVE APPLICATION
|
||||
// • Cut 3M VHB 5952F into ~50 × 40 mm pads (one per clip).
|
||||
// • Peel foil backing from VHB pad.
|
||||
// • Center pad on clip base; press firmly for 30 seconds.
|
||||
// • Peel clear polyester liner from exposed adhesive.
|
||||
//
|
||||
// 3. MOUNTING
|
||||
// • Position clip on chassis surface (e.g., along frame rail).
|
||||
// • Press and hold for 30 seconds, applying full body weight if possible.
|
||||
// • Let cure for 24 hours before loading cables.
|
||||
//
|
||||
// 4. CABLE INSERTION
|
||||
// • Gather cable bundle (power, signal, etc.); inspect for knots/damage.
|
||||
// • Align bundle perpendicular to clip jaws.
|
||||
// • Press upward with steady pressure until jaws snap closed (audible click).
|
||||
// • Tension should hold cable 5–10 N without slip.
|
||||
//
|
||||
// 5. ZIP-TIE ANCHORING (optional extra security)
|
||||
// • Thread 3.6 mm nylon zip-tie through jaw anchor tabs (left and right).
|
||||
// • Route around cable bundle; cinch to desired tension (avoid crushing).
|
||||
// • Trim excess tie length.
|
||||
//
|
||||
// 6. THREADED INSERTION (optional M3 redundancy)
|
||||
// • Install M3 threaded insert into ear hole (using M3 insertion tool).
|
||||
// • Thread M3 × 16 mm bolt with split washer through ear.
|
||||
// • Tighten 1.5 N·m (firm but not excessive).
|
||||
//
|
||||
// =============================================================================
|
||||
//
|
||||
// CABLE ROUTING BEST PRACTICES
|
||||
//
|
||||
// • Power cables (main): Use 12mm clips, spacing 150–200 mm apart.
|
||||
// • Mixed signal bundles: Use 8mm clips, spacing 100–150 mm apart.
|
||||
// • Individual sensor leads: Use 5mm clips or traditional P-clips.
|
||||
//
|
||||
// • Avoid sharp bends: Route bundles with R ≥ 50 mm (cable bundle diameter).
|
||||
// • Prevent abrasion: Use snap clips where cable crosses sharp edges.
|
||||
// • Allow thermal expansion: Leave ~2–3 mm slack in long runs.
|
||||
// • Color-code bundles: Use electrical tape or heatshrink before clipping.
|
||||
//
|
||||
// =============================================================================
|
||||
@ -0,0 +1,5 @@
|
||||
wheel_slip_detector:
|
||||
ros__parameters:
|
||||
frequency: 10
|
||||
slip_threshold: 0.1
|
||||
slip_timeout: 0.5
|
||||
@ -0,0 +1,24 @@
|
||||
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")],
|
||||
),
|
||||
])
|
||||
18
jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml
Normal file
18
jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml
Normal file
@ -0,0 +1,18 @@
|
||||
<?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>
|
||||
@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Wheel slip detector for SaltyBot."""
|
||||
|
||||
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):
|
||||
"""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"
|
||||
)
|
||||
|
||||
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()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script-dir=$base/lib/saltybot_wheel_slip_detector
|
||||
[install]
|
||||
install-scripts=$base/lib/saltybot_wheel_slip_detector
|
||||
25
jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py
Normal file
25
jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py
Normal file
@ -0,0 +1,25 @@
|
||||
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",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -0,0 +1,343 @@
|
||||
"""Unit tests for wheel_slip_detector_node."""
|
||||
|
||||
import pytest
|
||||
import math
|
||||
from geometry_msgs.msg import Twist
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
import rclpy
|
||||
|
||||
from saltybot_wheel_slip_detector.wheel_slip_detector_node import WheelSlipDetectorNode
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def rclpy_fixture():
|
||||
"""Initialize and cleanup rclpy."""
|
||||
rclpy.init()
|
||||
yield
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def node(rclpy_fixture):
|
||||
"""Create a wheel slip detector node instance."""
|
||||
node = WheelSlipDetectorNode()
|
||||
yield node
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
class TestNodeInitialization:
|
||||
"""Test suite for node initialization."""
|
||||
|
||||
def test_node_initialization(self, node):
|
||||
"""Test that node initializes with correct defaults."""
|
||||
assert node.cmd_vel is None
|
||||
assert node.actual_vel is None
|
||||
assert node.slip_threshold == 0.1
|
||||
assert node.slip_timeout == 0.5
|
||||
assert node.slip_duration == 0.0
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_frequency_parameter(self, node):
|
||||
"""Test frequency parameter is set correctly."""
|
||||
frequency = node.get_parameter("frequency").value
|
||||
assert frequency == 10
|
||||
|
||||
def test_slip_threshold_parameter(self, node):
|
||||
"""Test slip threshold parameter is set correctly."""
|
||||
threshold = node.get_parameter("slip_threshold").value
|
||||
assert threshold == 0.1
|
||||
|
||||
def test_slip_timeout_parameter(self, node):
|
||||
"""Test slip timeout parameter is set correctly."""
|
||||
timeout = node.get_parameter("slip_timeout").value
|
||||
assert timeout == 0.5
|
||||
|
||||
def test_period_calculation(self, node):
|
||||
"""Test that time period is correctly calculated from frequency."""
|
||||
assert node.period == pytest.approx(0.1)
|
||||
|
||||
|
||||
class TestSubscriptions:
|
||||
"""Test suite for subscription handling."""
|
||||
|
||||
def test_cmd_vel_subscription(self, node):
|
||||
"""Test that cmd_vel subscription updates node state."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
cmd.linear.y = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
assert node.cmd_vel is cmd
|
||||
assert node.cmd_vel.linear.x == 1.0
|
||||
|
||||
def test_odom_subscription(self, node):
|
||||
"""Test that odometry subscription updates actual velocity."""
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.95
|
||||
odom.twist.twist.linear.y = 0.48
|
||||
node._on_odom(odom)
|
||||
assert node.actual_vel is odom.twist.twist
|
||||
assert node.actual_vel.linear.x == 0.95
|
||||
|
||||
|
||||
class TestSlipDetection:
|
||||
"""Test suite for slip detection logic."""
|
||||
|
||||
def test_no_slip_perfect_match(self, node):
|
||||
"""Test no slip when commanded equals actual."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 1.0
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is False
|
||||
|
||||
def test_no_slip_small_difference(self, node):
|
||||
"""Test no slip when difference is below threshold."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.95
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is False
|
||||
|
||||
def test_slip_exceeds_threshold(self, node):
|
||||
"""Test slip detection when difference exceeds threshold."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.85
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is True
|
||||
|
||||
def test_slip_large_difference(self, node):
|
||||
"""Test slip detection with large velocity difference."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is True
|
||||
|
||||
def test_no_slip_both_zero(self, node):
|
||||
"""Test no slip when both commanded and actual are zero."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.0
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is False
|
||||
|
||||
def test_no_slip_both_near_zero(self, node):
|
||||
"""Test no slip when both are near zero (tolerance)."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.01
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.02
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is False
|
||||
|
||||
def test_slip_2d_velocity(self, node):
|
||||
"""Test slip detection with 2D velocity (x and y)."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.7
|
||||
cmd.linear.y = 0.7
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
odom.twist.twist.linear.y = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
assert node._check_slip() is True
|
||||
|
||||
|
||||
class TestSlipPersistence:
|
||||
"""Test suite for slip persistence timing."""
|
||||
|
||||
def test_slip_not_triggered_immediately(self, node):
|
||||
"""Test that slip is not triggered immediately but requires timeout."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_duration > 0.0
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_slip_declared_after_timeout(self, node):
|
||||
"""Test that slip is declared after timeout period."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
for _ in range(6):
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
|
||||
def test_slip_recovery_resets_duration(self, node):
|
||||
"""Test that slip duration resets when condition clears."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom1 = Odometry()
|
||||
odom1.twist.twist.linear.x = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom1)
|
||||
for _ in range(3):
|
||||
node._timer_callback()
|
||||
odom2 = Odometry()
|
||||
odom2.twist.twist.linear.x = 1.0
|
||||
node._on_odom(odom2)
|
||||
node._timer_callback()
|
||||
assert node.slip_duration == pytest.approx(0.0)
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_slip_cumulative_time(self, node):
|
||||
"""Test that slip duration accumulates across callbacks."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
for _ in range(3):
|
||||
node._timer_callback()
|
||||
assert node.slip_duration == pytest.approx(0.3)
|
||||
assert node.slip_detected is False
|
||||
for _ in range(3):
|
||||
node._timer_callback()
|
||||
assert node.slip_duration == pytest.approx(0.6)
|
||||
assert node.slip_detected is True
|
||||
|
||||
|
||||
class TestNoDataConditions:
|
||||
"""Test suite for behavior when sensor data is unavailable."""
|
||||
|
||||
def test_no_slip_without_cmd_vel(self, node):
|
||||
"""Test no slip declared when cmd_vel not received."""
|
||||
node.cmd_vel = None
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_no_slip_without_odometry(self, node):
|
||||
"""Test no slip declared when odometry not received."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
node._on_cmd_vel(cmd)
|
||||
node.actual_vel = None
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is False
|
||||
|
||||
|
||||
class TestScenarios:
|
||||
"""Integration-style tests for realistic scenarios."""
|
||||
|
||||
def test_scenario_normal_motion_no_slip(self, node):
|
||||
"""Scenario: Normal motion with good wheel traction."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.5
|
||||
for i in range(10):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5 + (i * 0.001)
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_scenario_ice_slip_persistent(self, node):
|
||||
"""Scenario: Ice causes persistent wheel slip."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
for _ in range(10):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.7
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
|
||||
def test_scenario_sandy_surface_intermittent_slip(self, node):
|
||||
"""Scenario: Sandy surface causes intermittent slip."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.8
|
||||
speeds = [0.7, 0.8, 0.6, 0.8, 0.7, 0.8]
|
||||
for speed in speeds:
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = speed
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_scenario_sudden_obstacle_slip(self, node):
|
||||
"""Scenario: Robot hits obstacle and wheels slip."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
for _ in range(3):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 1.0
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
for _ in range(8):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.2
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
|
||||
def test_scenario_wet_surface_recovery(self, node):
|
||||
"""Scenario: Wet surface slip, then wheel regains traction."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 1.0
|
||||
for _ in range(6):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.8
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
for _ in range(3):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 1.0
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is False
|
||||
|
||||
def test_scenario_backward_motion(self, node):
|
||||
"""Scenario: Backward motion with slip."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = -0.8
|
||||
for _ in range(6):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = -0.4
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
|
||||
def test_scenario_diagonal_motion_slip(self, node):
|
||||
"""Scenario: Diagonal motion with slip."""
|
||||
cmd = Twist()
|
||||
cmd.linear.x = 0.7
|
||||
cmd.linear.y = 0.7
|
||||
for _ in range(6):
|
||||
odom = Odometry()
|
||||
odom.twist.twist.linear.x = 0.5
|
||||
odom.twist.twist.linear.y = 0.5
|
||||
node._on_cmd_vel(cmd)
|
||||
node._on_odom(odom)
|
||||
node._timer_callback()
|
||||
assert node.slip_detected is True
|
||||
@ -58,6 +58,9 @@ import JoystickTeleop from './components/JoystickTeleop.jsx';
|
||||
// Network diagnostics (issue #222)
|
||||
import { NetworkPanel } from './components/NetworkPanel.jsx';
|
||||
|
||||
// Waypoint editor (issue #261)
|
||||
import { WaypointEditor } from './components/WaypointEditor.jsx';
|
||||
|
||||
const TAB_GROUPS = [
|
||||
{
|
||||
label: 'SOCIAL',
|
||||
@ -85,6 +88,13 @@ const TAB_GROUPS = [
|
||||
{ id: 'cameras', label: 'Cameras', },
|
||||
],
|
||||
},
|
||||
{
|
||||
label: 'NAVIGATION',
|
||||
color: 'text-teal-600',
|
||||
tabs: [
|
||||
{ id: 'waypoints', label: 'Waypoints' },
|
||||
],
|
||||
},
|
||||
{
|
||||
label: 'FLEET',
|
||||
color: 'text-green-600',
|
||||
@ -244,8 +254,10 @@ export default function App() {
|
||||
</div>
|
||||
</div>
|
||||
)}
|
||||
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
|
||||
{activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
|
||||
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
|
||||
{activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
|
||||
|
||||
{activeTab === 'waypoints' && <WaypointEditor subscribe={subscribe} publish={publishFn} callService={callService} />}
|
||||
|
||||
{activeTab === 'fleet' && <FleetPanel />}
|
||||
{activeTab === 'missions' && <MissionPlanner />}
|
||||
|
||||
403
ui/social-bot/src/components/WaypointEditor.jsx
Normal file
403
ui/social-bot/src/components/WaypointEditor.jsx
Normal file
@ -0,0 +1,403 @@
|
||||
/**
|
||||
* 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)
|
||||
*/
|
||||
|
||||
import { useEffect, useRef, useState } from 'react';
|
||||
|
||||
function WaypointEditor({ subscribe, publish, callService }) {
|
||||
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);
|
||||
|
||||
const [mapData, setMapData] = useState(null);
|
||||
const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 });
|
||||
|
||||
const canvasRef = useRef(null);
|
||||
const containerRef = useRef(null);
|
||||
|
||||
const mapDataRef = useRef(null);
|
||||
const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 });
|
||||
const waypointsRef = useRef([]);
|
||||
|
||||
// Subscribe to map data
|
||||
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
|
||||
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]);
|
||||
|
||||
const handleCanvasClick = (e) => {
|
||||
if (!mapDataRef.current || !containerRef.current) return;
|
||||
|
||||
const rect = containerRef.current.getBoundingClientRect();
|
||||
const clickX = e.clientX - rect.left;
|
||||
const clickY = e.clientY - rect.top;
|
||||
|
||||
const map = mapDataRef.current;
|
||||
const robot = robotPoseRef.current;
|
||||
const zoom = 1;
|
||||
|
||||
const centerX = containerRef.current.clientWidth / 2;
|
||||
const centerY = containerRef.current.clientHeight / 2;
|
||||
|
||||
const worldX = robot.x + (clickX - centerX) / zoom;
|
||||
const worldY = robot.y - (clickY - centerY) / zoom;
|
||||
|
||||
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 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);
|
||||
};
|
||||
|
||||
const sendNavGoal = async (waypoint) => {
|
||||
if (!callService) return;
|
||||
|
||||
try {
|
||||
const heading = waypoint.theta || 0;
|
||||
const halfHeading = heading / 2;
|
||||
|
||||
const goal = {
|
||||
pose: {
|
||||
position: {
|
||||
x: waypoint.x,
|
||||
y: waypoint.y,
|
||||
z: 0,
|
||||
},
|
||||
orientation: {
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: Math.sin(halfHeading),
|
||||
w: Math.cos(halfHeading),
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
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) break;
|
||||
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 */}
|
||||
<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={(e) => e.preventDefault()}
|
||||
>
|
||||
<svg className="absolute inset-0 w-full h-full pointer-events-none" id="waypoint-overlay">
|
||||
{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}>
|
||||
<circle
|
||||
cx={canvasX}
|
||||
cy={canvasY}
|
||||
r="10"
|
||||
fill={isActive ? '#ef4444' : isSelected ? '#fbbf24' : '#06b6d4'}
|
||||
opacity="0.8"
|
||||
/>
|
||||
<text
|
||||
x={canvasX}
|
||||
y={canvasY}
|
||||
textAnchor="middle"
|
||||
dominantBaseline="middle"
|
||||
fill="white"
|
||||
fontSize="10"
|
||||
fontWeight="bold"
|
||||
pointerEvents="none"
|
||||
>
|
||||
{idx + 1}
|
||||
</text>
|
||||
{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>
|
||||
);
|
||||
})}
|
||||
<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