🚀
micro-ROS for Arduinoを使ってAtomLiteから環境センサのデータを送信する
はじめに
前回のつづき。Atom LiteにENV III Unitを接続して得られた環境データをRaspberry Piに送ってみます。
Atom Lite側の開発
Atom LiteとENV III UnitをGroveケーブルで接続します。
以下をArduino IDEで書き込みます。
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/float32.h>
#include <M5Atom.h>
#include "M5UnitENV.h"
rcl_publisher_t publisher;
//std_msgs__msg__Int32 msg;
std_msgs__msg__Float32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
#define LED_PIN 13
// ENV3 Unit
SHT3X sht3x;
QMP6988 qmp6988;
float_t tmp = 0.0;
float_t hum = 0.0;
float_t pressure = 0.0;
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
void error_loop(){
while(1){
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
msg.data = tmp;
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
//msg.data++;
}
}
void setup() {
Serial.begin(115200);
M5.begin(true, false, true);
if (!qmp6988.begin(&Wire, QMP6988_SLAVE_ADDRESS_L, 26, 32, 400000U)) {
Serial.println("Couldn't find QMP6988");
while (1) delay(1);
}
if (!sht3x.begin(&Wire, SHT3X_I2C_ADDR, 26, 32, 400000U)) {
Serial.println("Couldn't find SHT3X");
while (1) delay(1);
}
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
delay(2000);
allocator = rcl_get_default_allocator();
//create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
//ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
"micro_ros_arduino_node_publisher"));
// create timer,
//const unsigned int timer_timeout = 1000;
const unsigned int timer_timeout = 10;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
msg.data = 0;
}
void loop() {
Serial.println(sht3x.update());
pressure = qmp6988.calcPressure();
if (sht3x.update()) {
tmp = sht3x.cTemp;
hum = sht3x.humidity;
} else {
tmp = 0, hum = 0;
}
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
プログラム的には変数tmpに温度、humに湿度、pressureに気圧を取得していますが、とりあえずtmpのみをpublishすることにしました。温度データがFloat型のデータになるのでメッセージ型をstd_msgs__msg__Float32 msg;としています。
Raspberry Pi側の設定
以下でmicro-ROS Agentを起動します。今回はUSBで待ち受けます。
cd ros2_ws
source install/setup.sh
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -v6
以下のように /dev/ttyUSB0が未接続でコネクション待ちのメッセージが繰り返されます
[1718025258.565083] info | TermiosAgentLinux.cpp | init | Serial port not found. | device: /dev/ttyUSB0, error 2, waiting for connection...
[1718025259.571867] info | TermiosAgentLinux.cpp | init | Serial port not found. | device: /dev/ttyUSB0, error 2, waiting for connection...
[1718025260.578252] info | TermiosAgentLinux.cpp | init | Serial port not found. | device: /dev/ttyUSB0, error 2, waiting for connection...
USBケーブルでAtom LiteとRaspberry Piを接続します。接続すると以下のようにデータが受信されるようになります。
[1718025328.993929] info | TermiosAgentLinux.cpp | init | running... | fd: 3
[1718025328.995267] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 6
[1718025330.821128] info | Root.cpp | create_client | create | client_key: 0x6B7DABB7, session_id: 0x81
[1718025330.823238] info | SessionManager.hpp | establish_session | session established | client_key: 0x6B7DABB7, address: 0
[1718025330.823796] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x6B7DABB7, len: 19, data:
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1718025330.836347] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x6B7DABB7, len: 56, data:
0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 1F 00 00 00 00 01 A5 A5 17 00 00 00 6D 69 63 72
0020: 6F 5F 72 6F 73 5F 61 72 64 75 69 6E 6F 5F 6E 6F 64 65 00 00 00 00 00 00
[1718025330.932411] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x6B7DABB7, len: 13, data:
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1718025330.942091] info | ProxyClient.cpp | create_participant | participant created | client_key: 0x6B7DABB7, participant_id: 0x000(1)
[1718025330.942417] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x6B7DABB7, len: 14, data:
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1718025330.942578] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x6B7DABB7, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1718025330.942623] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x6B7DABB7, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
ターミナルで別タブを開いて、トピックリストを表示します
ros2 topic list
/micro_ros_arduino_node_publisher
/parameter_events
/rosout
/micro_ros_arduino_node_publisherをechoしてみます
ros2 topic echo /micro_ros_arduino_node_publisher
data: 27.256427764892578
---
data: 27.269779205322266
---
data: 27.269779205322266
---
data: 27.256427764892578
---
data: 27.269779205322266
温度データが受信できているのを確認できました。デバイスはこんな感じです。
Subscriberを作ってみる
せっかくなのでRaspberry Pi側に簡単なSubscriberを作ってデータを購読してみます。公式のWriting a simple publisher and subscriber (Python)を参考にします。
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 simple_subscriber
cd simple_subscriber/simple_subscriber
vi subscriber_member_function.py
以下のコードをsubscriber_member_function.pyとして保存します。
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
class MinimalSubscriber(Node):
def __init__(self):
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(
Float32,
'micro_ros_arduino_node_publisher',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
def listener_callback(self, msg):
self.get_logger().info('%.3f °C' % msg.data)
def main(args=None):
rclpy.init(args=args)
minimal_subscriber = MinimalSubscriber()
rclpy.spin(minimal_subscriber)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
setup.pyにSubscriberのentory_pointを追加します。
cd ..
vi setup.py
...
entry_points={
'console_scripts': [
'listener = simple_subscriber.subscriber_member_function:main',
],
},
...
パッケージをビルドします。
cd ../.. # /home/pi/ros2_ws
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select simple_subscriber
実行します。
source install/setup.bash
ros2 run simple_subscriber listener
Subscriberが起動し以下のようにAtom Liteからの温度データが表示されます。
[INFO] [1718026651.817102808] [minimal_subscriber]: 28.204 °C
[INFO] [1718026652.360541217] [minimal_subscriber]: 28.204 °C
[INFO] [1718026652.968466967] [minimal_subscriber]: 28.175 °C
[INFO] [1718026653.576770411] [minimal_subscriber]: 28.175 °C
[INFO] [1718026654.184855135] [minimal_subscriber]: 28.204 °C
...
おわりに
前回でAtom LiteからメッセージがRaspberry Piで受信できるのを確認できていましたが、センサーデータを実際に受信できているのをみるとまたちょっと違ったおもしろさを感じられました。ロボットにどんなセンサーをつけて走らせるといいかとか。想像が膨らみますね。
Discussion