備忘録　ラズパイ5 ROS2 ⑩ Ubuntu Desktop 24.04LTSでROS2環境　C++ その6  VL53L1X.cpp 距離センサ

　Adafruitから入手したTOF（Time of Flight Distance）センサ VL53L1X（ST Microelectronics）を利用します。この距離センサ・ボードはマルツで購入しました。VL53L？？シリーズはいろいろありますが、ROS2のコードが見つかったのはこのモデルだけです。

https://www.st.com/en/imaging-and-photonics-solutions/vl53l1x.html

##### VL53L1Xのおもなスペック

- 測定最大距離；4m
- 測定頻度；50Hz
- 波長；940nm


### i2cのグループに属するように変更
＝＝＝＝＝未確認
　ラズパイも初期のころ、I2CやSPIバス経由でアクセスするとき、root権限が必要だったようです。4，5年前から、インストールするときのデフォルトのユーザには、ユーザ権限で、アクセスができるように何とかのグループに属するように変更されました。
　ubuntuでは、i2cのアクセスにはroot権限が必要な？？？未確認

　i2cデバイスを確認します。

```
$ ls /dev/i2c*
/dev/i2c-1  /dev/i2c-11  /dev/i2c-12
```

で、i2cグループに所属しているとアクセスができるようです。ユーザubuntuをi2cのグループのメンバに変更します。

```
$ sudo usermod -aG i2c ubuntu
```

### Githubからros2用VL53L1Xを持ってくる


```
yoshi@yoshi:~$ sudo apt install git
git clone https://github.com/slaghuis/ROS2-VL53L1X.git
```

``Failed to connect to github.com port 443 after 9 ms: Couldn't connect to server``
というエラーが出て解消できないです。
　ubuntuをアップデートしてrestartさせました。
```
yoshi@yoshi:~$ cd ros2_ws
yoshi@yoshi:~/ros2_ws$ git clone https://github.com/slaghuis/ROS2-VL53L1X.git
Cloning into 'ROS2-VL53L1X'...
remote: Enumerating objects: 87, done.
remote: Counting objects: 100% (26/26), done.
remote: Compressing objects: 100% (7/7), done.
remote: Total 87 (delta 24), reused 19 (delta 19), pack-reused 61
Receiving objects: 100% (87/87), 35.91 KiB | 4.49 MiB/s, done.
Resolving deltas: 100% (41/41), done.
yoshi@yoshi:~/ros2_ws$ ls
build  install  log  out  ROS2-VL53L1X  works
```
　持ってこれました。ビルドします。

```
yoshi@yoshi:~/ros2_ws$ source /opt/ros/jazzy/setup.bash
yoshi@yoshi:~/ros2_ws$ colcon build --packages-select vl53l1x
```

　``error warning:``がたくさんでました。

```bash
Starting >>> vl53l1x
stderr: vl53l1x
In file included from /home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:32:
/home/yoshi/ros2_ws/ROS2-VL53L1X/include/vl53l1x.h: In constructor ‘Vl53l1x::Vl53l1x()’:
/home/yoshi/ros2_ws/ROS2-VL53L1X/include/vl53l1x.h:232:12: warning: ‘Vl53l1x::io_timeout’ will be initialized after [-Wreorder]
  232 |   uint16_t io_timeout;
      |            ^~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/include/vl53l1x.h:231:8: warning:   ‘bool Vl53l1x::did_timeout’ [-Wreorder]
  231 |   bool did_timeout;
      |        ^~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:35:1: warning:   when initialized here [-Wreorder]
   35 | Vl53l1x::Vl53l1x()
      | ^~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/include/vl53l1x.h:239:11: warning: ‘Vl53l1x::saved_vhv_timeout’ will be initialized after [-Wreorder]
  239 |   uint8_t saved_vhv_timeout;
      |           ^~~~~~~~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/include/vl53l1x.h:229:11: warning:   ‘uint8_t Vl53l1x::last_status’ [-Wreorder]
  229 |   uint8_t last_status;
      |           ^~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:35:1: warning:   when initialized here [-Wreorder]
   35 | Vl53l1x::Vl53l1x()
      | ^~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp: In member function ‘uint8_t Vl53l1x::readReg(uint16_t)’:
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:513:51: warning: comparison of integer expressions of different signedness: ‘ssize_t’ {aka ‘long int’} and ‘long unsigned int’ [-Wsign-compare]
  513 |   if( write(fd, writeBuffer, sizeof(writeBuffer)) < sizeof(writeBuffer)) {
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp: In member function ‘uint16_t Vl53l1x::readReg16Bit(uint16_t)’:
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:532:51: warning: comparison of integer expressions of different signedness: ‘ssize_t’ {aka ‘long int’} and ‘long unsigned int’ [-Wsign-compare]
  532 |   if( write(fd, writeBuffer, sizeof(writeBuffer)) < sizeof(writeBuffer)) {
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:539:48: warning: comparison of integer expressions of different signedness: ‘ssize_t’ {aka ‘long int’} and ‘long unsigned int’ [-Wsign-compare]
  539 |   if( read(fd, readBuffer, sizeof(readBuffer)) < sizeof(readBuffer)) {
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp: In member function ‘void Vl53l1x::readResults()’:
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:637:51: warning: comparison of integer expressions of different signedness: ‘ssize_t’ {aka ‘long int’} and ‘long unsigned int’ [-Wsign-compare]
  637 |   if( write(fd, writeBuffer, sizeof(writeBuffer)) < sizeof(writeBuffer)) {
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~
/home/yoshi/ros2_ws/ROS2-VL53L1X/src/vl53l1x.cpp:644:48: warning: comparison of integer expressions of different signedness: ‘ssize_t’ {aka ‘long int’} and ‘long unsigned int’ [-Wsign-compare]
  644 |   if( read(fd, readBuffer, sizeof(readBuffer)) < sizeof(readBuffer)) {
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~
---
Finished <<< vl53l1x [9.95s]

Summary: 1 package finished [10.1s]
  1 package had stderr output: vl53l1x
```

