前回、BME280センサで測定した温度をPublishしました。BME280は三つのデータを同時に取得できるので、温度、湿度、気圧を全部Pubulishします。
環境
- Raspberry Pi 5 8GB
- 追加ボード；NVMe Base for Raspberry Pi 5 (NVMe Base by Pimoroni)
- Crucial クルーシャル P2シリーズ 500GB 3D NAND NVMe PCIe M.2 SSD CT500P2SSD8
- 初期；RaspberryPi OS Desktop 64bit (Debian version: 12 (bookworm) Release date: March 15th 2024)
- 現在；Ubuntu Desktop 24.04LTS(64-bit)
- ROS2 HumbleではなくJazzy
Windows10で、検索窓にcmdと入れ、コマンドプロンプトを起動します。
sshでログインします（第１回参照）。必要ならupdateします。
C:\Users\yoshi>ssh yoshi.local
温度、湿度、気圧を取得するプログラム
yoshi@yoshi:~/ros2_ws$ nano works/src/bme280/read3data.cpp
read3data.cpp
#include <iostream>
#include <fstream> // ファイル入出力用
#include <cstdlib> // exit()用
using namespace std;
std::tuple<float, float, float> read3data(){
ifstream ifs; // ifstream の変数（インスタンス）を生成．
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_temp_input");
// ファイルを開く．fopen() に相当
if( ! ifs ) { // ファイルが開けない場合の処理
cerr << "File open error ! " << endl;
exit(1);
}
char dataT[32]; // データを読み込むバッファ
ifs >> dataT; // ifs から入力
float temp = stof(dataT);
ifs.close();
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_pressure_input");
if( ! ifs ) {
cerr << "File open error ! " << endl;
exit(1);
}
char dataP[32];
ifs >> dataP;
float press = stof(dataP);
ifs.close();
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_humidityrelative_input");
if( ! ifs ) {
cerr << "File open error ! " << endl;
exit(1);
}
char dataH[32];
ifs >> dataH;
float humi = stof(dataH);
ifs.close();
return std::make_tuple((float)(temp/1000.0), (float)(press*10.0), (float)(humi/1000.0));
}
int main(void)
{
auto [temp, press, humi] = read3data();
cout << temp;
cout << "\n" << press;
cout << "\n" << humi;
auto datas = read3data();
cout << std::get<0>(datas);
cout << "\n";
cout << std::get<1>(datas);
cout << "\n";
cout << std::get<2>(datas);
cout << "\n";
return 0;
}
コンパイルします。実行すると、温度、気圧、湿度を表示します。出力の方法は二通りを試しています。
yoshi@yoshi:~/ros2_ws$ g++ -o out works/src/bme280/read3data.cpp
yoshi@yoshi:~/ros2_ws$ ./out
27.78
1007.58
55.206
27.78
1007.58
55.206
温度、気圧、湿度をpublishする
ディレクトリ構造です。srcのなかに、bme280ディレクトリを作り、bme280.cppでプログラムを保存されています。新しいプログラムは、前回のbmp280.cppに、read3data()関数を組み込んだ形にし、ファイル名はbmp280_3.cppにしました。
$ cd ros2_ws
tree
└── works
├── CMakeLists.txt
├── include
│ └── works
├── package.xml
└── src
├── bme280
│ ├── bme280.cpp
│ ├── read3data.cpp
│ └── readTemp.cpp
├── exec_sample
│ └── main.c
├── pub
│ └── pub.cpp
└── sub
└── sub.cpp
yoshi@yoshi:~/ros2_ws$ mkdir works/src/bme280
yoshi@yoshi:~/ros2_ws$ nano works/src/bme280/bme280_3.cpp
bme280_3.cpp
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <stdio.h>
#include <iostream>
#include <fstream> // ファイル入出力用
#include <cstdlib> // exit()用
using namespace std;
std::tuple<float, float, float> read3data(){
ifstream ifs; // ifstream の変数（インスタンス）を生成．
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_temp_input");
// ファイルを開く．fopen() に相当
if( ! ifs ) { // ファイルが開けない場合の処理
cerr << "File open error ! " << endl;
exit(1);
}
char dataT[32]; // データを読み込むバッファ
ifs >> dataT; // ifs から入力
float temp = stof(dataT);
ifs.close();
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_pressure_input");
if( ! ifs ) {
cerr << "File open error ! " << endl;
exit(1);
}
char dataP[32];
ifs >> dataP;
float press = stof(dataP);
ifs.close();
ifs.open("/sys/bus/i2c/devices/i2c-1/1-0077/iio:device0/in_humidityrelative_input");
if( ! ifs ) {
cerr << "File open error ! " << endl;
exit(1);
}
char dataH[32];
ifs >> dataH;
float humi = stof(dataH);
ifs.close();
return std::make_tuple((float)(temp/1000.0), (float)(press*10.0), (float)(humi/1000.0));
}
int main(int argc, char **argv)
{
printf("start publish bme280_3\n");
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("publish_bme280_3data");
auto publisher = node->create_publisher<std_msgs::msg::String>("greeting", 1);
rclcpp::WallRate loop(1);
while (rclcpp::ok()) {
auto msg = std_msgs::msg::String();
auto datas = read3data();
msg.data = "Temp, Press, Humi " + std::to_string(std::get<0>(datas)) + ","+ std::to_string(std::get<1>(datas))+ ","+ std::to_string(std::get<2>(datas));
publisher->publish(msg);
loop.sleep();
}
rclcpp::shutdown();
return 0;
}
CMakeLists.txtを修正します。
yoshi@yoshi:~/ros2_ws$ nano works/CMakeLists.txt
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(works)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(sub src/sub/sub.cpp)
ament_target_dependencies(sub rclcpp std_msgs)
install(
TARGETS sub
DESTINATION lib/${PROJECT_NAME}
)
add_executable(bme280_3 src/bme280/bme280_3.cpp)
ament_target_dependencies(bme280_3 rclcpp std_msgs)
install(
TARGETS bme280_3
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
source /opt/ros/jazzy/setup.bash
colcon buildでビルドしたものを登録します。subscriberのsubを起動します。
yoshi@yoshi:~/ros2_ws$ source ~/ros2_ws/install/setup.bash
yoshi@yoshi:~/ros2_ws$ ros2 run works sub
もう一つターミナル（Windows10上）を立ち上げます。publisherのbme280_3を起動します。
ssh yoshi.local
yoshi@yoshi:~/ros2_ws$ source ~/ros2_ws/install/setup.bash
yoshi@yoshi:~/ros2_ws$ ros2 run works bme280_3
start publish bme280
subが動いている最初のターミナルの様子です。実行中にBME280を指でつまんでいます。温度と湿度が変化しています。
yoshi@yoshi:~/ros2_ws$ ros2 run works sub
start subscribe test
[INFO] [1722197903.804267695] [yoshi_subscriber]: Temp, Press, Humi 26.480000,1007.604309,62.273998
[INFO] [1722197904.804259643] [yoshi_subscriber]: Temp, Press, Humi 26.490000,1007.584839,62.173000
[INFO] [1722197905.804244120] [yoshi_subscriber]: Temp, Press, Humi 26.490000,1007.566833,62.083000
[INFO] [1722197906.804258090] [yoshi_subscriber]: Temp, Press, Humi 26.490000,1007.587341,62.004002
[INFO] [1722197907.804295793] [yoshi_subscriber]: Temp, Press, Humi 26.799999,1007.545654,62.709000
[INFO] [1722197908.804249859] [yoshi_subscriber]: Temp, Press, Humi 28.730000,1007.588135,61.862999
[INFO] [1722197909.804247213] [yoshi_subscriber]: Temp, Press, Humi 29.629999,1007.627319,61.564999
[INFO] [1722197910.804302040] [yoshi_subscriber]: Temp, Press, Humi 30.260000,1007.604370,61.458000
[INFO] [1722197911.804279490] [yoshi_subscriber]: Temp, Press, Humi 30.730000,1007.547974,61.487000
[INFO] [1722197912.804299524] [yoshi_subscriber]: Temp, Press, Humi 31.049999,1007.585999,61.585999
[INFO] [1722197913.804243940] [yoshi_subscriber]: Temp, Press, Humi 31.280001,1007.607971,61.719002
[INFO] [1722197914.804251978] [yoshi_subscriber]: Temp, Press, Humi 31.480000,1007.517029,61.848999
[INFO] [1722197915.804258897] [yoshi_subscriber]: Temp, Press, Humi 31.340000,1007.393738,61.919998
[INFO] [1722197916.804258715] [yoshi_subscriber]: Temp, Press, Humi 30.830000,1007.540833,58.755001
[INFO] [1722197917.804300191] [yoshi_subscriber]: Temp, Press, Humi 30.680000,1007.546143,56.366001
└── works
├── CMakeLists.txt
├── include
│ └── works
├── package.xml
└── src
├── bme280
│ ├── bme280_3.cpp
│ ├── bme280.cpp
│ ├── read3data.cpp
│ └── readTemp.cpp
├── exec_sample
│ └── main.c
├── pub
│ └── pub.cpp
└── sub
└── sub.cpp
備忘録 ラズパイ5 ROS2
① ハードの用意とUbuntu Desktop 24.04LTS
② Ubuntu Desktop 24.04LTSでROS2環境 rqt_graphとturtlesim
③ Ubuntu Desktop 24.04LTSでROS2環境 Python その１ responder（セットアップ）
④ Ubuntu Desktop 24.04LTSでROS2環境 Pythonその２responder（コーディングと実行；失敗）
⑤ Ubuntu Desktop 24.04LTSでROS2環境 C++ その1 セットアップ main.c
⑥ Ubuntu Desktop 24.04LTSでROS2環境 C++ その2 セットアップ pub.cpp rqt_graph
⑦ Ubuntu Desktop 24.04LTSでROS2環境 C++ その3 セットアップ sub.cpp rqt_graph
⑧ Ubuntu Desktop 24.04LTSでROS2環境 C++ その4 bme280.cpp 温度
⑨ Ubuntu Desktop 24.04LTSでROS2環境 C++ その5 bme280_3.cpp 温度、湿度、気圧
⑩ Ubuntu Desktop 24.04LTSでROS2環境 C++ その6 VL53L1X.cpp 距離センサ