Webでクローラーの制御::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その１０）

# Webでクローラーの制御

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51900/22bf0dc6-0fcd-12aa-386c-b244963a066a.png)

今回は、上記スクショのバーチャルジョイスティック部分の作成と、クローラーの制御記録です。

## モータードライバの接続

モータードライバは以下のTB67H450を使用

https://akizukidenshi.com/catalog/g/gK-14753/


・左と右、２つ使用
| TB67H450 | Raspberry Pi |モーター|バッテリー|
|:-|:-|:-|:-|
|1. GND  |6. GND||-|
|2. IN2  |左24. 右22.|||
|3. IN1  |左23. 右27.|||
|4. VREF |1. 3V3 power|||
|5. VM   |      ||+|
|6. OUT1 |      |IN1||
|7. RS   |6. GND|||
|8. OUT2 |      |IN2||

※ VM-GND間には100μFの電解コンデンサをつけました。
※ 今回は電流制限を行わないので、RS-GND間には抵抗を付けていません。
※ バッテリーは秋月電子でDCDCコンバータモジュール（AE-MYMGK00506ERSR-5V0）を使用して、５V供給しました。

https://akizukidenshi.com/catalog/g/gK-15108/

## ドライバーのインストール

モーターの制御には、RPi.GPIOを使用します。

※最初はpigpioで試してみたのですが、OS起動時にpigpiodが起動しているようで起動しない問題が出て断念しました。。。

RPi.GPIOは、そのままだと一般ユーザーで実行できないのですが、以下のライブラリ入れたら一般ユーザーで動きました（なんでだろ？ｗ

```bash
$ apt update
$ apt upgrade
$ sudo apt install rpi.gpio-common
```

このあたりは実装時の記録が残っておらず、記憶も曖昧なので話半分で見てください＞＜

## ROSパッケージの作成

今回、カスタムメッセージを使用しますので、インターフェースを作成します。

```bash
$ cd ~/[ワークスペース名]/src
$ ros2 pkg create robot_interfaces --build-type ament_cmake
```

雛形ができたら、msgディレクトリに以下のファイルを作成します。
```msg/Crawler.msg
int64 x
int64 y
int64 b
int64 up
int64 down
int64 left
int64 right
```
次に、CMakeLists.txtを編集します。

```CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(robot_interfaces)

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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Crawler.msg"
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

```

たぶん、雛形のファイルから書き足すのは以下の部分だけだと思います。
```
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Crawler.msg"
)
```

つぎは、package.xmlの編集
```xml
<?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>robot_interfaces</name>
  <version>0.0.1</version>
  <description>TODO: Package description</description>
  <maintainer email="hoge@example.jp">hogehoge</maintainer>
  <license>TODO: License declaration</license>

  <depend>geometry_msgs</depend>

  <build_depend>rosidl_default_generators</build_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
```

といっても、description や email のところを書き換えるくらいで、ほかはいじらなくても良かった気がします。

作成したらビルドしておきましょう。


次に、robot_teleop という名前でクローラー制御のパッケージを作成します。
```bash
$ ros2 pkg create --build-type ament_python --node-name robot_teleop_node robot_teleop
```
パッケージの雛形ができたら、robot_teleop/robot_teleop/robot_teleop_node.pyを編集します。