　いくつかの``will be initialized after [-Wreorder]``は、初期化の順番に合わせて、ヘッダー・ファイルの宣言部分の順序を入れ替えて対処しました。

```C++
// Constructor /////////////////////////////////////////////////////
Vl53l1x::Vl53l1x()
  : address(AddressDefault)
  , last_status(0)
  , did_timeout(false)
  , io_timeout(0) // no timeout
  , calibrated(false)
  , saved_vhv_init(0)
  , saved_vhv_timeout(0)
  , distance_mode(Unknown)
  ```

  　残りは、if文の中で、i2cのアクセスの結果と、sizeofの型が異なるために出ているようです。sizeofはなにの型かを検索すると、unsigneとそうでない両方が見つかり、困った。
   　なので、if文をコメントアウト、i2cのアクセスだけの記述を残すように変更しました（5か所）。
    
```
write(fd, writeBuffer, sizeof(writeBuffer));
//  if( write(fd, writeBuffer, sizeof(writeBuffer)) < sizeof(writeBuffer)) {
    //something went wrong
//    std::cout << "DEBUG: ERROR on write to i2c" << std::endl;
//    return 0x00;
//  }
```    
 　ビルドしなおします。   
```
yoshi@yoshi:~/ros2_ws$ colcon build --packages-select vl53l1x
Starting >>> vl53l1x
Finished <<< vl53l1x [1.38s]

Summary: 1 package finished [1.52s]
```

　ビルドできたようです。

### 接続
　BME280の後ろにStemma QT/Qwiicケーブルでつなぎます。
 

![lps22-bme280-VL53.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/6e6ff585-eae5-99be-7d74-3c9022577e92.png)

　0x29に見つけてきました。
 
```
yoshi@yoshi:~/ros2_ws$ i2cdetect -y 1
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:                         -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- 29 -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- UU

```

　登録し、vl53l1x_nodeを起動します。

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


　ラズパイ側でターミナルを開き、可視化ツールを起動します。リフレッシュしたら、Publishしている様子が表示されました。

 ```
 $ source /opt/ros/jazzy/setup.bash
 $ rqt_graph
 ```


![Screenshot from 2024-08-07 03-54-28.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/49a8a0c1-6334-b3a6-7c9f-26967e959dcd.png)

 Windows10で別のコマンド・プロンプトを起動し、vl53l1x/rangeのechoを表示します。
 
 
```
ssh yoshi.local
 
yoshi@yoshi:~/ros2_ws$ source /opt/ros/jazzy/setup.bash
yoshi@yoshi:~$ ros2 topic echo vl53l1x/range
```

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/d660663d-0668-d07c-7dad-c8dd58593512.png">

　ToFは測定を実行しているようです。


-----------------
<h2>備忘録　ラズパイ5 ROS2</h2>

[①　ハードの用意とUbuntu Desktop 24.04LTS](https://qiita.com/jamjam/items/9ad621bda6ab2e1d325b)

[②　Ubuntu Desktop 24.04LTSでROS2環境　rqt_graphとturtlesim](https://qiita.com/jamjam/items/b7bed8ec9ff68426ac8d)

[③　Ubuntu Desktop 24.04LTSでROS2環境　Python その１ responder（セットアップ）](https://qiita.com/jamjam/items/d124c662813085bc69e3)

[④　Ubuntu Desktop 24.04LTSでROS2環境　Pythonその２responder（コーディングと実行；失敗）](https://qiita.com/jamjam/items/d488e2e4fa88d9882626)

[⑤　Ubuntu Desktop 24.04LTSでROS2環境　C++ その1 セットアップ main.c](https://qiita.com/jamjam/items/4bba331c08805ccda0a8)

[⑥　Ubuntu Desktop 24.04LTSでROS2環境　C++ その2 セットアップ pub.cpp rqt_graph](https://qiita.com/jamjam/items/51eeb26400517883aa59)

[⑦　Ubuntu Desktop 24.04LTSでROS2環境　C++ その3 セットアップ sub.cpp rqt_graph](https://qiita.com/jamjam/items/50f3bc352e4cd0a7959d)

[⑧　Ubuntu Desktop 24.04LTSでROS2環境　C++ その4 bme280.cpp 温度](https://qiita.com/jamjam/items/f64eaae91fefed4eac34)

[⑨ Ubuntu Desktop 24.04LTSでROS2環境　C++ その5 bme280_3.cpp 温度、湿度、気圧](https://qiita.com/jamjam/items/edb799ec88bdfba29c4c)

[⑩ Ubuntu Desktop 24.04LTSでROS2環境　C++ その6 VL53L1X.cpp 距離センサ](https://qiita.com/jamjam/items/21074d7ed830926d7ff4)

--------------

https://qiita.com/jamjam/items/692568361dde688fad16