final change

Author: minggangw

electron_demo/turtle_tf2/README.md

@@ -9,6 +9,7 @@ The turtle_tf2 demo showcases:
 - **Transform Broadcasting**: Multiple TF2 broadcasters publishing coordinate frame relationships
 - **Transform Listening**: Real-time monitoring and visualization of coordinate transformations
 - **Turtle Simulation**: Integration with turtlesim for turtle pose tracking and control
+- **Turtle Following**: Intelligent turtle2 behavior that automatically follows turtle1 using TF2 transforms
 - **3D Visualization**: WebGL-based rendering using Three.js for immersive coordinate frame visualization
 - **Interactive Controls**: Web interface for spawning turtles, controlling motion, and managing transforms
 
@@ -21,6 +22,13 @@ The turtle_tf2 demo showcases:
 - **Fixed Frame Broadcaster**: Maintains constant offset transforms
 - **Turtle Transform Broadcaster**: Converts turtle poses to TF2 transforms
 
+### Turtle Following System
+
+- **Real-time Following**: turtle2 automatically follows turtle1 using distance and angle calculations
+- **Smart Movement**: Proportional velocity control based on distance to target
+- **Collision Avoidance**: turtle2 stops when within optimal following distance (0.5 units)
+- **Transform Integration**: Following logic uses turtle pose data from TF2 coordinate frames
+
 ### Visualization
 
 - **3D Scene**: Interactive Three.js environment with orbit controls