```python robot_teleop_node.py
import RPi.GPIO as GPIO
import time

import rclpy
from rclpy.node import Node
from robot_interfaces.msg import Crawler

class RobotMoveControlService(Node):
    def __init__(self):
        super().__init__('robot_move_control_service')
        self.crawlerSub = self.create_subscription(
            Crawler,
            'crawler_move',
            self.crawlerSubscription,
            10
        )
        self.crawlerSub

        self.gpio_crawler_left_1 = 24
        self.gpio_crawler_left_2 = 23
        self.gpio_crawler_right_1 = 22
        self.gpio_crawler_right_2 = 27

        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)

        GPIO.setup(self.gpio_crawler_left_1, GPIO.OUT)
        GPIO.setup(self.gpio_crawler_left_2, GPIO.OUT)
        GPIO.setup(self.gpio_crawler_right_1, GPIO.OUT)
        GPIO.setup(self.gpio_crawler_right_2, GPIO.OUT)

        self.crawler_left_1 = GPIO.PWM(self.gpio_crawler_left_1, 50)   # 周波数50Hz
        self.crawler_left_2 = GPIO.PWM(self.gpio_crawler_left_2, 50)   # 周波数50Hz
        self.crawler_right_1 = GPIO.PWM(self.gpio_crawler_right_1, 50)  # 周波数50Hz
        self.crawler_right_2 = GPIO.PWM(self.gpio_crawler_right_2, 50)   # 周波数50Hz
        self.crawler_left_1.start(0)
        self.crawler_left_2.start(0)
        self.crawler_right_1.start(0)
        self.crawler_right_2.start(0)

    def crawlerSubscription(self, msg):
        time.sleep(0.1)
        if msg.b == 1:
            self.crawler_left_1.ChangeDutyCycle(100)
            self.crawler_left_2.ChangeDutyCycle(100)
            self.crawler_right_1.ChangeDutyCycle(100)
            self.crawler_right_2.ChangeDutyCycle(100)
        else:
            if msg.up == 1:
                if msg.left == 1:
                    # up left
                    diff = abs(msg.x) * 2 / 100

                    duty_left1 = (abs(msg.y)) * 2
                    duty_right1 = abs(msg.y) - (abs(msg.y) * diff)
                elif msg.right == 1:
                    # up right
                    diff = abs(msg.x) * 2 / 100

                    duty_left1 = abs(msg.y) - (abs(msg.y) * diff)
                    duty_right1 = (abs(msg.y)) * 2
                else:
                    # up
                    duty_left1 = (abs(msg.y)) * 2
                    duty_right1 = duty_left1

                duty_left2 = 0
                duty_right2 = 0
            elif msg.down == 1:
                if msg.left == 1:
                    # down left
                    diff = abs(msg.x) * 2 / 100

                    duty_left2 = (abs(msg.y)) * 2
                    duty_right2 = abs(msg.y) - (abs(msg.y) * diff)
                elif msg.right == 1:
                    # down right
                    diff = abs(msg.x) * 2 / 100

                    duty_left2 = abs(msg.y) - (abs(msg.y) * diff)
                    duty_right2 = (abs(msg.y)) * 2
                else:
                    # down
                    duty_left2 = (abs(msg.y)) * 2
                    duty_right2 = duty_left2
                duty_left1 = 0
                duty_right1 = 0
            elif msg.left == 1:
                # left
                duty_left1 = (abs(msg.x)) * 2
                duty_right2 = duty_left1
                duty_left2 = 0
                duty_right1 = 0
            elif msg.right == 1:
                # right
                duty_left2 = (abs(msg.x)) * 2
                duty_right1 = duty_left2
                duty_left1 = 0
                duty_right2 = 0
            else:
                duty_left1 = 0
                duty_left2 = 0
                duty_right1 = 0
                duty_right2 = 0

            #self.get_logger().info('Incoming request\nb: %d x: %d y: %d up: %d down: %d left: %d right: %d duty l-1: %d l-2: %d r-1: %d r-2: %d' % (request.b, request.x, request.y, request.up, request.down, request.left, request.right, duty_left1, duty_left2, duty_right1, duty_right2))

            if duty_left1 < 100 and duty_left2 < 100 and duty_right1 < 100 and duty_right2 < 100:
                self.crawler_left_1.ChangeDutyCycle(duty_left1)
                self.crawler_left_2.ChangeDutyCycle(duty_left2)
                self.crawler_right_1.ChangeDutyCycle(duty_right1)
                self.crawler_right_2.ChangeDutyCycle(duty_right2)

def main():
    try:
        print('Hi from robot_teleop.')
        rclpy.init()
        node = RobotMoveControlService()
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Ctrl+C received - exiting...')
    finally:
        node.get_logger().info('ROS node shutdown')
        node.destroy_node()
        rclpy.shutdown()

        node.crawler_left_1.stop()
        node.crawler_left_2.stop()
        node.crawler_right_1.stop()
        node.crawler_right_2.stop()

        GPIO.cleanup()


        print('program close')


if __name__ == '__main__':
    main()

```

上記パッケージができたら、ローンチファイルに追加しておくと便利です。

