ToF距離センサのVL53L1XのPublisherが下記の記事で動きました。ここでは、Subscriberを作ります。
前回、Stringのメッセージを扱いましが、これは、たくさんの方々が取り上げているので、マネをすればいいのですが、センサ関係は、見つかってもIMUぐらいです。
で、ToFセンサの事例は検索しても全く今現在見つかっていません。なので、勉強しながらプログラムを組まないとならないという、初心者にはつらい時代です。センサ関連はROS1のころからあるようなので、歴史はあります。
Topicのmessageの内容
VL53L1XのPublisherがPublishしているプログラムの中から、Topicのメッセージの部分を見ます。
auto message = sensor_msgs::msg::Range();
message.header.frame_id = "vl53l1x";
message.header.stamp = now;
message.radiation_type = sensor_msgs::msg::Range::INFRARED;
message.field_of_view = 0.47; // Typically 27 degrees or 0,471239 radians
message.min_range = 0.14; // 140 mm. (It is actully much less, but this makes sense in the context
message.max_range = 3.00; // 3.6 m. in the dark, down to 73cm in bright light
message.range = (float) distance / 1000.0; // range in meters
センサが測定した距離は、rangeの値を読めば得られます。
sensor_msg
sensor_msgは、温度やIMUなど、たくさん定義されています。
ToFセンサは、sensor_msgs/msg/rangeです。
ros2のコマンドにはいくつもあります。VL53L1XのPublisherのパッケージを登録します。
今現在、setup.bashはエラーになって、パッケージの登録ができません。
yoshi@yoshi:~/ros2_ws$ source /opt/ros/jazzy/setup.bash
yoshi@yoshi:~/ros2_ws$ source ~/ros2_ws/install/setup.bash
パッケージを起動
yoshi@yoshi:~/ros2_ws$ ros2 run vl53l1x vl53l1x_node &
[1] 17323
今うごいているパッケージを表示
yoshi@yoshi:~/ros2_ws$ ros2 topic list
/parameter_events
/rosout
/vl53l1x/range
今うごいているnodeのリストを表示
yoshi@yoshi:~/ros2_ws$ ros2 node list
/VL53L1X_publisher
/rqt_gui_py_node_13309
指定したパッケージ（ノード）が出力しているTopicの内容を表示
yoshi@yoshi:~/ros2_ws$ ros2 topic echo vl53l1x/range
header:
stamp:
sec: 1723058463
nanosec: 348837093
frame_id: vl53l1x
radiation_type: 1
field_of_view: 0.4699999988079071
min_range: 0.14000000059604645
max_range: 3.0
range: 0.23399999737739563
variance: 0.0
---
header:
stamp:
sec: 1723058463
nanosec: 422800252
frame_id: vl53l1x
radiation_type: 1
field_of_view: 0.4699999988079071
min_range: 0.14000000059604645
max_range: 3.0
range: 0.23999999463558197
variance: 0.0
Rangeは、
一点の距離を測定した結果を格納するメッセージ。測定方法(IRや音)、視野角[rad]や最大・最低測定可能距離[m]、測定結果[m]を格納できる。ToF、超音波などの測距センサに使える。
とのことです。
ros2コマンドのinterface showで、詳細を表示します。
yoshi@yoshi:~/ros2_ws$ ros2 interface show sensor_msgs/msg/Range
# Single range reading from an active ranger that emits energy and reports
# one range reading that is valid along an arc at the distance measured.
# This message is not appropriate for laser scanners. See the LaserScan
# message if you are working with a laser scanner.
#
# This message also can represent a fixed-distance (binary) ranger. This
# sensor will have min_range===max_range===distance of detection.
# These sensors follow REP 117 and will output -Inf if the object is detected
# and +Inf if the object is outside of the detection range.
std_msgs/Header header # timestamp in the header is the time the ranger
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# returned the distance reading
# Radiation type enums
# If you want a value added to this list, send an email to the ros-users list
uint8 ULTRASOUND=0
uint8 INFRARED=1
uint8 radiation_type # the type of radiation used by the sensor
# (sound, IR, etc) [enum]
float32 field_of_view # the size of the arc that the distance reading is
# valid for [rad]
# the object causing the range reading may have
# been anywhere within -field_of_view/2 and
# field_of_view/2 at the measured range.
# 0 angle corresponds to the x-axis of the sensor.
float32 min_range # minimum range value [m]
float32 max_range # maximum range value [m]
# Fixed distance rangers require min_range==max_range
float32 range # range data [m]
# (Note: values < range_min or > range_max should be discarded)
# Fixed distance rangers only output -Inf or +Inf.
# -Inf represents a detection within fixed distance.
# (Detection too close to the sensor to quantify)
# +Inf represents no detection within the fixed distance.
# (Object out of range)
float32 variance # variance of the range sensor
# 0 is interpreted as variance unknown
rangeは、float32で、単位はm（メートル）だと書かれています。
Subscriberで、sensor_msgs/msg/Rangeを扱う方法は
VL53L1XのPublisherはありましたが、Subscriberは見つかっていません。今まで、Subscriberは、検索でみつかったプリミティブな事例をベースをちょこっと修正して利用していました。そのほとんどが、Topicは文字列で送っていました。
floatの配列で送る事例が見つかります。
sensor_msgs/msg/Rangeを扱う方法は見つかっていませんがよく似た事例があるので、何とかソースを修正しました。しかし、ここにきて何も勉強していないので、記述の仕方とかわかりません。
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <iostream>
#include "sensor_msgs/msg/range.hpp"
rclcpp::Node::SharedPtr g_node = nullptr;
void tof_callback(const sensor_msgs::msg::Range::SharedPtr msg)
{
RCLCPP_INFO(g_node->get_logger(), "%f m", msg->range);
}
int main(int argc, char **argv)
{
std::cout << "start subscribe VL53L1X vl53l1x/range\n";
rclcpp::init(argc, argv);
g_node = rclcpp::Node::make_shared("vl53l1x_subscriber");
auto subscriber = g_node->create_subscription<sensor_msgs::msg::Range>("vl53l1x/range", 1, tof_callback);
rclcpp::spin(g_node);
g_node = nullptr;
rclcpp::shutdown();
return 0;
}
CMakeLists.txtです。前回に比べてfind_package(sensor_msgs REQUIRED)が増えています。これは、ソースの#include "sensor_msgs/msg/range.hpp"に対応する記述です。
cmake_minimum_required(VERSION 3.8)
project(worksfirst)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)
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)
find_package(sensor_msgs REQUIRED)
add_executable(sub002 sub002.cpp)
ament_target_dependencies(sub002 rclcpp std_msgs sensor_msgs)
install(
TARGETS sub002
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
package.xmlに変更はありません。
ビルドして、動かします。
$ source /opt/ros/jazzy/setup.bash
$ cmake -S . -B build
$ cd build
$ make
$ ./sub002
start subscribe VL53L1X vl53l1x/range
[INFO] [1724699066.233789836] [vl53l1x_subscriber]: 123.456001 m
[INFO] [1724699067.233771477] [vl53l1x_subscriber]: 123.456001 m
[INFO] [1724699068.233776031] [vl53l1x_subscriber]: 123.456001 m
[INFO] [1724699069.233794514] [vl53l1x_subscriber]: 123.456001 m
別のターミナルで、publishしています。上記の受け取った「123.456001」は、下記のros2 topic pubコマンドが発行しています。
$ source /opt/ros/jazzy/setup.bash
$ ros2 topic pub /vl53l1x/range sensor_msgs/msg/Range "range: 123.456"
publishing #341: sensor_msgs.msg.Range(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), radiation_type=0, field_of_view=0.0, min_range=0.0, max_range=0.0, range=123.456, variance=0.0)
publishing #342: sensor_msgs.msg.Range(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), radiation_type=0, field_of_view=0.0, min_range=0.0, max_range=0.0, range=123.456, variance=0.0)
publishing #343: sensor_msgs.msg.Range(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), radiation_type=0, field_of_view=0.0, min_range=0.0, max_range=0.0, range=123.456, variance=0.0)
publishing #344: sensor_msgs.msg.Range(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), radiation_type=0, field_of_view=0.0, min_range=0.0, max_range=0.0, range=123.456, variance=0.0)
プログラムの働きを解説してみる。。。
最初の部分です。
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <iostream>
#include "sensor_msgs/msg/range.hpp"
いつものinclude文にセンサ用"sensor_msgs/msg/range.hpp"を追加しています。
このセンサ利用事例、C++,ROS2という条件下で、下記のページが見つかりましたが、上記のプログラムが動くようになって内容が少し理解できるわけですんけど、callback関数内で＆や＊がいっぱい出てきて、最初に読んだ時は何をしているのかが不明でした。
rclcpp::Node::SharedPtr g_node = nullptr;
これは、前回説明しましたが、ポインタ変数g_nodeにnullptrを代入して初期化します。プログラムの最後では、いろいろ使ったので、g_nodeに同じくnullptrを代入して終了しています。どうも、これがセオリーのようです。ここで記述しているので、グローバル変数ですね。C++は、関数の中で何も断りなく自由に使えるようです。
void tof_callback(const sensor_msgs::msg::Range::SharedPtr msg)
この記述にたどり着くのに相当な時間がかかりました。
sensor_msgs関係を検索すると、下記の記述が見つかります。include文からROS1の時代というのが（あとで）わかってきました。
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{ROS_INFO("I heard: [%s]", msg->data.c_str());}
検索して、わりとConstPtr& msgというのを見つけたので、&がついているので、これって参照渡しだよね。だから、勉強しようとなったわけで。
でも、中身で、->を使っているので、このcallback関数はポインタ渡しだとわかるのは、勉強をそこそこしてからでした。
ConstPtrはポインタのエイリアスです。
ConstPtrはBoostのShared Pointerにtypedefされています。このShared Pointerはスマートポインタという賢いポインタで、普通のポインタで使い終わったら必要となるdeleteしなくても自動的にメモリを開放してくれるすぐれものです。
ですが、ROS1時代の遺物で、
ROS2 では、rclcpp の std::shared_ptr の boost は廃止されています。
と回答のところに書かれています。検索するとROS1ばっかりにが出てくるので、それを参考にしている人が多いのではないかと。
＞void GetData( odomData*& msg ) //Passing the pointer by reference, so use the &
普通に書いたら、odomData*& msgになるのかな？ 本当にわからなくなってしまいます。「ポインタの参照」？！
したがって、::ConstPtr& は「定数メッセージへの共有ポインタへの参照」として理解
どんどんわからなくなっていきます。
つまりですね、世の中ROS2に移行しようとして何年かたったのだけれど、ROS2になって、プログラミングの解説した事例がインターネットには極端に少ないということなのです。
RCLCPP_INFO(g_node->get_logger(), "%f m", msg->range);
%dとしたら、msg->rangeはdoubleですと言われたので、%fにしました。
C++は、小数点3桁以降は切り捨てるという機能の関数が見つかりませんので、なんだか、無駄な桁数が表示されます。気にしないことにしました。
msg->rangeと、簡単に書いていますが、ほかの事例を参考に、msg->xxx.rangeとIMUなどの事例を参考にいろいろ試していたら、そんなメンバはないと怒られます。で、rangeだけにしてOKだというのは、ちょっとがっくりしました。構造体の処理方法がわかっていない証拠ですね。
mainの説明は省略します。subscriberとして一般的な記述です。