diff --git a/src/components/panels/ArmControlPanel.tsx b/src/components/panels/ArmControlPanel.tsx index 3539777..b6deba6 100644 --- a/src/components/panels/ArmControlPanel.tsx +++ b/src/components/panels/ArmControlPanel.tsx @@ -10,8 +10,13 @@ const ArmControlPanel: React.FC = () => { const [poseNames, setPoseNames] = useState([]); const [selectedPose, setSelectedPose] = useState(''); const [presetName, setPresetName] = useState(''); + const [armState, setArmState] = useState(null); + const [servoStatus, setServoStatus] = useState(null); const [distance, setDistance] = useState(null); const [distanceStatus, setDistanceStatus] = useState(null); + const [checkCollisions, setCheckCollisions] = useState(false); + const [collisionParameterLoaded, setCollisionParameterLoaded] = useState(false); + const [collisionParameterUpdating, setCollisionParameterUpdating] = useState(false); const [response, setResponse] = useState<{ success: boolean; message: string } | null>(null); const refreshPoseNames = (preferredPose?: string) => { @@ -48,8 +53,83 @@ const ArmControlPanel: React.FC = () => { }); }; + const refreshCollisionChecking = () => { + if (!ros) return; + + setCollisionParameterLoaded(false); + + const service = new ROSLIB.Service({ + ros, + name: '/servo_node/get_parameters', + serviceType: 'rcl_interfaces/srv/GetParameters', + }); + + const request = new ROSLIB.ServiceRequest({ + names: ['moveit_servo.check_collisions'], + }); + + service.callService(request, (result: any) => { + const value = result.values?.[0]; + + if (!value || value.type !== 1) { + setCollisionParameterLoaded(false); + setResponse({ + success: false, + message: 'Failed to read /servo_node check_collisions parameter', + }); + return; + } + + setCheckCollisions(value.bool_value ?? false); + setCollisionParameterLoaded(true); + }); + }; + useEffect(() => { refreshPoseNames(); + refreshCollisionChecking(); + }, [ros]); + + useEffect(() => { + if (!ros) return; + + const stateTopic = new ROSLIB.Topic({ + ros, + name: '/arm_teleop_node/state', + messageType: 'std_msgs/msg/String', + queue_size: 1, + }); + + const handleState = (msg: any) => { + setArmState(msg.data); + }; + + stateTopic.subscribe(handleState); + + return () => { + stateTopic.unsubscribe(handleState); + }; + }, [ros]); + + useEffect(() => { + if (!ros) return; + + const statusTopic = new ROSLIB.Topic({ + ros, + name: '/servo_node/status', + messageType: 'moveit_msgs/msg/ServoStatus', + queue_size: 1, + }); + + const handleServoStatus = (msg: any) => { + setServoStatus(msg.message); + }; + + statusTopic.subscribe(handleServoStatus); + + return () => { + statusTopic.unsubscribe(handleServoStatus); + }; }, [ros]); useEffect(() => { @@ -84,6 +164,56 @@ const ArmControlPanel: React.FC = () => { return distance_with_offset.toFixed(3); }; + const handleCollisionCheckingChange = (enabled: boolean) => { + if (!ros) { + alert('ROS is not connected'); + return; + } + + setCollisionParameterUpdating(true); + + const service = new ROSLIB.Service({ + ros, + name: '/servo_node/set_parameters', + serviceType: 'rcl_interfaces/srv/SetParameters', + }); + + const request = new ROSLIB.ServiceRequest({ + parameters: [ + { + name: 'moveit_servo.check_collisions', + value: { + type: 1, + bool_value: enabled, + }, + }, + ], + }); + + service.callService(request, (result: any) => { + const parameterResult = result.results?.[0]; + const success = parameterResult?.successful ?? false; + const reason = parameterResult?.reason ?? ''; + + setCollisionParameterUpdating(false); + + if (!success) { + setResponse({ + success: false, + message: reason || 'Failed to update collision checking', + }); + refreshCollisionChecking(); + return; + } + + setCheckCollisions(enabled); + setResponse({ + success: true, + message: `Collision checking ${enabled ? 'enabled' : 'disabled'}`, + }); + }); + }; + const handleSavePreset = () => { if (!ros) { alert('ROS is not connected'); @@ -176,6 +306,13 @@ const ArmControlPanel: React.FC = () => { return (
+
+ State: + {armState ?? 'No data'} + Servo Status: + {servoStatus ?? 'No data'} +
+
handleCollisionCheckingChange(e.target.checked)} + disabled={!collisionParameterLoaded || collisionParameterUpdating} + /> + + @@ -297,6 +448,40 @@ const ArmControlPanel: React.FC = () => { font-weight: 600; } + .toggle-row { + display: flex; + justify-content: space-between; + align-items: center; + margin-bottom: 0.75rem; + padding: 0.5rem; + border: 1px solid #333; + border-radius: 4px; + background: #2b2b2b; + cursor: pointer; + } + + .toggle-row span { + display: flex; + flex-direction: column; + color: #ccc; + } + + .toggle-row small { + margin-top: 0.15rem; + color: #888; + font-size: 0.7rem; + } + + .toggle-row input { + width: 1.1rem; + height: 1.1rem; + cursor: pointer; + } + + .toggle-row input:disabled { + cursor: not-allowed; + } + button { background: #0070f3; color: #f1f1f1;