feat: remove legacy EncoderRead interface; implement unified odometry interface with nav_msgs/Odometry; update Blockly blocks for odometry data extraction

readme.md

@@ -155,19 +155,21 @@ i2cdetect -y 1          # scan device di bus 1 (perlu i2c-tools)
 - [ ] End-to-end: Blockly block → executor (real) → `/pwm/write` → pca9685_node → I2C write
 
 ## 4 Enhancement: AS5600 — 12-bit Magnetic Rotary Encoder (I2C) : [ ]
-AS5600 adalah 12-bit magnetic rotary position sensor via I2C. Kiwi wheel AMR menggunakan 3 encoder (satu per roda) untuk feedback posisi. AS5600 memiliki **fixed I2C address (0x36)** — untuk 3 module, setiap encoder menggunakan **I2C bus terpisah** (e.g., `/dev/i2c-1`, `/dev/i2c-3`, `/dev/i2c-4`). Node ini publisher — membaca angle secara periodik dan publish ke ROS2 topic.
+AS5600 adalah 12-bit magnetic rotary position sensor via I2C. Kiwi wheel AMR menggunakan 3 encoder (satu per roda) untuk feedback posisi. AS5600 memiliki **fixed I2C address (0x36)** — untuk 3 module, setiap encoder menggunakan **I2C bus terpisah** (e.g., `/dev/i2c-1`, `/dev/i2c-3`, `/dev/i2c-4`).
+
+> **Note**: Legacy `EncoderRead.msg`, `encoder.py` handler, dan `encoderRead.js` block telah dihapus dan diganti oleh **Task #5 (Unified Odometry Interface)**. `as5600_node` sekarang hanya publish `nav_msgs/Odometry` ke `odometry_encoder/odom`.
 
 ### Implementasi
 
 #### A. Package Structure (C++, ament_cmake)
 ```
 src/as5600_node/
-├── CMakeLists.txt              # ament_cmake — NO external lib dependency
+├── CMakeLists.txt              # ament_cmake — depend: rclcpp, nav_msgs, geometry_msgs
-├── package.xml                 # depend: rclcpp, blockly_interfaces
+├── package.xml
 ├── include/as5600_node/
-│   └── as5600_node.hpp         # As5600Node class + I2C helpers
+│   └── as5600_node.hpp         # As5600Node class + kinematics + I2C helpers
 └── src/
-    ├── as5600_node.cpp          # I2C init, timer_callback, read_raw_angle()
+    ├── as5600_node.cpp          # I2C read + kiwi wheel kinematics + nav_msgs publish
     └── main.cpp                 # rclcpp::spin(node)
 ```
 
@@ -175,93 +177,49 @@ Hardware interface menggunakan Linux I2C (`/dev/i2c-X`) via `ioctl()` — tidak
 
 #### B. ROS2 Interface
 
-**New message** — `blockly_interfaces/msg/EncoderRead.msg`:
+Menggunakan **`nav_msgs/Odometry`** (standar ROS2) — lihat Task #5 untuk detail fields dan kinematics.
-```
-uint8 encoder_id       # Encoder index (0, 1, 2, ...)
-float32 angle          # Angle in degrees (0.0-360.0)
-uint16 raw_angle       # Raw 12-bit value (0-4095)
-```
 
-**Topic**: `/encoder/state` (as5600_node → executor)
+**Topic**: `odometry_encoder/odom` (as5600_node → executor)
 
 **ROS2 Parameters** (configurable via `--ros-args -p`):
 | Parameter | Type | Default | Fungsi |
 |---|---|---|---|
 | `i2c_devices` | string[] | `["/dev/i2c-1"]` | List of I2C device paths, satu per encoder |
 | `publish_rate` | double | 10.0 | Publish frequency Hz |
+| `wheel_radius` | double | 5.0 | Radius roda (cm) — HARUS di-tune |
+| `wheel_distance` | double | 15.0 | Jarak center-to-wheel (cm) — HARUS di-tune |
+| `wheel_angles` | double[] | [0, 2π/3, 4π/3] | Sudut posisi roda (rad) |
 
