a2nr
/
amr-ros-k4
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

master
a2nr 2026-03-17 21:07:05 +07:00
parent 0f62045260
commit 8a9f8771d7
16 changed files with 295 additions and 315 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
readme.md
View File

@ -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_<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)
`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_<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
- `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_<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 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

2
src/as5600_node/CMakeLists.txt
View File

@ -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
)

3
src/as5600_node/include/as5600_node/as5600_node.hpp
View File

@ -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_;

1
src/as5600_node/package.xml
View File

@ -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>

11
src/as5600_node/src/as5600_node.cpp
View File

@ -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_) {

26
src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js
View File

@ -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];
}
});

2
src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js
View File

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

35
src/blockly_app/blockly_app/ui/blockly/blocks/odometryGet.js Normal file
View File

@ -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];
}
});

22
src/blockly_app/blockly_app/ui/blockly/blocks/odometryRead.js
View File

@ -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];
}
});

55
src/blockly_executor/blockly_executor/handlers/encoder.py
View File

@ -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")

24
src/blockly_executor/blockly_executor/handlers/odometry.py
View File

@ -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))

30
src/blockly_executor/test/test_block_encoder.py
View File

@ -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

71
src/blockly_executor/test/test_block_odometry.py
View File

@ -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

1
src/blockly_interfaces/CMakeLists.txt
View File

@ -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)

3
src/blockly_interfaces/msg/EncoderRead.msg
View File

@ -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)

174
workspace.json
View File

@ -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"
}
]
}