@@ -133,8 +141,9 @@ npm start
    ```
 
 4. **Use the web interface to**:
-   - Click "Spawn Turtle1" to create the first turtle
    - Click "Spawn Turtle2" to create the second turtle
+   - Use WASD keys to control turtle1 movement
+   - Watch turtle2 automatically follow turtle1
    - Click "Start Demo" to initialize all TF2 broadcasters
    - Use frame toggle buttons to show/hide specific transforms
 
@@ -146,15 +155,16 @@ npm start
 - **TF2 Dynamic Broadcaster**: Publishes time-varying `carrot1_static → carrot1_dynamic` transform
 - **Fixed Frame Broadcaster**: Publishes constant offset `turtle1 → carrot1_fixed` transform
 - **Turtle TF2 Broadcaster**: Converts turtle poses to `world → turtle1/turtle2` transforms
-- **Turtle TF2 Listener**: Controls turtle2 to follow turtle1 using transform lookups
+- **Turtle TF2 Listener**: Monitors turtle poses and controls turtle2 following behavior using real-time transform data
 
 ### Renderer Process (renderer.js)
 
 - **3D Scene Management**: Three.js scene setup with lighting and camera controls
 - **Coordinate Frame Visualization**: Colored axes representation (X=red, Y=green, Z=blue)
 - **Turtle Rendering**: 3D turtle models with real-time pose updates
+- **Following Logic**: Calculates distance, angle, and velocity commands for turtle2 following behavior
 - **Transform Monitoring**: Live display of transform data and frame relationships
-- **User Interaction**: Control buttons and visual feedback systems
+- **User Interaction**: Control buttons, keyboard handling, and visual feedback systems
 
 ### HTML Interface (index.html)
 
@@ -201,7 +211,7 @@ world → turtle2
 - **A**: Turn left
 - **D**: Turn right
 
-💡 **Tip**: Click on the 3D visualization area first to ensure keyboard focus, then use WASD keys to drive turtle1 around the turtlesim environment.
+💡 **Tip**: Click on the 3D visualization area first to ensure keyboard focus, then use WASD keys to drive turtle1 around the turtlesim environment. turtle2 will automatically follow turtle1!
 
 **Camera Controls**:
 
@@ -214,10 +224,30 @@ world → turtle2
 
 ### Turtle Management
 
-- **Spawn Turtle1**: Creates turtle1 at position (5.5, 5.5)
-- **Spawn Turtle2**: Creates turtle2 at position (4.0, 2.0)
+- **Spawn Turtle2**: Creates turtle2 at position (4.0, 2.0) - turtle1 is automatically spawned by turtlesim
 - **Stop All**: Halts all turtle motion commands
 
+### Turtle Following Behavior
+
+Once turtle2 is spawned, it will automatically follow turtle1 with the following intelligent behaviors:
+
+- **Distance-based Speed**: turtle2 moves faster when far from turtle1, slower when close
+- **Angle Correction**: turtle2 continuously adjusts its heading to face turtle1
+- **Smart Stopping**: turtle2 stops moving when within 0.5 units of turtle1 to avoid collision
+- **Real-time Updates**: Following commands are sent every second based on current turtle positions
+
+**Following Algorithm Details**:
+
+- **Linear Velocity**: Proportional to distance (max speed: 2.0 units/sec)
+- **Angular Velocity**: Proportional to angle difference (4.0 × angle error)
+- **Minimum Following Distance**: 0.5 units (prevents excessive oscillation)
+
+You can observe the following behavior by:
+
+1. Spawning turtle2 using the "Spawn Turtle2" button
+2. Using WASD keys to move turtle1 around
+3. Watching turtle2 chase turtle1 in both the turtlesim window and 3D visualization
+
 ### Demo Control
 
 - **Start Demo**: Initializes all TF2 broadcasters and systems
@@ -246,10 +276,12 @@ world → turtle2
    - Check if ROS2 daemon is running: `ros2 daemon status`
    - Verify ROS2 installation: `ros2 --version`
 
-2. **"Turtlesim not responding"**
+2. **"Turtlesim not responding" or "Failed to spawn turtle2"**
 
    - Verify turtlesim is running: `ros2 run turtlesim turtlesim_node`
    - Check available topics: `ros2 topic list`
+   - Ensure spawn service is available: `ros2 service list | grep spawn`
+   - Try restarting turtlesim_node if spawn calls fail
 
 3. **"No transforms detected"**
 

electron_demo/turtle_tf2/main.js

@@ -285,10 +285,12 @@ async function createTurtleTf2Listener() {
     '/turtle2/cmd_vel'
   );
 
+  // Store turtle2 velocity publisher
+  turtleTf2Nodes.turtle2VelocityPublisher = velocityPublisher;
+
   // Create service client to spawn turtle2
   const spawner = node.createClient('turtlesim/srv/Spawn', '/spawn');
 
-  let turtleSpawned = false;
   let turtleSpawningServiceReady = false;
 
   // Transform listener functionality
@@ -342,7 +344,7 @@ async function createTurtleTf2Listener() {
 
         // Simple following logic (in real implementation, this would use TF lookup)
         // For demo purposes, we'll simulate the transform lookup behavior
-        if (turtleSpawned) {
+        if (turtleTf2Nodes.turtle2Spawned) {
           // This is a simplified version - in real TF2, we'd lookup transforms
           // For the demo, we'll let the renderer handle the following logic
           if (mainWindow) {
@@ -591,75 +593,104 @@ ipcMain.on('turtle-velocity-command', (event, data) => {
 });
 
 // Handle spawning requests from renderer
-ipcMain.on('spawn-turtle-request', async (event, data) => {
+ipcMain.on('spawn-turtle-request', (event, data) => {
   const { name, x, y, theta } = data;
 
   if (turtleTf2Nodes.listener) {
-    try {
-      console.log(
-        `Spawn request received: ${name || 'turtle2'} at (${x || 4}, ${y || 2}, ${theta || 0})`
-      );
+    console.log(
+      `Spawn request received: ${name || 'turtle2'} at (${x || 4}, ${y || 2}, ${theta || 0})`
+    );
 
-      // Use the existing spawner from the listener node or create one
-      const listenerNode = turtleTf2Nodes.listener;
-      let spawner = listenerNode._spawner;
+    // Use the existing spawner from the listener node or create one
+    const listenerNode = turtleTf2Nodes.listener;
+    let spawner = listenerNode._spawner;
 
-      if (!spawner) {
-        console.log('Creating new spawn service client...');
-        spawner = listenerNode.createClient('turtlesim/srv/Spawn', '/spawn');
-        listenerNode._spawner = spawner; // Cache it
+    if (!spawner) {
+      console.log('Creating new spawn service client...');
+      spawner = listenerNode.createClient('turtlesim/srv/Spawn', '/spawn');
+      listenerNode._spawner = spawner; // Cache it
 
-        // Wait for service to be ready
-        let retries = 0;
-        while (!spawner.isServiceServerAvailable() && retries < 10) {
-          console.log(`Waiting for spawn service... (attempt ${retries + 1})`);
-          await new Promise((resolve) => setTimeout(resolve, 200));
+      // Wait for service to be ready with timeout
+      let retries = 0;
+      const maxRetries = 10;
+
+      const checkService = () => {
+        if (spawner.isServiceServerAvailable()) {
+          performSpawn();
+        } else if (retries < maxRetries) {
           retries++;
+          console.log(`Waiting for spawn service... (attempt ${retries})`);
+          setTimeout(checkService, 200);
+        } else {
+          console.error('Turtlesim spawn service not available after waiting');
+          if (mainWindow) {
+            mainWindow.webContents.send('spawn-error', {
+              message: 'Turtlesim spawn service not available after waiting',
+            });
+          }
         }
-      }
+      };
 
-      if (!spawner.isServiceServerAvailable()) {
-        throw new Error('Turtlesim spawn service not available after waiting');
-      }
+      checkService();
+    } else {
+      performSpawn();
+    }
 
-      // Use the simplest possible request format
+    function performSpawn() {
       const xPos = x || 4.0;
       const yPos = y || 2.0;
       const angle = theta || 0.0;
       const turtleName = name || 'turtle2';
 
       console.log(`Spawning ${turtleName} at (${xPos}, ${yPos}, ${angle})`);
 
-      // Try the most basic service call format
-      const response = await spawner.sendRequest({
-        x: xPos,
-        y: yPos,
-        theta: angle,
-        name: turtleName,
-      });
-
-      console.log('Spawn response:', response);
-
-      if (response) {
-        const spawnedName = response.name || response || turtleName;
-        console.log(`Successfully spawned ${spawnedName}`);
-
-        if (mainWindow) {
-          mainWindow.webContents.send('turtle-spawned', {
-            name: spawnedName,
+      // Use callback pattern as required by rclnodejs
+      try {
+        spawner.sendRequest(
+          {
             x: xPos,
             y: yPos,
             theta: angle,
+            name: turtleName,
+          },
+          (response) => {
+            console.log('Spawn response:', response);
+
+            if (response && response.name) {
+              const spawnedName = response.name;
+              console.log(`Successfully spawned ${spawnedName}`);
+
+              // Set turtle spawned flag for following logic
+              if (spawnedName === 'turtle2') {
+                turtleTf2Nodes.turtle2Spawned = true;
+              }
+
+              if (mainWindow) {
+                mainWindow.webContents.send('turtle-spawned', {
+                  name: spawnedName,
+                  x: xPos,
+                  y: yPos,
+                  theta: angle,
+                });
+              }
+            } else {
+              console.error('Invalid or empty response from spawn service');
+              if (mainWindow) {
+                mainWindow.webContents.send('spawn-error', {
+                  message: 'Invalid response from spawn service',
+                });
+              }
+            }
+          }
+        );
+      } catch (error) {
+        console.error(`Error sending spawn request: ${error.message}`);
+        if (mainWindow) {
+          mainWindow.webContents.send('spawn-error', {
+            message: `Error sending spawn request: ${error.message}`,
           });
         }
       }
-    } catch (error) {
-      console.error(`Failed to spawn ${name || 'turtle2'}:`, error);
-      if (mainWindow) {
-        mainWindow.webContents.send('spawn-error', {
-          message: `Failed to spawn ${name || 'turtle2'}: ${error.message}`,
-        });
-      }
     }
   } else {
     console.error('Listener node not initialized');
@@ -683,6 +714,18 @@ ipcMain.on('turtle-cmd-vel', (event, data) => {
   }
 });
 
+// Handle turtle2 following commands
+ipcMain.on('turtle2-cmd-vel', (event, data) => {
+  if (turtleTf2Nodes.turtle2VelocityPublisher) {
+    const velocity = {
+      linear: data.linear,
+      angular: data.angular,
+    };
+    // Send velocity command to turtle2
+    turtleTf2Nodes.turtle2VelocityPublisher.publish(velocity);
+  }
+});
+
 app.whenReady().then(async () => {
   createWindow();
 

electron_demo/turtle_tf2/renderer.js

@@ -17,6 +17,7 @@ let scene, camera, renderer, controls;
 let turtles = {};
 let frames = {};
 let coordinateFrames = {};
+let turtlePoses = {}; // Track turtle poses for following logic
 
 // Transform tracking
 let activeTransforms = new Map();
@@ -283,6 +284,9 @@ function createFrame(name, position, quaternion, color, scale = 0.5) {
 }
 
 function updateTurtlePose(name, pose) {
+  // Store the pose for following logic
+  turtlePoses[name] = pose;
+
   if (!turtles[name]) {
     // Create turtle if it doesn't exist
     const color = name === 'turtle1' ? 0x00ff00 : 0x0088ff;
@@ -507,8 +511,48 @@ function setupROSListeners() {
 
   // Listen for follow requests
   ipcRenderer.on('request-turtle-follow', (event) => {
-    // Implement turtle following logic if needed
-    console.log('Follow request received');
+    // Implement turtle2 following turtle1 logic
+    if (turtlePoses['turtle1'] && turtlePoses['turtle2']) {
+      const turtle1Pose = turtlePoses['turtle1'];
+      const turtle2Pose = turtlePoses['turtle2'];
+
+      // Calculate distance and angle to turtle1
+      const dx = turtle1Pose.x - turtle2Pose.x;
+      const dy = turtle1Pose.y - turtle2Pose.y;
+      const distance = Math.sqrt(dx * dx + dy * dy);
+
+      // Calculate angle to target
+      const targetAngle = Math.atan2(dy, dx);
+      const currentAngle = turtle2Pose.theta;
+
+      // Calculate angular difference (handling wrap-around)
+      let angleDiff = targetAngle - currentAngle;
+      while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
+      while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
+
+      // Calculate velocities
+      const linearVel = Math.min(2.0 * distance, 2.0); // Max linear velocity of 2.0
+      const angularVel = 4.0 * angleDiff; // Proportional angular velocity
+
+      // Only move if there's significant distance
+      if (distance > 0.5) {
+        // Send velocity command to turtle2
+        ipcRenderer.send('turtle2-cmd-vel', {
+          linear: { x: linearVel, y: 0.0, z: 0.0 },
+          angular: { x: 0.0, y: 0.0, z: angularVel },
+        });
+
+        console.log(
+          `Turtle2 following: distance=${distance.toFixed(2)}, linear=${linearVel.toFixed(2)}, angular=${angularVel.toFixed(2)}`
+        );
+      } else {
+        // Stop turtle2 when close enough
+        ipcRenderer.send('turtle2-cmd-vel', {
+          linear: { x: 0.0, y: 0.0, z: 0.0 },
+          angular: { x: 0.0, y: 0.0, z: 0.0 },
+        });
+      }
+    }
   });
 }
 function updateTransformList(transform) {