feat: add AS5600 magnetic encoder node with I2C support; implement message handling and integration tests
parent
c23e1471b2
commit
89a12c85cc
|
|
@ -19,6 +19,7 @@ See [readme.md](readme.md) for project overview and status.
|
|||
| `blockly_interfaces` — ROS2 action & message interfaces | [src/blockly_interfaces/README.md](src/blockly_interfaces/README.md) |
|
||||
| `gpio_node` — Raspberry Pi GPIO node (C++, libgpiod) | [src/gpio_node/](src/gpio_node/) |
|
||||
| `pca9685_node` — PCA9685 16-channel PWM controller (C++, I2C) | [src/pca9685_node/](src/pca9685_node/) |
|
||||
| `as5600_node` — AS5600 12-bit magnetic encoder (C++, I2C) | [src/as5600_node/](src/as5600_node/) |
|
||||
|
||||
---
|
||||
|
||||
|
|
@ -42,11 +43,13 @@ pixi run app # Terminal 2 — start desktop GUI
|
|||
pixi install # install ROS2 + deps via conda
|
||||
pixi run build-gpio # installs system deps (apt) + builds gpio_node
|
||||
pixi run build-pca9685 # installs system deps (apt) + builds pca9685_node
|
||||
pixi run build-as5600 # installs system deps (apt) + builds as5600_node
|
||||
|
||||
pixi run gpio-node # start GPIO node
|
||||
pixi run pca9685-node # start PCA9685 PWM node
|
||||
pixi run as5600-node # start AS5600 encoder node
|
||||
```
|
||||
|
||||
`build-gpio` and `build-pca9685` automatically run `setup-dep` which installs system libraries (`libgpiod-dev`, `liblttng-ust-dev`, `i2c-tools`) via `apt`.
|
||||
`build-gpio`, `build-pca9685`, and `build-as5600` automatically run `setup-dep` which installs system libraries (`libgpiod-dev`, `liblttng-ust-dev`, `i2c-tools`) via `apt`.
|
||||
|
||||
See [docs/installation.md](docs/installation.md) for full setup and prerequisites.
|
||||
|
|
|
|||
|
|
@ -50,4 +50,6 @@ setup-dep = { cmd = "sudo apt update && sudo apt install -y liblttng-ust-
|
|||
build-gpio = { cmd = "colcon build --symlink-install --packages-select gpio_node", depends-on = ["setup-dep", "build-interfaces"] }
|
||||
gpio-node = { cmd = "bash -c 'source install/setup.bash && ros2 run gpio_node gpio_node'", depends-on = ["build-gpio"] }
|
||||
build-pca9685 = { cmd = "colcon build --symlink-install --packages-select pca9685_node", depends-on = ["setup-dep", "build-interfaces"] }
|
||||
pca9685-node = { cmd = "bash -c 'source install/setup.bash && ros2 run pca9685_node pca9685_node'", depends-on = ["build-pca9685"] }
|
||||
pca9685-node = { cmd = "bash -c 'source install/setup.bash && ros2 run pca9685_node pca9685_node'", depends-on = ["build-pca9685"] }
|
||||
build-as5600 = { cmd = "colcon build --symlink-install --packages-select as5600_node", depends-on = ["setup-dep", "build-interfaces"] }
|
||||
as5600-node = { cmd = "bash -c 'source install/setup.bash && ros2 run as5600_node as5600_node'", depends-on = ["build-as5600"] }
|
||||
129
readme.md
129
readme.md
|
|
@ -3,7 +3,7 @@
|
|||
> **Project**: Blockly ROS2 Robot Controller (Kiwi Wheel AMR)
|
||||
> **ROS2 Distro**: Jazzy
|
||||
> **Last Updated**: 2026-03-16
|
||||
> **Current Focus**: Task #3 — PCA9685 PWM Controller (I2C)
|
||||
> **Current Focus**: Task #4 — AS5600 Magnetic Encoder (I2C)
|
||||
|
||||
Dokumentasi lengkap dapat dilihat di [DOCUMENTATION.md](DOCUMENTATION.md).
|
||||
|
||||
|
|
@ -36,9 +36,9 @@ jelaskan apa yang dimaksut untuk menyelesaikan task
|
|||
|
||||
# Potential Enhancements
|
||||
this list is short by priority
|
||||
- **AS5600 feature**: AS5600 magnetic based encoder with I2C interface. lets make publisher node in C/C++ to be able pusblih 3 module encoder. use parameter ins ros2 command to chose linux dev interface.
|
||||
- **Feasibility Study to implement Adaptive Controller**: mobile robot need controller to move flawlesly.
|
||||
- **Launch files**: `blockly_bringup` package with ROS2 launch files to start all nodes with one command
|
||||
- **Blockly UI Enhancement**: Lets make Human Interface in same view to help me monitoring node that needed. it programaticaly using block is real good, you can take labview interface as refrence. you can separated program as main_program to handle human interface.
|
||||
- **Feasibility Study to implement Controller**: mobile robot need controller to move flawlesly.
|
||||
- **Launch files**: ROS2 launch files to start all nodes with one command includ node in raspberry pi
|
||||
- **Simulation**: Integrate with Gazebo/Isaac Sim for testing Kiwi Wheel kinematics before deploying to hardware
|
||||
- **Block categories**: Future blocks grouped into Robot, Sensors, Navigation categories
|
||||
|
||||
|
|
@ -208,6 +208,23 @@ Lazy-create publisher di `hardware.node._pwm_write_pub`, sama dengan pola `gpio.
|
|||
|
||||
Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel.
|
||||
|
||||
#### G. Penggunaan
|
||||
|
||||
```bash
|
||||
# Default — /dev/i2c-1, 50 Hz
|
||||
pixi run pca9685-node
|
||||
|
||||
# Ganti I2C device dan frequency via --ros-args
|
||||
source install/setup.bash
|
||||
ros2 run pca9685_node pca9685_node --ros-args -p i2c_device:=/dev/i2c-0 -p frequency:=1000
|
||||
|
||||
# Cek I2C bus yang tersedia di Pi
|
||||
ls /dev/i2c-* # list semua bus
|
||||
i2cdetect -y 1 # scan device di bus 1 (perlu i2c-tools)
|
||||
```
|
||||
|
||||
**Catatan**: `pixi run pca9685-node` menggunakan parameter default. Untuk override parameter, jalankan `ros2 run` langsung (setelah `source install/setup.bash`) karena pixi task tidak meneruskan `--ros-args` ke proses inner.
|
||||
|
||||
### Definition Of Done
|
||||
- [x] `src/pca9685_node/` berisi `CMakeLists.txt`, `package.xml`, `include/`, `src/`
|
||||
- [x] `blockly_interfaces/msg/PwmWrite.msg` terdaftar di `rosidl_generate_interfaces()`
|
||||
|
|
@ -218,3 +235,107 @@ Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel.
|
|||
- [x] Handler `pwm_write` berfungsi di dummy mode (test passes)
|
||||
- [x] Blockly block `pwmWrite` muncul di toolbox, generate valid JS code
|
||||
- [ ] 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.
|
||||
|
||||
### Implementasi
|
||||
|
||||
#### A. Package Structure (C++, ament_cmake)
|
||||
```
|
||||
src/as5600_node/
|
||||
├── CMakeLists.txt # ament_cmake — NO external lib dependency
|
||||
├── package.xml # depend: rclcpp, blockly_interfaces
|
||||
├── include/as5600_node/
|
||||
│ └── as5600_node.hpp # As5600Node class + I2C helpers
|
||||
└── src/
|
||||
├── as5600_node.cpp # I2C init, timer_callback, read_raw_angle()
|
||||
└── main.cpp # rclcpp::spin(node)
|
||||
```
|
||||
|
||||
Hardware interface menggunakan Linux I2C (`/dev/i2c-X`) via `ioctl()` — tidak perlu external library, cukup `linux/i2c-dev.h` (kernel header).
|
||||
|
||||
#### 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)
|
||||
```
|
||||
|
||||
**Topic**: `/encoder/state` (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 |
|
||||
|
||||
#### 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
|
||||
- `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
|
||||
|
||||
```bash
|
||||
# Default — /dev/i2c-1, 10 Hz, 1 encoder
|
||||
pixi run as5600-node
|
||||
|
||||
# 3 encoder pada bus terpisah, 20 Hz
|
||||
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
|
||||
```
|
||||
|
||||
### 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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,32 @@
|
|||
cmake_minimum_required(VERSION 3.10)
|
||||
project(as5600_node)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ROS2 dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(blockly_interfaces REQUIRED)
|
||||
|
||||
add_executable(as5600_node
|
||||
src/main.cpp
|
||||
src/as5600_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(as5600_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
ament_target_dependencies(as5600_node
|
||||
rclcpp
|
||||
blockly_interfaces
|
||||
)
|
||||
|
||||
install(TARGETS as5600_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef AS5600_NODE__AS5600_NODE_HPP_
|
||||
#define AS5600_NODE__AS5600_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include "blockly_interfaces/msg/encoder_read.hpp"
|
||||
|
||||
class As5600Node : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
As5600Node();
|
||||
~As5600Node() override;
|
||||
|
||||
private:
|
||||
void open_i2c_buses(const std::vector<std::string> & device_paths);
|
||||
uint16_t read_raw_angle(int fd);
|
||||
void timer_callback();
|
||||
|
||||
std::vector<int> i2c_fds_;
|
||||
|
||||
// AS5600 fixed I2C address
|
||||
static constexpr uint8_t AS5600_ADDR = 0x36;
|
||||
|
||||
// AS5600 register addresses
|
||||
static constexpr uint8_t REG_RAW_ANGLE_H = 0x0C;
|
||||
static constexpr uint8_t REG_STATUS = 0x0B;
|
||||
|
||||
rclcpp::Publisher<blockly_interfaces::msg::EncoderRead>::SharedPtr encoder_pub_;
|
||||
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||
};
|
||||
|
||||
#endif // AS5600_NODE__AS5600_NODE_HPP_
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>as5600_node</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS2 AS5600 magnetic encoder node — 12-bit rotary position via Linux i2c-dev (C++)</description>
|
||||
<maintainer email="dev@example.com">developer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>blockly_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
#include "as5600_node/as5600_node.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <stdexcept>
|
||||
|
||||
As5600Node::As5600Node()
|
||||
: Node("as5600_node")
|
||||
{
|
||||
// Declare ROS2 parameters
|
||||
this->declare_parameter("i2c_devices", std::vector<std::string>{"/dev/i2c-1"});
|
||||
this->declare_parameter("publish_rate", 10.0);
|
||||
|
||||
auto device_paths = this->get_parameter("i2c_devices").as_string_array();
|
||||
double rate = this->get_parameter("publish_rate").as_double();
|
||||
|
||||
// Open all I2C buses
|
||||
open_i2c_buses(device_paths);
|
||||
|
||||
// Create publisher for /encoder/state
|
||||
encoder_pub_ = this->create_publisher<blockly_interfaces::msg::EncoderRead>(
|
||||
"/encoder/state", 10);
|
||||
|
||||
// Create timer for periodic reads
|
||||
auto period = std::chrono::duration<double>(1.0 / rate);
|
||||
read_timer_ = this->create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
|
||||
std::bind(&As5600Node::timer_callback, this));
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"As5600Node ready — %zu encoder(s), publish_rate=%.1f Hz",
|
||||
i2c_fds_.size(), rate);
|
||||
}
|
||||
|
||||
As5600Node::~As5600Node()
|
||||
{
|
||||
for (int fd : i2c_fds_) {
|
||||
if (fd >= 0) {
|
||||
close(fd);
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "I2C devices closed");
|
||||
}
|
||||
|
||||
void As5600Node::open_i2c_buses(const std::vector<std::string> & device_paths)
|
||||
{
|
||||
for (size_t i = 0; i < device_paths.size(); ++i) {
|
||||
int fd = open(device_paths[i].c_str(), O_RDWR);
|
||||
if (fd < 0) {
|
||||
throw std::runtime_error(
|
||||
"Failed to open I2C device: " + device_paths[i]);
|
||||
}
|
||||
|
||||
// Set slave address once — each fd is dedicated to one AS5600
|
||||
if (ioctl(fd, I2C_SLAVE, AS5600_ADDR) < 0) {
|
||||
close(fd);
|
||||
throw std::runtime_error(
|
||||
"Failed to set I2C slave address 0x36 on " + device_paths[i]);
|
||||
}
|
||||
|
||||
i2c_fds_.push_back(fd);
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Encoder %zu opened on %s (addr=0x%02X)",
|
||||
i, device_paths[i].c_str(), AS5600_ADDR);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t As5600Node::read_raw_angle(int fd)
|
||||
{
|
||||
// AS5600 RAW_ANGLE register: 0x0C (high byte) + 0x0D (low byte)
|
||||
// Write register address, then read 2 bytes
|
||||
uint8_t reg = REG_RAW_ANGLE_H;
|
||||
if (write(fd, ®, 1) != 1) {
|
||||
RCLCPP_ERROR(this->get_logger(), "I2C register select failed");
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t buf[2] = {0, 0};
|
||||
if (read(fd, buf, 2) != 2) {
|
||||
RCLCPP_ERROR(this->get_logger(), "I2C read failed");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 12-bit value: high nibble of buf[0] + full buf[1]
|
||||
return ((buf[0] & 0x0F) << 8) | buf[1];
|
||||
}
|
||||
|
||||
void As5600Node::timer_callback()
|
||||
{
|
||||
for (size_t i = 0; i < i2c_fds_.size(); ++i) {
|
||||
uint16_t raw = read_raw_angle(i2c_fds_[i]);
|
||||
float angle = static_cast<float>(raw) * 360.0f / 4096.0f;
|
||||
|
||||
blockly_interfaces::msg::EncoderRead msg;
|
||||
msg.encoder_id = static_cast<uint8_t>(i);
|
||||
msg.raw_angle = raw;
|
||||
msg.angle = angle;
|
||||
encoder_pub_->publish(msg);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "as5600_node/as5600_node.hpp"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<As5600Node>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
|
||||
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];
|
||||
}
|
||||
});
|
||||
|
|
@ -14,4 +14,5 @@ const BLOCK_FILES = [
|
|||
'digitalIn.js',
|
||||
'delay.js',
|
||||
'pwmWrite.js',
|
||||
'encoderRead.js',
|
||||
];
|
||||
|
|
|
|||
|
|
@ -0,0 +1,55 @@
|
|||
"""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")
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
"""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
|
||||
|
|
@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||
"msg/GpioWrite.msg"
|
||||
"msg/GpioRead.msg"
|
||||
"msg/PwmWrite.msg"
|
||||
"msg/EncoderRead.msg"
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,3 @@
|
|||
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)
|
||||
Loading…
Reference in New Issue