diff --git a/readme.md b/readme.md index 1d1aea8..60bc379 100644 --- a/readme.md +++ b/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 -├── package.xml # depend: rclcpp, blockly_interfaces +├── CMakeLists.txt # ament_cmake — depend: rclcpp, nav_msgs, geometry_msgs +├── 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`: -``` -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) -``` +Menggunakan **`nav_msgs/Odometry`** (standar ROS2) — lihat Task #5 untuk detail fields dan kinematics. -**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` -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 +#### C. pixi.toml - `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. - -#### G. Penggunaan +#### D. 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 - -# Raspberry Pi: enable extra I2C buses via config.txt -# 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 + -p publish_rate:=20.0 \ + -p wheel_radius:=5.0 \ + -p wheel_distance:=15.0 ``` ### 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` -- [ ] Parameter `i2c_devices`, `publish_rate` berfungsi via `--ros-args -p` -- [x] Handler `encoder_read` berfungsi di dummy mode (test passes) -- [x] Blockly block `encoderRead` muncul di toolbox, generate valid JS code -- [ ] End-to-end: Blockly block → executor (real) → cache `/encoder/state` → return angle +- [ ] Node berjalan: `pixi run as5600-node` — publish `odometry_encoder/odom` +- [ ] Parameter `i2c_devices`, `publish_rate`, `wheel_radius`, `wheel_distance` berfungsi via `--ros-args -p` +- [ ] End-to-end: as5600_node → `odometry_encoder/odom` → executor cache → Blockly ## 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_/odom` menggunakan message type yang sama. -**Motivasi**: Modularitas — tambah sensor baru cukup buat node yang publish `nav_msgs/Odometry` ke `odometry_/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_/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) -`as5600_node` menghitung robot-level odometry langsung dari 3 encoder menggunakan kiwi wheel forward kinematics, lalu publish ke **dua** topic: -- `/encoder/state` (`EncoderRead`) — legacy per-wheel angle, backward compat -- `odometry_encoder/odom` (`nav_msgs/Odometry`) — robot-level pose + twist +#### B. AS5600 Node — Kiwi Wheel Kinematics +`as5600_node` menghitung robot-level odometry langsung dari 3 encoder menggunakan kiwi wheel forward kinematics, publish ke `odometry_encoder/odom` (`nav_msgs/Odometry`). + +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`): -| 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` +#### C. Handler — `odometry.py` (returns JSON) ```python @handler("odometry_read") def handle_odometry_read(params, hardware): - source = params.get("source", "encoder") # "encoder" | "imu" | "optical" - field = params["field"] # "x" | "y" | "heading" | "vx" | "vy" | "omega_z" + source = params.get("source", "encoder") + # 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_/odom`. Cache robot-level fields dengan quaternion → yaw conversion. +Lazy-create subscriber per source ke `odometry_/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 -- `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 +**Block 2: `getValueOdometry`** (`odometryGet.js`) — Value block, extract field (no action call): +``` +┌───────────────────────────────────────────────┐ +│ 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_/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_/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 value -- [x] Backward compat: `encoderRead` block dan `encoder_read` handler tetap berfungsi +- [ ] End-to-end: Blockly → executor (real) → cache `odometry_encoder/odom` → return JSON - [x] Integration test `test_block_odometry.py` passes di dummy mode diff --git a/src/as5600_node/CMakeLists.txt b/src/as5600_node/CMakeLists.txt index ee54a9e..0ca8e8e 100644 --- a/src/as5600_node/CMakeLists.txt +++ b/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 ) diff --git a/src/as5600_node/include/as5600_node/as5600_node.hpp b/src/as5600_node/include/as5600_node/as5600_node.hpp index 67509e7..ee28479 100644 --- a/src/as5600_node/include/as5600_node/as5600_node.hpp +++ b/src/as5600_node/include/as5600_node/as5600_node.hpp @@ -7,7 +7,6 @@ #include -#include "blockly_interfaces/msg/encoder_read.hpp" #include 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::SharedPtr encoder_pub_; // Odometry publisher (nav_msgs/Odometry on odometry_encoder/odom) rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::TimerBase::SharedPtr read_timer_; diff --git a/src/as5600_node/package.xml b/src/as5600_node/package.xml index 873d847..9a55df9 100644 --- a/src/as5600_node/package.xml +++ b/src/as5600_node/package.xml @@ -10,7 +10,6 @@ ament_cmake rclcpp - blockly_interfaces nav_msgs geometry_msgs diff --git a/src/as5600_node/src/as5600_node.cpp b/src/as5600_node/src/as5600_node.cpp index dc938a7..3eacdf4 100644 --- a/src/as5600_node/src/as5600_node.cpp +++ b/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( - "/encoder/state", 10); - // Create odometry publisher on odometry_encoder/odom (nav_msgs/Odometry) odom_pub_ = this->create_publisher( "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(i); - enc_msg.raw_angle = raw; - enc_msg.angle = static_cast(raw) * 360.0f / 4096.0f; - encoder_pub_->publish(enc_msg); } if (first_reading_) { diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js b/src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js deleted file mode 100644 index 05e323d..0000000 --- a/src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js +++ /dev/null @@ -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]; - } -}); diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js b/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js index f6a9110..b291170 100644 --- a/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js +++ b/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', ]; diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/odometryGet.js b/src/blockly_app/blockly_app/ui/blockly/blocks/odometryGet.js new file mode 100644 index 0000000..b23e1a6 --- /dev/null +++ b/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]; + } +}); diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/odometryRead.js b/src/blockly_app/blockly_app/ui/blockly/blocks/odometryRead.js index a70981e..9323def 100644 --- a/src/blockly_app/blockly_app/ui/blockly/blocks/odometryRead.js +++ b/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') - .appendField('field:') - .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'); + ]), 'SOURCE'); + this.setOutput(true, null); 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]; } }); diff --git a/src/blockly_executor/blockly_executor/handlers/encoder.py b/src/blockly_executor/blockly_executor/handlers/encoder.py deleted file mode 100644 index 62eb902..0000000 --- a/src/blockly_executor/blockly_executor/handlers/encoder.py +++ /dev/null @@ -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") diff --git a/src/blockly_executor/blockly_executor/handlers/odometry.py b/src/blockly_executor/blockly_executor/handlers/odometry.py index 4ff5a8b..97fb08f 100644 --- a/src/blockly_executor/blockly_executor/handlers/odometry.py +++ b/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" - lock_attr = "_odometry_lock" - subs_attr = "_odometry_subs" - - if not hasattr(hardware.node, cache_attr): + if not hasattr(hardware.node, "_odometry_cache"): 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}, field={field})") + hardware.log(f"odometry_read(source={source})") + + 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: - return (True, "0.0") + if cached is not None: + data = cached - value = data.get(field, 0.0) - return (True, str(value)) - - # Dummy mode — return 0.0 - return (True, "0.0") + return (True, json.dumps(data)) diff --git a/src/blockly_executor/test/test_block_encoder.py b/src/blockly_executor/test/test_block_encoder.py deleted file mode 100644 index 9963ee6..0000000 --- a/src/blockly_executor/test/test_block_encoder.py +++ /dev/null @@ -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 diff --git a/src/blockly_executor/test/test_block_odometry.py b/src/blockly_executor/test/test_block_odometry.py index aac22c6..f7d3e6f 100644 --- a/src/blockly_executor/test/test_block_odometry.py +++ b/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): - result = exe_action("odometry_read", source="encoder", field="x") +def test_odometry_read_returns_json(exe_action): + """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) - assert value == 0.0 # Dummy mode returns 0.0 + data = json.loads(result.result.message) + 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): - result = exe_action("odometry_read", source="encoder", field="y") - assert result.result.success is True - assert float(result.result.message) == 0.0 - - -def test_odometry_read_heading(exe_action): - result = exe_action("odometry_read", source="encoder", field="heading") - assert result.result.success is True - assert float(result.result.message) == 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_dummy_values(exe_action): + """Dummy mode returns all zeros.""" + result = exe_action("odometry_read", source="encoder") + data = json.loads(result.result.message) + assert data["x"] == 0.0 + assert data["y"] == 0.0 + assert data["heading"] == 0.0 + assert data["vx"] == 0.0 + assert data["vy"] == 0.0 + assert data["omega_z"] == 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 diff --git a/src/blockly_interfaces/CMakeLists.txt b/src/blockly_interfaces/CMakeLists.txt index 6a7a9c1..8b69443 100644 --- a/src/blockly_interfaces/CMakeLists.txt +++ b/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) diff --git a/src/blockly_interfaces/msg/EncoderRead.msg b/src/blockly_interfaces/msg/EncoderRead.msg deleted file mode 100644 index 6ee0d92..0000000 --- a/src/blockly_interfaces/msg/EncoderRead.msg +++ /dev/null @@ -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) diff --git a/workspace.json b/workspace.json index 05d3c68..dc1802f 100644 --- a/workspace.json +++ b/workspace.json @@ -5,23 +5,20 @@ { "type": "main_program", "id": "COLVqmFP{j*XNMc.9rz+", - "x": 210, - "y": 190, + "x": -50, + "y": -30, "inputs": { "BODY": { "block": { - "type": "controls_whileUntil", - "id": "UZH|b`4F=4X`zYjNW9zL", - "fields": { - "MODE": "WHILE" - }, + "type": "controls_repeat_ext", + "id": "l?jsj;kx|7.`vK%OX$hB", "inputs": { - "BOOL": { - "block": { - "type": "logic_boolean", - "id": "nsD(~e:hYj[+9m-)yu-4", + "TIMES": { + "shadow": { + "type": "math_number", + "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, - "y": 210, + "x": 330, + "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" } ] } \ No newline at end of file