-#### C. Node Behavior — `As5600Node`
+#### C. pixi.toml
-1. **Constructor**: open setiap I2C bus, set slave address 0x36 via `ioctl(I2C_SLAVE)`, create publisher dan timer
-2. **Timer callback**: iterate semua I2C fds → read 2-byte RAW_ANGLE register → compute `angle = raw * 360.0 / 4096.0` → publish EncoderRead
-3. **`read_raw_angle(fd)`**: write register address 0x0C, read 2 bytes → `((buf[0] & 0x0F) << 8) | buf[1]` (12-bit)
-4. **Multi-bus support**: satu node mengontrol semua encoder — setiap fd dedicated ke satu bus/encoder
-5. **Cleanup**: close semua file descriptors di destructor
-
-AS5600 register map:
-| Register | Address | Fungsi |
-|---|---|---|
-| RAW_ANGLE | 0x0C-0x0D | 12-bit raw angle (0-4095) |
-| STATUS | 0x0B | Magnet detect status |
-| AGC | 0x1A | Automatic gain control |
-
-#### D. Handler — `blockly_executor/handlers/encoder.py`
-```python
-@handler("encoder_read")
-def handle_encoder_read(params, hardware):
-    encoder_id = int(params["encoder_id"])
-    # Dummy: return "0.0". Real: subscribe /encoder/state, return cached angle
-```
-Lazy-create subscriber dengan cache `{encoder_id: {angle, raw_angle}}`, sama dengan pola `digital_in` di `gpio.py`.
-
-#### E. Blockly Block — `encoderRead.js`
-```
-┌──────────────────────────────────────┐
-│ Encoder Read  id: [0]                │
-└──────────────────────────────────────┘
-```
-- **id**: `FieldNumber` (0–2)
-- Returns: `Number` (angle 0-360)
-- Category: `Robot`, Command: `encoder_read`
-- Output block (can be used in expressions, e.g., `set variable to [Encoder Read id: 0]`)
-
-#### F. pixi.toml Changes
 - `build-as5600`: `colcon build --packages-select as5600_node` (depends-on: setup-dep, build-interfaces)
 - `as5600-node`: `ros2 run as5600_node as5600_node`
 
-Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel.
+#### D. Penggunaan
-
-#### G. Penggunaan
 
 ```bash
 # Default — /dev/i2c-1, 10 Hz, 1 encoder
 pixi run as5600-node
 
-# 3 encoder pada bus terpisah, 20 Hz
+# 3 encoder pada bus terpisah, 20 Hz, custom wheel geometry
 source install/setup.bash
 ros2 run as5600_node as5600_node --ros-args \
   -p i2c_devices:="['/dev/i2c-1', '/dev/i2c-3', '/dev/i2c-4']" \
-  -p publish_rate:=20.0
+  -p publish_rate:=20.0 \
-
+  -p wheel_radius:=5.0 \
-# Raspberry Pi: enable extra I2C buses via config.txt
+  -p wheel_distance:=15.0
-# dtoverlay=i2c-gpio,bus=3,i2c_gpio_sda=17,i2c_gpio_scl=27
-# dtoverlay=i2c-gpio,bus=4,i2c_gpio_sda=22,i2c_gpio_scl=23
 ```
 
 ### Definition Of Done
 - [x] `src/as5600_node/` berisi `CMakeLists.txt`, `package.xml`, `include/`, `src/`
-- [x] `blockly_interfaces/msg/EncoderRead.msg` terdaftar di `rosidl_generate_interfaces()`
-- [ ] `pixi run build-interfaces` berhasil — EncoderRead.msg ter-generate
 - [ ] `pixi run build-as5600` berhasil di Raspberry Pi (native build) tanpa error
-- [ ] Node berjalan: `pixi run as5600-node` — publish `/encoder/state`
+- [ ] Node berjalan: `pixi run as5600-node` — publish `odometry_encoder/odom`
-- [ ] Parameter `i2c_devices`, `publish_rate` berfungsi via `--ros-args -p`
+- [ ] Parameter `i2c_devices`, `publish_rate`, `wheel_radius`, `wheel_distance` berfungsi via `--ros-args -p`
-- [x] Handler `encoder_read` berfungsi di dummy mode (test passes)
+- [ ] End-to-end: as5600_node → `odometry_encoder/odom` → executor cache → Blockly
-- [x] Blockly block `encoderRead` muncul di toolbox, generate valid JS code
-- [ ] End-to-end: Blockly block → executor (real) → cache `/encoder/state` → return angle
 
 ## 5 Enhancement: Unified Odometry Interface — nav_msgs/Odometry : [ ]
 Interface odometry menggunakan standar ROS2 `nav_msgs/Odometry` agar kompatibel dengan ekosistem ROS2. Setiap jenis sensor odometry (encoder, IMU, optical) publish ke topic terpisah `odometry_<type>/odom` menggunakan message type yang sama.
 
