View on GitHub

introduction-to-ros

ROS (Robot Operating System) 入門

演習:部屋を巡回するルンバのROSプログラミング

前のページ

概要

今までの章でROSの基礎,2D LiDARを積んだ移動ロボットのシミュレーションモデルの作成を行いました.本章では,2D LiDARで取得した周囲の障害物情報を用いて,障害物にぶつからないように部屋を巡回する知能ロボットのコード作成をROSを用いて行います.

問題設定確認

  1. 2D LiDARの情報を取得
  2. ロボットに速度指令値を与える

URDFにアクチュエータを追加ロボットモデルにセンサを追加で設定した通り,2D LiDARのROSトピック名は/scan, 速度指令値のROSトピック名は/cmd_velです.

ここで,これらのトピックの型を調べてみましょう.

roslaunch my_urdf_tutorial robot_simulation.launch

問題0:CommandLine Toolsの内容を思い出しながら,/scan, /cmd_velの型を調べましょう.


解答:rostopic info /scan等で以下のようにわかります.

よって,以下では

するROSノードを作成しましょう.

問題1:上記のpubsubを行うROSノードを作成する

問題1.1: 新たにroom_circuit_controllerという名前のROSパッケージを作成しましょう.

問題1.2: room_circuit_controller.cppを適切な場所に作成し,上記のROSノードを作成しましょう.

設定

参考:トピック通信実践

解答はこちら

問題2:/scanの内容に応じて/cmd_velをpublish

/scan, /cmd_velの具体的な内容は以下のリンク先記載の通りです. ROSトピックの型名でググると出てくるので,ググれるようになりましょう.

/scan

/cmd_vel

今回は,以下のような動作フローを考えてみます.

問題2.1: 以下のソースコードにおける「ここを書く!!!!!」の部分を適切に埋めましょう.

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

ros::Publisher pub;
ros::Subscriber sub;

// variables
float velocity = 0.2; // 直進速度
float angular_velocity = 1.57; // 回転速度
float avoid_distance = 1.0; // これ以上近いと障害物とみなす距離
float avoid_angle = 60; // 障害物を探索する範囲

void scanCallback(const sensor_msgs::LaserScan &msg)
{
    int center_index = msg.ranges.size() / 2;             // ロボット前方方向を表すインデックス
    int index_avoid_angle = avoid_angle * (M_PI / 180.0) / msg.angle_increment; // 障害物を探索する範囲に相当するインデックス
    int start_index = center_index - index_avoid_angle / 2; // 探索開始インデックス
    int last_index = center_index + index_avoid_angle / 2; // 探索終了インデックス

    bool detect_obstacle = false;
    std::string detect_side;

    for (int i = start_index; i <= last_index; i++)
    {
        float range = ここを書く!!!!!; // 距離

        // 値が無効な場合の排除
        if (range < msg.range_min || // エラー値の場合
            range > msg.range_max || // 測定範囲外の場合
            std::isnan(range))       // 無限遠の場合
            ;

        // 値が有効である場合
        else
        {
            if (ここを書く!!!!!) // 近くに障害物がある場合
            {
                detect_obstacle = true;
                if (i <= center_index)
                {
                    detect_side = "right";
                }
                else
                {
                    detect_side = "left";
                }
                break;
            }
        }
    }

    geometry_msgs::Twist cmd_vel;

    if (detect_obstacle)
    {
        if (detect_side == "right")
        {
            // その場で反時計回り回転
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = angular_velocity;
            pub.publish(cmd_vel); // publish
        }
        else
        {
            // その場時計回り回転
            ここを書く!!!!!
        }
    }
    else
    {
        // 直進
        ここを書く!!!!!
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "room_circuit_controller");
    ros::NodeHandle n;

    pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    sub = n.subscribe("/scan", 1, scanCallback);

    ros::spin();
}

ヒント:cmd_vel.linear.zに直進速度,cmd_vel.angular.zに回転速度を入れます.

問題2.2: CMakeLists.txtを適切に編集しましょう.

問題2.3: ビルドしましょう.

問題2.4: my_urdf_tutorial/launch/robot_simulation.launchをincludeし,かつroom_circuit_controllerノードを実行するroom_circuit_controller.launchを適切な場所に作成しましょう.

参考

トピック通信実践

roslaunch

解答はこちら

動作確認

roslaunch room_circuit_controller room_circuit_controller.launch

動いたらOKで,さらに前方に障害物を置いてみて障害物回避が行われれば成功です!

また,実際に部屋のような環境で確認したい方は,私の方で部屋のような環境を用意したので以下の手順に従えば導入できます!

  1. my_urdf_tutorial配下にworldディレクトリを作成
  2. worldディレクトリの中にroom.worldという名前のファイルを作成
  3. room.worldこちらの内容をコピペ
  4. my_urdf_tutorial/launch/robot_simulation.launchを下記のように変更.gazebo起動時にworldファイルを読み込むように編集します.

     <?xml version="1.0"?>
     <launch>
     <param name="robot_description" textfile="$(find my_urdf_tutorial)/urdf/roomba_sim.urdf"/>
     <arg name="rvizconfig" default="$(find my_urdf_tutorial)/config/robot_simulation.rviz" />
    
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <arg name="world_name" value="$(find my_urdf_tutorial)/world/room.world" />
     </include>
    
     <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -x 0.0 -y 0.0 -z 0.5 -R 0 -P 0 -Y 0 -urdf -model roomba_sim" />
    
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
        
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig) -f odom"/>
     </launch>
    

ここまで行い,

roslaunch room_circuit_controller room_circuit_controller.launch

すると以下のような環境が立ち上がります!(初回起動時はオブジェクトのダウンロードの為少し時間がかかります)

問題3

問題3.1: velocity, angular_velocity, avoid_distance, avoid_angleをrosparam化しましょう.

問題3.2: room_circuit_controller.launchからパラメータを渡し,変更できるようにしてみましょう.

参考

rosparam

roslaunch

解答はこちら

ここまでで演習は終了です!完成したら色々とパラメータをいじって遊んでみてください.

最後に

今回扱えなかったが重要なもの

リンク先の記事の余談に導入程度の記載をしています.興味のある方は追加で調べてみてください.

自律移動ロボット

本記事で自律移動ロボットのシミュレータが作成できたので,本環境を用いてそのまま自律移動ロボットの勉強が可能です.自律移動ロボットの代表的な技術として以下が挙げられます.

そして,上記の(基礎的な)技術が実装されているROSパッケージが公開されています.

navigation stack

本記事で作成したシミュレータでもnavigation stackは適用することができるため,興味のある方はぜひ導入してみてください.

navigation stackの構成がわかりやすい記事

自律移動ロボットに関連する大会

リンク

目次