Turtlebot3(Burger)をROS BridgeしてRustで動かす

はじめに

この記事はTurtlebot3(Burger)をROS BridgeしてRustで動かす準備の続きです。

記事について

Turtlebot3のリポジトリにturtlebot3_teleopというサブモジュールがあります。C++で書いてあるものをRustで書き直してみました。完全なコピーではないですが。

Rustは、なかなか難しい気がしますが、動いた時は嬉しかったです。再入門中。
社内でも勉強会が毎週金曜日にあります。

ロボットと相性がいいらしい

環境

Turtlebot3 Burger: Ubuntu 18.04, Dashing(ROS2), ROS Bridge
RemotePC: Ubuntu 18.04, Melodic(ROS1)

teleopとは

wキー : 前
aキー : 左
dキー : 右
xキー : 後
sキー : 停止
qキー : プログラム終了

キーボードからの入力でTurtlebot3(Burger)を動かすプログラムを作ります。

Turtlebot3起動

RemotePC(ROS1)

• ROSマスタ起動

$ roscore

Turtlebot3 Burger(ROS2, ROS Bridge)

• ROS Bridge起動

$ . /opt/ros/melodic/setup.bash
$ . /opt/ros/dashing/setup.bash
$ ros2 run ros1_bridge dynamic_bridge

• Turtlebot3起動

$ cd turtlebot3_ws/
$ source install/setup.bash
$ ros2 launch turtlebot3_bringup robot.launch.py

RemotePC(ROS1)

• Rustプログラム起動

$ cd　/your_workspace/
$ catkin_make
$ source devel/setup.bash
$ rosrun rust_controller rust_controller

Rustプログラムについて

• フォルダ構造(rust_controllerプロジェクト以下)

$ tree
.
├── CMakeLists.txt
├── Cargo.toml
├── package.xml
└── src
    └── main.rs

• 実行ファイル

下記の実行ファイルは、main.rsです。

$ rosrun rust_controller rust_controller

main.rs

extern crate termion;

use termion::event::Key;
use termion::input::TermRead;
use termion::raw::IntoRawMode;
use std::io::{Write, stdout, stdin};

use env_logger;
use rosrust;

use std::process;

const BURGER_MAX_LIN_VEL: f64 = 0.22;
const BURGER_MAX_ANG_VEL: f64 = 2.84;

// const WAFFLE_MAX_LIN_VEL: f64 = 0.26;
// const WAFFLE_MAX_ANG_VEL: f64 = 1.82;

const LIN_VEL_STEP_SIZE: f64 = 0.01;
const ANG_VEL_STEP_SIZE: f64 = 0.1;

fn get_key(current_key: char) ->  char {
    let stdin = stdin();

    for c in &mut stdin.keys() {
        match c.unwrap() {
            Key::Char(c) => {
                return c;
            }
            _ => {}
        }
    }

    return current_key
}

fn constrain(input: f64, low: f64, high: f64) -> f64{
    if input < low {
        return low;
    } else if input > high {
        return high;
    }

    return input;
}

fn check_liner_limit_velocity(vel: f64) -> f64{
    return constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL);
}

fn check_angular_limit_velocity(vel: f64) -> f64{
    return constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL);
}

fn make_cmd_velocity(vel: f64, size: f64) -> f64{
    vel + size
}

fn main() {
    // Logger
    env_logger::init();
    // Initialize node
    rosrust::init("turtlebot3_rust_controller");
    // Create publisher
    let cmd_vel_pub = rosrust::publish("cmd_vel", 10).unwrap();
    // Create object that maintains 10Hz between sleep requests
    let rate = rosrust::rate(10.0);

    // Breaks when a shutdown signal is sent
    while rosrust::is_ok() {
        let current_key = ' ';
        let mut target_linear_vel: f64 = 0.0;
        let mut target_angular_vel: f64 = 0.0;

        loop {
            let mut stdout = stdout().into_raw_mode().unwrap();
            let key = get_key(current_key);
            match key {
                'q' => {
                    process::exit(0);
                },
                'w' => {
                    target_linear_vel = check_liner_limit_velocity(make_cmd_velocity(target_linear_vel , LIN_VEL_STEP_SIZE))
                }
                'x' => {
                    target_linear_vel = check_liner_limit_velocity(make_cmd_velocity(target_linear_vel,  -LIN_VEL_STEP_SIZE))
                }
                'a' => {[termionrepository]
                    target_angular_vel = check_angular_limit_velocity(make_cmd_velocity(target_angular_vel, ANG_VEL_STEP_SIZE));
                }
                'd' => {
                    target_angular_vel = check_angular_limit_velocity(make_cmd_velocity(target_angular_vel, -ANG_VEL_STEP_SIZE))
                }
                's' => {
                    target_linear_vel = 0.00;
                    target_angular_vel = 0.00;
                },
                _ => {}
            }

            let mut new_cmd_vel_data = rosrust_msg::geometry_msgs::Twist::default();

            new_cmd_vel_data.linear.x = target_linear_vel;
            new_cmd_vel_data.linear.y = 0.00;
            new_cmd_vel_data.linear.z = 0.00;

            new_cmd_vel_data.angular.x = 0.00;
            new_cmd_vel_data.angular.y = 0.00;
            new_cmd_vel_data.angular.z = target_angular_vel;

            // Publish cmd
            cmd_vel_pub.send(new_cmd_vel_data).unwrap();

            stdout.flush().unwrap();
        }
    }

    rate.sleep();
    rosrust::spin();
}

Tomlファイル

キーボード入力に使用したクレート: termion = "1.5.5"

[dependencies]
env_logger = "0.7.1"
rosrust = "0.9"
rosrust_msg = "0.1"
termion = "1.5.5"

おわりに

Rustは現在勉強中です。簡単なプログラム書くのにもコンパイルが通らないのでもっと勉強しなければ。
actix-webが気になっています。

参考

[rosrust]
ROSのnodeをRustで書いてcatkinで管理する