-**Motivasi**: Modularitas — tambah sensor baru cukup buat node yang publish `nav_msgs/Odometry` ke `odometry_<type>/odom`. Dari Blockly, user pilih source dan field yang mau dibaca.
+**Motivasi**: Modularitas — tambah sensor baru cukup buat node yang publish `nav_msgs/Odometry` ke `odometry_<type>/odom`. Dari Blockly, user pilih source via dropdown.
 
 **Arsitektur**:
 ```
@@ -277,17 +235,17 @@ future: optical_node      → odometry_optical/odom   (nav_msgs/Odometry)
 ### Implementasi
 
 #### A. Standard Interface — `nav_msgs/Odometry`
-Tidak menggunakan custom message. `nav_msgs/Odometry` sudah tersedia di RoboStack (`ros-jazzy-desktop` dan `ros-jazzy-ros-base`). Message fields yang digunakan:
+Tidak menggunakan custom message. `nav_msgs/Odometry` sudah tersedia di RoboStack. Message fields yang digunakan:
 - `pose.pose.position.x/y` — posisi robot (cm)
 - `pose.pose.orientation` — quaternion dari heading (2D: z=sin(θ/2), w=cos(θ/2))
 - `twist.twist.linear.x/y` — kecepatan robot (cm/s)
 - `twist.twist.angular.z` — kecepatan angular (rad/s)
 - `header.frame_id` = `"odom"`, `child_frame_id` = `"base_link"`
 
-#### B. AS5600 Node — Kiwi Wheel Kinematics (backward compatible)
+#### B. AS5600 Node — Kiwi Wheel Kinematics
-`as5600_node` menghitung robot-level odometry langsung dari 3 encoder menggunakan kiwi wheel forward kinematics, lalu publish ke **dua** topic:
+`as5600_node` menghitung robot-level odometry langsung dari 3 encoder menggunakan kiwi wheel forward kinematics, publish ke `odometry_encoder/odom` (`nav_msgs/Odometry`).
-- `/encoder/state` (`EncoderRead`) — legacy per-wheel angle, backward compat
+
-- `odometry_encoder/odom` (`nav_msgs/Odometry`) — robot-level pose + twist
+Legacy `EncoderRead.msg` dan `/encoder/state` topic telah dihapus — clean break, hanya `nav_msgs/Odometry`.
 
 **Kiwi Wheel Forward Kinematics** (3 roda @ 120°):
 ```
@@ -304,38 +262,38 @@ y  += (vx·sin(θ) + vy·cos(θ))·dt   [cm]
 θ  += ωz·dt                         [rad]
 ```
 
-**Parameter kiwi wheel** (configurable via `--ros-args -p`):
+#### C. Handler — `odometry.py` (returns JSON)
-| Parameter | Type | Default | Keterangan |
-|---|---|---|---|
-| `wheel_radius` | double | 5.0 | Radius roda (cm) — HARUS di-tune |
-| `wheel_distance` | double | 15.0 | Jarak center-to-wheel (cm) — HARUS di-tune |
-| `wheel_angles` | double[] | [0, 2π/3, 4π/3] | Sudut posisi roda (rad) |
-
-#### C. Handler — `odometry.py`
 ```python
 @handler("odometry_read")
 def handle_odometry_read(params, hardware):
-    source = params.get("source", "encoder")  # "encoder" | "imu" | "optical"
+    source = params.get("source", "encoder")
-    field = params["field"]                     # "x" | "y" | "heading" | "vx" | "vy" | "omega_z"
+    # Returns ALL fields as JSON: {"x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega_z":0.0}
 ```
-Lazy-create subscriber per source ke `odometry_<source>/odom`. Cache robot-level fields dengan quaternion → yaw conversion.
+Lazy-create subscriber per source ke `odometry_<source>/odom`. Satu action call return semua data sekaligus — efisien, tidak perlu action call per-field.
 
-#### D. Blockly Block — `odometryRead.js`
+#### D. Blockly Blocks — Fetch Once, Extract Many
+Dua block terpisah untuk efisiensi (1 action call untuk semua field):
+
+**Block 1: `getOdometry`** (`odometryRead.js`) — Value block, fetch all data:
 ```