```python
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    v4l2_node = Node(
        package='v4l2_camera',
        executable='v4l2_camera_node',
        output='screen',
        parameters=[{'image_size': [320, 240]}]
    )

    image_transport = Node(
        package='image_transport',
        executable='republish',
        arguments=["raw"],
        remappings=[('in', '/image_raw'),
                    ('out', '/image_raw/compressed')
                   ]
    )

    web_image = Node(
        package='web_image',
        executable='web_image_node'
    )

    i2c = Node(
        package='ros2_i2c',
        executable='ros2_i2c'
    )

    robot = Node(
        package='robot_teleop',
        executable='robot_teleop_node'
    )

    return LaunchDescription([
        v4l2_node,
        image_transport,
        web_image,
        i2c,
        robot
    ])
```
いまのローンチファイルはこんな感じです。
適宜書き換えて使ってみてください。

## Web側の制御

### バーチャルジョイスティックの使用

バーチャルジョイスティックのライブラリはこちらのものを使用しました。

https://github.com/jeromeetienne/virtualjoystick.js/blob/master/README.md

すごい便利です。
上記リンクにサンプルもあるのでご参考ください。

で、このライブラリを使って、ジョイスティックの情報をラズパイに送ります。

まずはJSプログラムを書きます。

```javascript
$(function() {
    console.log("touchscreen is", VirtualJoystick.touchScreenAvailable() ? "available" : "not available");

    let joystick	= new VirtualJoystick({
        container	: document.getElementById('joystick-area'),
        mouseSupport	: true,
        limitStickTravel: true,
		stickRadius	: 50
    });

    let crawlerStartFlag = false;

    joystick.addEventListener('touchStart', function(){
        crawlerStartFlag = true;
        console.log('crawler start.')
    })
    joystick.addEventListener('touchEnd', function(){
        crawlerStartFlag = false;
        console.log('crawler stop.', true)

        let crawler = new ROSLIB.Message({
            x : 0,
            y : 0,
            b : 1,
            up: 0,
            down : 0,
            left : 0,
            right : 0
        });

          // And finally, publish.
        crawlerPub.publish(crawler);


    })

    setInterval(function(){
        if (crawlerStartFlag) {
            let dx = Math.abs(parseInt(joystick.deltaX()));
            let dy = Math.abs(parseInt(joystick.deltaY()));

            let crawler = new ROSLIB.Message({
                x : dx,
                y : dy,
                b : 0,
                up : joystick.up() ? 1 : 0,
                down : joystick.down() ? 1 : 0,
                left : joystick.left() ? 1 : 0,
                right : joystick.right() ? 1 : 0
            });

              // And finally, publish.
            crawlerPub.publish(crawler);

        }
    }, 100);

}
```

先述の記事に足した形ですので、WSの起動などの処理は省いています。過去記事に記載がありますのでご参照ください。

https://qiita.com/shoichi4411/items/e0aa33d9ce286485c5a8

Javascriptの記述ができたら、html側も書き足していきます。

といっても、ジョイスティックを使用できるエリアをDIVで書き足すだけなので、記述としてはどこか適当なところに

```html
<div id="joystick-contents">
    <div id="joystick-area"></div>
</div>
```

として書き足すだけです。

ただ、それだけだと上記DIVに高さも幅もないので、CSSで高さの幅を指定します。
※一番最初のスクショにあるように、ユーザーが操作しないインフォメーションエリアなどに重ねるように設置することも可能です。

```css
#main-contents {
    position: relative;
    text-align: center;
    margin: auto;
    padding: 0;
    height: 100svh;
    width: 100dvw;
}

#joystick-contents {
    position: absolute;
    top: 0;
    left: 0;
    width: 180px;
    height: 100svh;
    z-index: 3;
}
#joystick-area {
    width: 100%;
    height: 100svh;
}
```
※上記は過去記事のCSSに書き足したものです。
※あくまでコードの抜粋なので、レイアウトの調整はご自身で調整お願いします。

Web側の作成完了です。

これで、クローラーの制御は完了です。


次回は、フルカラーLED（NeoPixcel）の制御あたりを書く予定です。
ラズパイで上記LEDを制御するのは割と問題（GPIOポートが制限されるなど）があるため、ArduinoにフルカラーLEDを接続して、ArduinoとラズパイをI2C接続でLEDを制御します。

[[←前回]足回りの制作::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（ハードウェア編その１）](https://qiita.com/shoichi4411/items/75966efb74bcbf6d78c4)