-┌───────────────────────────────────────────────────────────┐
+┌─────────────────────────────────┐
-│ Odometry Read  source: [Encoder ▾]  field: [X (cm) ▾]    │
+│ getOdometry [Encoder ▾]         │ → output: Object
-└───────────────────────────────────────────────────────────┘
+└─────────────────────────────────┘
+```
+Digunakan dengan Blockly built-in "set variable to" block:
+```
+set [odom ▾] to [getOdometry [Encoder ▾]]    ← 1 action call
 ```
-- **source**: `FieldDropdown` — `Encoder` (extensible: `IMU`, `Optical`)
-- **field**: `FieldDropdown` — `X (cm)`, `Y (cm)`, `Heading (rad)`, `Vel X (cm/s)`, `Vel Y (cm/s)`, `Angular Vel (rad/s)`
-- Returns: `Number`, Category: `Robot`, Command: `odometry_read`
 
-#### E. Backward Compatibility
+**Block 2: `getValueOdometry`** (`odometryGet.js`) — Value block, extract field (no action call):
-- `EncoderRead.msg`, `encoder.py`, `encoderRead.js` tetap ada dan berfungsi
+```
-- Existing test `test_block_encoder.py` tetap pass
+┌───────────────────────────────────────────────┐
-- Saved workspace yang pakai `encoderRead` block tetap bisa di-load
+│ getValueOdometry [X (cm) ▾] from [odom ▾]     │ → output: Number
+└───────────────────────────────────────────────┘
+```
+Fields: `X (cm)`, `Y (cm)`, `Heading (rad)`, `Vel X (cm/s)`, `Vel Y (cm/s)`, `Angular Vel (rad/s)`
 
-#### F. Future Phases (blocked on mekanik)
+#### E. Future Phases (blocked on mekanik)
 - Sensor nodes baru (`imu_node`, `optical_node`) publish `nav_msgs/Odometry` ke `odometry_<type>/odom`
 - Update `odometryRead.js` dropdown source untuk sensor baru
 - Handler `odometry.py` auto-subscribe ke topic baru via `_SOURCE_TOPICS` dict
@@ -343,10 +301,10 @@ Lazy-create subscriber per source ke `odometry_<source>/odom`. Cache robot-level
 ### Definition Of Done
 - [x] Interface menggunakan `nav_msgs/Odometry` (bukan custom message)
 - [x] `as5600_node` publish ke `odometry_encoder/odom` dengan kiwi wheel kinematics
-- [x] Parameter `wheel_radius`, `wheel_distance`, `wheel_angles` configurable
+- [x] Legacy `EncoderRead.msg`, `encoder.py`, `encoderRead.js` dihapus — clean break
+- [x] Handler `odometry_read` return JSON semua fields (bukan per-field)
+- [x] Blockly: `getOdometry` (fetch) + `getValueOdometry` (extract) — 1 action call
 - [ ] `pixi run build-as5600` berhasil — as5600_node compile dengan nav_msgs dependency
 - [x] Handler `odometry_read` berfungsi di dummy mode (test passes)
-- [x] Blockly block `odometryRead` dengan source + field dropdowns
+- [ ] End-to-end: Blockly → executor (real) → cache `odometry_encoder/odom` → return JSON
-- [ ] End-to-end: Blockly → executor (real) → cache `odometry_encoder/odom` → return value
-- [x] Backward compat: `encoderRead` block dan `encoder_read` handler tetap berfungsi
 - [x] Integration test `test_block_odometry.py` passes di dummy mode

src/as5600_node/CMakeLists.txt

@@ -8,7 +8,6 @@ endif()
 # ROS2 dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(blockly_interfaces REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
@@ -24,7 +23,6 @@ target_include_directories(as5600_node PUBLIC
 
 ament_target_dependencies(as5600_node
   rclcpp
-  blockly_interfaces
   nav_msgs
   geometry_msgs
 )

src/as5600_node/include/as5600_node/as5600_node.hpp

@@ -7,7 +7,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "blockly_interfaces/msg/encoder_read.hpp"
 #include <nav_msgs/msg/odometry.hpp>
 
 class As5600Node : public rclcpp::Node
@@ -34,8 +33,6 @@ private:
   // STATUS register bit masks
   static constexpr uint8_t STATUS_MD = 0x20;  // Magnet Detected
 
-  // Legacy publisher (EncoderRead on /encoder/state)
-  rclcpp::Publisher<blockly_interfaces::msg::EncoderRead>::SharedPtr encoder_pub_;
   // Odometry publisher (nav_msgs/Odometry on odometry_encoder/odom)
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::TimerBase::SharedPtr read_timer_;

src/as5600_node/package.xml

@@ -10,7 +10,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>blockly_interfaces</depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
 

src/as5600_node/src/as5600_node.cpp

@@ -37,10 +37,6 @@ As5600Node::As5600Node()
   // Initialize velocity computation state
   prev_angle_rad_.resize(i2c_fds_.size(), 0.0);
 
-  // Create legacy publisher for /encoder/state (backward compat)
-  encoder_pub_ = this->create_publisher<blockly_interfaces::msg::EncoderRead>(
-    "/encoder/state", 10);
-
   // Create odometry publisher on odometry_encoder/odom (nav_msgs/Odometry)
   odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>(
     "odometry_encoder/odom", 10);
@@ -145,13 +141,6 @@ void As5600Node::timer_callback()
       wheel_velocities[i] = delta / dt_;
     }
     prev_angle_rad_[i] = angle_rad;
-
-    // Legacy: publish EncoderRead on /encoder/state (backward compat)
-    blockly_interfaces::msg::EncoderRead enc_msg;
-    enc_msg.encoder_id = static_cast<uint8_t>(i);
-    enc_msg.raw_angle = raw;
-    enc_msg.angle = static_cast<float>(raw) * 360.0f / 4096.0f;
-    encoder_pub_->publish(enc_msg);
   }
 
   if (first_reading_) {

src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js

@@ -1,26 +0,0 @@
-
-BlockRegistry.register({
-  name: 'encoderRead',
-  category: 'Robot',
-  categoryColor: '#5b80a5',
-  color: '#9C27B0',
-  tooltip: 'Read AS5600 encoder angle in degrees (0-360)',
-
-  definition: {
-    init: function () {
-      this.appendDummyInput()
-        .appendField('Encoder Read id:')
-        .appendField(new Blockly.FieldNumber(0, 0, 2, 1), 'ENCODER_ID');
-      this.setOutput(true, 'Number');
-      this.setColour('#9C27B0');
-      this.setTooltip('Read AS5600 encoder angle in degrees (0-360)');
-    }
-  },
-
-  generator: function (block) {
-    var encoderId = block.getFieldValue('ENCODER_ID');
-    var code =
-      'parseFloat((await executeAction(\'encoder_read\', { encoder_id: \'' + encoderId + '\' })).message)';
-    return [code, Blockly.JavaScript.ORDER_AWAIT];
-  }
-});

src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js

@@ -14,6 +14,6 @@ const BLOCK_FILES = [
   'digitalIn.js',
   'delay.js',
   'pwmWrite.js',
-  'encoderRead.js',
   'odometryRead.js',
+  'odometryGet.js',
 ];

src/blockly_app/blockly_app/ui/blockly/blocks/odometryGet.js

@@ -0,0 +1,35 @@
+
+BlockRegistry.register({
+  name: 'odometryGet',
+  category: 'Robot',
+  categoryColor: '#5b80a5',
+  color: '#00897B',
+  tooltip: 'Extract a field from odometry data — connect a variable block to "from"',
+
+  definition: {
+    init: function () {
+      this.appendValueInput('VAR')
+        .appendField('getValueOdometry')
+        .appendField(new Blockly.FieldDropdown([
+          ['X (cm)', 'x'],
+          ['Y (cm)', 'y'],
+          ['Heading (rad)', 'heading'],
+          ['Vel X (cm/s)', 'vx'],
+          ['Vel Y (cm/s)', 'vy'],
+          ['Angular Vel (rad/s)', 'omega_z']
+        ]), 'FIELD')
+        .appendField('from');
+      this.setOutput(true, 'Number');
+      this.setColour('#00897B');
+      this.setTooltip('Extract a value from odometry data — use after "set var to getOdometry"');
+    }
+  },
+
+  generator: function (block) {
+    var varCode = Blockly.JavaScript.valueToCode(
+      block, 'VAR', Blockly.JavaScript.ORDER_MEMBER) || '{}';
+    var field = block.getFieldValue('FIELD');
+    var code = '(' + varCode + '.' + field + ')';
+    return [code, Blockly.JavaScript.ORDER_MEMBER];
+  }
+});

src/blockly_app/blockly_app/ui/blockly/blocks/odometryRead.js

@@ -4,35 +4,25 @@ BlockRegistry.register({
   category: 'Robot',
   categoryColor: '#5b80a5',
   color: '#00897B',
-  tooltip: 'Read robot odometry — position (cm), velocity (cm/s), or heading (rad)',
+  tooltip: 'Fetch all odometry data from source — use with "set variable" block, then extract with getValueOdometry',
 
   definition: {
     init: function () {
       this.appendDummyInput()
-        .appendField('Odometry Read source:')
+        .appendField('getOdometry')
         .appendField(new Blockly.FieldDropdown([
           ['Encoder', 'encoder']
-        ]), 'SOURCE')
+        ]), 'SOURCE');
-        .appendField('field:')
+      this.setOutput(true, null);
-        .appendField(new Blockly.FieldDropdown([
-          ['X (cm)', 'x'],
-          ['Y (cm)', 'y'],
-          ['Heading (rad)', 'heading'],
-          ['Vel X (cm/s)', 'vx'],
-          ['Vel Y (cm/s)', 'vy'],
-          ['Angular Vel (rad/s)', 'omega_z']
-        ]), 'FIELD');
-      this.setOutput(true, 'Number');
       this.setColour('#00897B');
-      this.setTooltip('Read robot odometry — position (cm), velocity (cm/s), or heading (rad)');
+      this.setTooltip('Fetch all odometry data (x, y, heading, vx, vy, omega_z) from source');
     }
   },
 
   generator: function (block) {
     var source = block.getFieldValue('SOURCE');
-    var field = block.getFieldValue('FIELD');
     var code =
-      'parseFloat((await executeAction(\'odometry_read\', { source: \'' + source + '\', field: \'' + field + '\' })).message)';
+      'JSON.parse((await executeAction(\'odometry_read\', { source: \'' + source + '\' })).message)';
     return [code, Blockly.JavaScript.ORDER_AWAIT];
   }
 });

src/blockly_executor/blockly_executor/handlers/encoder.py

@@ -1,55 +0,0 @@
-"""AS5600 encoder handler — read cached angle from /encoder/state topic.
-
-Real mode communication:
-  encoder_read ← subscribe EncoderRead from /encoder/state ← as5600_node reads I2C
-"""
-
-import threading
-
-from . import handler
-from .hardware import Hardware
-
-
-def _get_encoder_state_subscriber(hardware: Hardware):
-    """Lazy-create a subscriber for /encoder/state with per-encoder cache.
-
-    as5600_node publishes EncoderRead per encoder periodically.
-    Subscriber caches latest angle per encoder_id.
-    """
-    if not hasattr(hardware.node, "_encoder_state_cache"):
-        from blockly_interfaces.msg import EncoderRead
-
-        hardware.node._encoder_state_cache = {}
-        hardware.node._encoder_state_lock = threading.Lock()
-
-        def _encoder_state_cb(msg: EncoderRead):
-            with hardware.node._encoder_state_lock:
-                hardware.node._encoder_state_cache[msg.encoder_id] = {
-                    "angle": msg.angle,
-                    "raw_angle": msg.raw_angle,
-                }
-
-        hardware.node._encoder_state_sub = hardware.node.create_subscription(
-            EncoderRead, "/encoder/state", _encoder_state_cb, 10
-        )
-    return hardware.node._encoder_state_cache
-
-
-@handler("encoder_read")
-def handle_encoder_read(
-    params: dict[str, str], hardware: Hardware
-) -> tuple[bool, str]:
-    encoder_id = int(params["encoder_id"])
-    hardware.log(f"encoder_read(id={encoder_id})")
-
-    if hardware.is_real():
-        cache = _get_encoder_state_subscriber(hardware)
-        with hardware.node._encoder_state_lock:
-            data = cache.get(encoder_id)
-
-        if data is None:
-            return (True, "0.0")
-        return (True, str(data["angle"]))
-
-    # Dummy mode — return 0.0
-    return (True, "0.0")

src/blockly_executor/blockly_executor/handlers/odometry.py

@@ -7,6 +7,7 @@ Real mode communication:
 Units: cm for position, cm/s for velocity, rad for heading, rad/s for angular velocity.
 """
 
+import json
 import math
 import threading
 
@@ -31,11 +32,7 @@ def _get_odometry_subscriber(hardware: Hardware, source: str):
 
     Each source gets its own subscription and cache entry.
     """
-    cache_attr = "_odometry_cache"
+    if not hasattr(hardware.node, "_odometry_cache"):
-    lock_attr = "_odometry_lock"
-    subs_attr = "_odometry_subs"
-
-    if not hasattr(hardware.node, cache_attr):
         hardware.node._odometry_cache = {}
         hardware.node._odometry_lock = threading.Lock()
         hardware.node._odometry_subs = {}
@@ -69,19 +66,16 @@ def handle_odometry_read(
     params: dict[str, str], hardware: Hardware
 ) -> tuple[bool, str]:
     source = params.get("source", "encoder")
-    field = params["field"]
+    hardware.log(f"odometry_read(source={source})")
-    hardware.log(f"odometry_read(source={source}, field={field})")
+
+    data = {"x": 0.0, "y": 0.0, "heading": 0.0, "vx": 0.0, "vy": 0.0, "omega_z": 0.0}
 
     if hardware.is_real():
         cache = _get_odometry_subscriber(hardware, source)
         with hardware.node._odometry_lock:
-            data = cache.get(source)
+            cached = cache.get(source)
 
-        if data is None:
+        if cached is not None:
-            return (True, "0.0")
+            data = cached
 
-        value = data.get(field, 0.0)
+    return (True, json.dumps(data))
-        return (True, str(value))
-
-    # Dummy mode — return 0.0
-    return (True, "0.0")

src/blockly_executor/test/test_block_encoder.py

@@ -1,30 +0,0 @@
-"""Integration tests for encoder_read handler (AS5600)."""
-
-
-def test_encoder_read_success(exe_action):
-    result = exe_action("encoder_read", encoder_id="0")
-    assert result.result.success is True
-    # Dummy mode returns "0.0"
-    angle = float(result.result.message)
-    assert 0.0 <= angle <= 360.0
-
-
-def test_encoder_read_id_1(exe_action):
-    result = exe_action("encoder_read", encoder_id="1")
-    assert result.result.success is True
-
-
-def test_encoder_read_id_2(exe_action):
-    result = exe_action("encoder_read", encoder_id="2")
-    assert result.result.success is True
-
-
-def test_encoder_read_sends_feedback(exe_action):
-    result = exe_action("encoder_read", encoder_id="0")
-    assert len(result.feedbacks) > 0
-    assert result.feedbacks[0].status == "executing"
-
-
-def test_encoder_read_missing_params_fails(exe_action):
-    result = exe_action("encoder_read")
-    assert result.result.success is False

src/blockly_executor/test/test_block_odometry.py

@@ -1,56 +1,45 @@
-"""Integration tests for odometry_read handler (nav_msgs/Odometry interface)."""
+"""Integration tests for odometry_read handler (nav_msgs/Odometry interface).
+
+Handler returns JSON with all 6 fields: x, y, heading, vx, vy, omega_z.
+"""
+
+import json
 
 
-def test_odometry_read_x(exe_action):
+def test_odometry_read_returns_json(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="x")
+    """Handler returns JSON with all 6 fields."""
+    result = exe_action("odometry_read", source="encoder")
     assert result.result.success is True
-    value = float(result.result.message)
+    data = json.loads(result.result.message)
-    assert value == 0.0  # Dummy mode returns 0.0
+    assert "x" in data
+    assert "y" in data
+    assert "heading" in data
+    assert "vx" in data
+    assert "vy" in data
+    assert "omega_z" in data
 
 
-def test_odometry_read_y(exe_action):
+def test_odometry_read_dummy_values(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="y")
+    """Dummy mode returns all zeros."""
-    assert result.result.success is True
+    result = exe_action("odometry_read", source="encoder")
-    assert float(result.result.message) == 0.0
+    data = json.loads(result.result.message)
-
+    assert data["x"] == 0.0
-
+    assert data["y"] == 0.0
-def test_odometry_read_heading(exe_action):
+    assert data["heading"] == 0.0
-    result = exe_action("odometry_read", source="encoder", field="heading")
+    assert data["vx"] == 0.0
-    assert result.result.success is True
+    assert data["vy"] == 0.0
-    assert float(result.result.message) == 0.0
+    assert data["omega_z"] == 0.0
-
-
-def test_odometry_read_vx(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="vx")
-    assert result.result.success is True
-    assert float(result.result.message) == 0.0
-
-
-def test_odometry_read_vy(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="vy")
-    assert result.result.success is True
-    assert float(result.result.message) == 0.0
-
-
-def test_odometry_read_omega_z(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="omega_z")
-    assert result.result.success is True
-    assert float(result.result.message) == 0.0
 
 
 def test_odometry_read_default_source(exe_action):
     """When source param is omitted, defaults to encoder."""
-    result = exe_action("odometry_read", field="x")
+    result = exe_action("odometry_read")
     assert result.result.success is True
-    assert float(result.result.message) == 0.0
+    data = json.loads(result.result.message)
+    assert data["x"] == 0.0
 
 
 def test_odometry_read_sends_feedback(exe_action):
-    result = exe_action("odometry_read", source="encoder", field="x")
+    result = exe_action("odometry_read", source="encoder")
     assert len(result.feedbacks) > 0
     assert result.feedbacks[0].status == "executing"
-
-
-def test_odometry_read_missing_field_fails(exe_action):
-    result = exe_action("odometry_read", source="encoder")
-    assert result.result.success is False

src/blockly_interfaces/CMakeLists.txt

@@ -9,7 +9,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GpioWrite.msg"
   "msg/GpioRead.msg"
   "msg/PwmWrite.msg"
-  "msg/EncoderRead.msg"
 )
 
 ament_export_dependencies(rosidl_default_runtime)

src/blockly_interfaces/msg/EncoderRead.msg

@@ -1,3 +0,0 @@
-uint8 encoder_id       # Encoder index (0, 1, 2, ...)
-float32 angle          # Angle in degrees (0.0-360.0)
-uint16 raw_angle       # Raw 12-bit value (0-4095)

workspace.json

@@ -5,23 +5,20 @@
       {
         "type": "main_program",
         "id": "COLVqmFP{j*XNMc.9rz+",
-        "x": 210,
+        "x": -50,
-        "y": 190,
+        "y": -30,
         "inputs": {
           "BODY": {
             "block": {
-              "type": "controls_whileUntil",
+              "type": "controls_repeat_ext",
-              "id": "UZH|b`4F=4X`zYjNW9zL",
+              "id": "l?jsj;kx|7.`vK%OX$hB",
-              "fields": {
-                "MODE": "WHILE"
-              },
               "inputs": {
-                "BOOL": {
+                "TIMES": {
-                  "block": {
+                  "shadow": {
-                    "type": "logic_boolean",
+                    "type": "math_number",
-                    "id": "nsD(~e:hYj[+9m-)yu-4",
+                    "id": "|BZr(3i5I3Fv=SLR#wy$",
                     "fields": {
-                      "BOOL": "TRUE"
+                      "NUM": 1
                     }
                   }
                 },
@@ -63,6 +60,139 @@
                         "id": "IXp?_lac7+V*GG!lW{]0",
                         "fields": {
                           "DURATION_MS": 1000
+                        },
+                        "next": {
+                          "block": {
+                            "type": "variables_set",
+                            "id": ",tz/Zq,NX|Jd6V+|dDl{",
+                            "fields": {
+                              "VAR": {
+                                "id": "C_:{ED@bJimgLzEmC6(`"
+                              }
+                            },
+                            "inputs": {
+                              "VALUE": {
+                                "block": {
+                                  "type": "digitalIn",
+                                  "id": "DJpFh.6H~L9fX2V4SDJd",
+                                  "fields": {
+                                    "GPIO": 16
+                                  }
+                                }
+                              }
+                            },
+                            "next": {
+                              "block": {
+                                "type": "delay",
+                                "id": "VQy`Sl3]ey49sP%+N6$R",
+                                "fields": {
+                                  "DURATION_MS": 500
+                                },
+                                "next": {
+                                  "block": {
+                                    "type": "variables_set",
+                                    "id": "i|LkDgVjImZd2}owndlz",
+                                    "fields": {
+                                      "VAR": {
+                                        "id": "[g,f6Mp!O$eZPCFs0U[H"
+                                      }
+                                    },
+                                    "inputs": {
+                                      "VALUE": {
+                                        "block": {
+                                          "type": "math_number",
+                                          "id": "[C@fwlekugl(`pi1b;1(",
+                                          "fields": {
+                                            "NUM": 100
+                                          }
+                                        }
+                                      }
+                                    },
+                                    "next": {
+                                      "block": {
+                                        "type": "pwmWrite",
+                                        "id": "Ezn#r.|lvDj5{Q1-C:E$",
+                                        "fields": {
+                                          "ADDRESS": "64",
+                                          "CHANNEL": 0
+                                        },
+                                        "inputs": {
+                                          "PWM_VALUE": {
+                                            "block": {
+                                              "type": "variables_get",
+                                              "id": "OkA-}PRPzgi;[I)@vcG$",
+                                              "fields": {
+                                                "VAR": {
+                                                  "id": "[g,f6Mp!O$eZPCFs0U[H"
+                                                }
+                                              }
+                                            }
+                                          }
+                                        },
+                                        "next": {
+                                          "block": {
+                                            "type": "variables_set",
+                                            "id": "m{+MhlXz-1tpl`mPPBh5",
+                                            "fields": {
+                                              "VAR": {
+                                                "id": "ju{xs[rjZumqS87$0nhu"
+                                              }
+                                            },
+                                            "inputs": {
+                                              "VALUE": {
+                                                "block": {
+                                                  "type": "odometryRead",
+                                                  "id": "v=Js89HC8D0UUA.-pN[q",
+                                                  "fields": {
+                                                    "SOURCE": "encoder"
+                                                  }
+                                                }
+                                              }
+                                            },
+                                            "next": {
+                                              "block": {
+                                                "type": "variables_set",
+                                                "id": "]LcUOwlc-y=`.e?EVgTa",
+                                                "fields": {
+                                                  "VAR": {
+                                                    "id": "Ug!mIa*[PnsL?H#9Ar*G"
+                                                  }
+                                                },
+                                                "inputs": {
+                                                  "VALUE": {
+                                                    "block": {
+                                                      "type": "odometryGet",
+                                                      "id": "VG2Q/8?zcyU}s4!W;V/M",
+                                                      "fields": {
+                                                        "FIELD": "x"
+                                                      },
+                                                      "inputs": {
+                                                        "VAR": {
+                                                          "block": {
+                                                            "type": "variables_get",
+                                                            "id": "^AW6|z21?ycRyzJ2y5u9",
+                                                            "fields": {
+                                                              "VAR": {
+                                                                "id": "ju{xs[rjZumqS87$0nhu"
+                                                              }
+                                                            }
+                                                          }
+                                                        }
+                                                      }
+                                                    }
+                                                  }
+                                                }
+                                              }
+                                            }
+                                          }
+                                        }
+                                      }
+                                    }
+                                  }
+                                }
+                              }
+                            }
+                          }
                         }
                       }
                     }
@@ -76,8 +206,8 @@
       {
         "type": "procedures_defreturn",
         "id": "4W(2:w1NGV^I;j6@^_I|",
-        "x": 630,
+        "x": 330,
-        "y": 210,
+        "y": -30,
         "extraState": {
           "params": [
             {
@@ -146,6 +276,22 @@
     {
       "name": "logic",
       "id": "-HsGyh[-?q^.O;|%cRw="
+    },
+    {
+      "name": "pinIn",
+      "id": "C_:{ED@bJimgLzEmC6(`"
+    },
+    {
+      "name": "pwm1",
+      "id": "[g,f6Mp!O$eZPCFs0U[H"
+    },
+    {
+      "name": "odometry",
+      "id": "ju{xs[rjZumqS87$0nhu"
+    },
+    {
+      "name": "valX",
+      "id": "Ug!mIa*[PnsL?H#9Ar*G"
     }
   ]
 }