8.2.5 障害物が存在する場合に手動でゴールを設定する

8.2.4 仮想障害物の回避

8.2.3 move_baseを使った四角形に沿った移動

　8.2.3 move_baseを使った四角形に沿った移動

8.2 ArbotiX シミュレータでmove_baseノードをテストする

8.2 ArbotiX シミュレータによる move_base のテスト

まだ地図が出来ていないので~/catkin_ws/src/rbx1/rbx1_nav/maps/blank_map.pgmという何も無い環境（真っ黒な地図）でロボットを動かす。
地図の指定は以下のように~/catkin_ws/src/rbx1/rbx1_nav/maps/blank_map.yamlファイルで行う。

ノードを実行可能にすることを忘れないように：

mage: blank_map.pgm
resolution: 0.01
origin: [-2.5, -2.5, 0]
occupied_thresh: 0.65
free_thresh: 0.196 # Taken from the Willow Garage map in the　turtlebot_navigation package
negate: 0

move_baseノードはfake_move_base_blank_map.launchで起動する

<launch>
<!-- Run the map server with a blank map -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
<!-- Launch move_base and load all navigation parameters -->
<include file="$(find rbx1_nav)/launch/fake_move_base.launch" />
<!-- Run a static transform between /odom and /map -->
<node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /odom /map 100" />
</launch>

始めに、map_serverノードをblank_map.yamlを引数として起動する
次に、以下で説明する fake_move_base.launch を読み込む。fake_move_base.launch ではmove_baseノード
を起動する。
最後に、ここではブランク地図を使い、シミュレータ内のロボットはセンサを持たないので距離データを使えないので、
代わりにオドメトリが完全なものとして、マップ上の自己位置を更新する。

fake_move_base.launchは次のような内容です

<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
<rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
<rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />
</node>
</launch>

パラメータファイルに関しては8.1.2を見てください。

起動は以下のようにします。
始めに、ArbotiXシミュレータを起動します。

$roslaunch rbx1_bringup fake_turtlebot.launch

ここで、KOBUKIを使いたいときには

$roslaunch rbx1_bringup fake_turtlebot2.launch

とします。
次に、move_baseノードをブランク地図で起動します。

$roslaunch rbx1_nav fake_move_base_blank_map.launch

さらに、可視化のためにRvizを起動します。

$rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz

最後に、コマンドラインからmove_baseアクションを送ることでロボットが指定された位置・姿勢に向けて移動します。
以下の例は、ロボットをX軸方向に1mだけ移動させます。
※以下のコマンドをコピペする時に、継続行を示す”バックスラッシュ”は不要です。

$rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \'{ header: { frame_id: "map" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

こちらを使って下さい。

$rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "map" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

次のコマンドでは、グローバル座標系で初期位置・姿勢に戻ります。

$rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

ロボットの軌跡が直線経路ではなく遠回りしているように思えます。
これは、経路上に障害物が無いので経路探索プログラムが一番スムーズだと思える経路を計算したからです。

pdist_scale (0.4) 
gdist_scale (0.8) 
最大速度 (max_vel_x)

このパラメータを変えると経路が変わります。
このコマンドでパラメータを変えることができます。

$rosrun rqt_reconfigure rqt_reconfigure

8.2.1 RViz上で目的地をクリックして移動させる
「2D Nav Goal」メニューをクリックし、マウスでゴールの位置と姿勢を指定する。


クォータニオンについて
一般的にロボットの姿勢を表すにはオイラー角であるRoll, Pitch, Yawを使いました。

8.1.2 経路探索のパラメータの設定

move_base ノードでは以下の4つのパラメータファイルが必要です。

(1) base_local_planner_params.yaml

controller_frequency: 3.0
　　経路のプランニングを行う周期。普通のノートPCで3〜5回／秒程度に設定する。
max_vel_x: 0.3
　　ロボットの最高並進速度（メートル毎秒）。室内だと0.3〜0.5程度
min_vel_x: 0.05:
　　ロボットの最低並進速度（メートル毎秒）
max_rotational_vel: 1.0
　　ロボットの最高回転速度（ラジアン毎秒）
min_in_place_vel_theta: 0.5
　　ロボットの最低回転速度（ラジアン毎秒）
escape_vel: -0.1
　　ロボットが回避するときの速度（メートル毎秒）。負の値は後退
acc_lim_x: 2.5
　　x座標方向への加速度の最大値
acc_lim_y: 0.0
　　y座標方向への加速度の最大値。0の場合は非ホロノミック台車の場合で真横には動けない
acc_lim_theta: 3.2
　　回転の加速度の最大値
holonomic_robot: false
　　全方位移動台車を使っていない場合は常に false
yaw_goal_tolerance: 0.1
　　目的の姿勢との許容誤差（ラジアン）。この値が小さいとゴール付近で振動してしまう。　　
xy_goal_tolerance: 0.1
　　目的の位置との許容誤差（メートル）。地図の解像度よりも小さい値にしないこと。
pdist_scale: 0.8
　　グローバルパスにどれだけ正確に沿って移動するか。gdist_scaleより大きな値にすると、よりグローバルパスに沿って動く。
gdist_scale: 0.4
　　ゴールを目指すか、グローバルパスに沿うか、を決める。　　
occdist_scale: 0.1
　　障害物を回避する程度を決める。
sim_time: 1.0
　　どれくらい先を考慮するか
dwa: true
　　Dynamic Window Approach を使うか否か

(2) costmap_common_params.yaml

robot_radius: 0.165
　　円形ロボットの場合、ロボットの半径（メートル）。
　　円形で無いロボットの場合はfootprintパラメータを使う。
　　0.165はTurtlebotのサイズ
footprint: [[x0, y0], [x1, y1], [x2, y2], [x3, y3], etc]
　　ロボットの形状
inflation_radius: 0.3
　　ロボットが障害物にぶつからないようにするための膨張パラメータ

(3) global_costmap_params.yaml

global_frame: /map
　　global cost map のフレーム名称
robot_base_fame: /base_footprint
　　TurtleBot用には /base_footprint　を使う。
update_frequency: 1.0
　　global mapを更新する頻度
publish_frequency: 0
　　static globalマップは通常連続して配信する必要な無い
static_map: true
　　global map は静的
rolling_window: false
　　global map はロボットが動いても変更されないのでfalseにする
transform_tolerance: 1.0
　　ネットワークの遅延に対処するための待ち時間（秒）

(4) local_costmap_params.yaml

global_frame: /odom
　　local cost mapにおけるオドメトリのフレーム
robot_base_fame: /base_footprint
　　/base_link あるいは /base_footprintを設定
　　TurtleBot の場合は /base_footprint.
update_frequency: 3.0
　　local map の更新頻度
publish_frequency: 1.0
　　local map の配信頻度　　
static_map: false
　　local map は更新されるので、静的では無い。
rolling_window: true
　　local map は更新される。
width: 6.0
　　rolling map　x座標方向
height: 6.0
　　rolling map　y座標方向
Resolution: 0.01
　　rolling mapの解像度
transform_tolerance: 1.0
　　ネットワークの遅延に対処するための待ち時間（秒）

8.1 move_baseを使った経路探索と障害物回避

8.1 move_baseパッケージによる経路生成と障害物回避
これまでロボットを四角形に沿って動かすスクリプトを書いた。
move_baseパッケージは更に洗練された方法で同じことが実現できる。
move_baseパッケージはROS Action

8.1.1 move_baseパッケージにおけるゴールの指定方法
MoveBaseActionGoalメッセージによる
rosmsg show MoveBaseActionGoalで確認してください。
Point型の位置（x座標、y座標、z座標）と
Quaternion型の姿勢(z, y, z, w)で指定します。

8. ナビゲーション、経路探索とSLAM

Navigation Stackを構成する中心的なパッケージ

• move_base 目的に位置・姿勢にロボットを移動させる
• gmaping　地図作成
• amcl　地図を使って自己位置を求める

この3つがあれば、地図上の任意の場所に障害物を避けながら移動することが可能になる。

http://wiki.ros.org/navigation/Tutorials
http://wiki.ros.org/navigation/Tutorials/RobotSetup

7.10.2 ゲームパッドを使った実機ロボットの操縦

キーボードと同様にジョイスティックで実機ロボット（Kobuki)を動かしてみます。

$sudo apt-get install ros-hydro-joystick-drivers

ジョイスティックのドライバーをインストールしてあることを確認して。

7.10.1 キーボートを使った実機ロボットの操縦

いよいよ実機ロボットを動かしてみます。
私のところにあるのはKobukiベースのTurtlebot2なので本のコマンドとは多少違います。

必要なパッケージのインストール

次回以降、ジョイスティック等を使ってロボットのコントールも行うので、併せて必要なパッケージをインストールしておきます。

$ sudo apt-get install ros-hydro-kobuki*

$sudo apt-get install ros-hydro-joystick-drivers

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

始めに、Kobukiのドライバーをインストールします。RTのページを参考にしました。

$rosrun kobuki_ftdi create_udev_rules

スーパユーザ(sudo)のパスワードを求められるので、入力して下さい。 コマンドが成功して、KobukiのUSBケーブルを接続すれば、以降は /dev/kobuki で実機Kobukiにアクセスができます。

Kobukiを動かす

$roslaunch kobuki_node minimal.launch

別のTerminalウィンドウで

$roslaunch kobuki_keyop keyop.launch 

矢印キーを入力すると，kobukiが動きます．キーを長押しすると速度が増します．スペースキーで停止します．「q」キーで終了します．

Turtlebotのパッケージを使っても同じようなことができます。

$roslaunch turtlebot_bringup minimal.launch

別のTerminalウィンドウで

$roslaunch kobuki_keyop keyop.launch 

あるいは

roslaunch turtlebot_teleop keyboard_teleop.launch

他にもドッキングステーションへの帰還など様々な機能があります。

詳細はRTのページあるいはROSのチュートリアルページを御覧ください。

7.10 ロボットを操縦する

7.10 ロボットを操縦する
これまでの章で /cmd_vel ノードにTwistメッセージを配信することでロボットを動かしました。
ここでは、キーボードやゲームパッド等を使いロボットを動かしてみます。


7.10.3 ArbotiX コントール GUI

7.10.4 Interactive MarkersによるTurtleBotの操縦

7.10.5 オリジナルの操縦ノードを書く

7.9.4 デッドレコニングの問題点

ロボットが移動する際に、エンコーダの値（モータの回転量）などの内部センサにだけに頼り、外部のランドマークなどを利用しない手法をデッドレコニングと呼ぶ。

皆さんも御存知のように、デッドレコニングによる自己位置の推定は時間が経つにつれ徐々に誤差が蓄積され、やがては自己位置が解らなくなってしまいます。

例えば、自分の姿勢が1度ずれたまま、3m進むと約5cmの誤差を生じます。しかし、ロボットは自分の思っている位置と実際の正しい位置との誤差の5cmを知ることは出来ないのです。

ROSでは主にSLAMという手法を用いることで、このようなデッドレコニングの問題点を解決しようとしています。

7.9.3 四角形に沿って移動する nav_square.py を見てみる

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

ここまではお馴染みですね

class NavSquare():
    def __init__(self):
        # Give the node a name
        rospy.init_node('nav_square', anonymous=False)
        
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        rate = 20
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)

今回のスクリプトの周期は20Hzです

        # Set the parameters for the target square
        goal_distance = rospy.get_param("~goal_distance", 1.0)      # meters
        goal_angle = rospy.get_param("~goal_angle", radians(90))    # degrees converted to radians
        linear_speed = rospy.get_param("~linear_speed", 0.2)        # meters per second
        angular_speed = rospy.get_param("~angular_speed", 0.7)      # radians per second
        angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians

今回のスクリプトでは、rospy.get_param()でパラメータを設定しています。

パラメータに関するチュートリアルはこちらです。

値は.launchファイルで与えます。

 

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
         
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

base frameはTurtlebotの場合は base_footprint, PiRobotの場合は base_link です

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Set the odom frame
        self.odom_frame = '/odom'

オドメトリのフレームは　/odom です。

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")

/base_link あるいは /base_footprint

        # Initialize the position variable as a Point type
        position = Point()

        # Cycle through the four sides of the square
        for i in range(4):

位置を初期化して、四角形に一辺づつ動くので、繰り返しを4にします。

            # Initialize the movement command
            move_cmd = Twist()
            
            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed
            
            # Get the starting position values     
            (position, rotation) = self.get_odom()
                        
            x_start = position.x
            y_start = position.y

移動のためのTwistメッセージを初期化して、直進の移動スピードを設定して、現在のオドメトリの値を初期位置・姿勢に代入します。

            # Keep track of the distance traveled
            distance = 0
            
            # Enter the loop to move along a side
            while distance < goal_distance and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                
                r.sleep()
        
                # Get the current position
                (position, rotation) = self.get_odom()
                
                # Compute the Euclidean distance from the start
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))

指定した距離だけ動くために。

　　　速度をパブリッシュして、1サイクルだけスリープします。

　　　自己位置（オドメトリの値）を取得して、初期位置からの移動距離を求めます。

            # Stop the robot before rotating
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)
            
            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed
            
            # Track the last angle measured
            last_angle = rotation
            
            # Track how far we have turned
            turn_angle = 0
            
            # Begin the rotation
            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                
                r.sleep()
                
                # Get the current rotation
                (position, rotation) = self.get_odom()
                
                # Compute the amount of rotation since the last lopp
                delta_angle = normalize_angle(rotation - last_angle)
                
                turn_angle += delta_angle
                last_angle = rotation

同様に、90度回転します。

で、また直進、90度回転、直進、90度回転、直進、90度回転

これで四角形に沿って移動しました。

7.9 オドメトリを使った四角形に沿った移動

7.9 オドメトリを使った四角形に沿った移動
ここでは、四角形の頂点を4つの経由点とし、四角形に沿った移動を行ないます。

7.9.1 シミュレータによる四角形に沿った移動

7.9.2 実機ロボットによる四角形に沿った移動

7.8.4 /odom トピックか /odom フレームか？

トピック
トピックはROSのノード間で配信（発信）したり購読（受信）するデータのことです。

フレーム
ロボットシステムでは、一般的に、たくさんのワールドフレーム、ベースフレーム、グリッパーフレーム、ヘッドフレームなどなどの常に変化する座標系フレームを持ちます。
tfは常にこれらのフレームをトラックし、以下のようなことを聞きだせるようにしています。
　・5秒前のworldフレームから見たheadフレームはどこ？
　・基準に対してロボットの手にあるオブジェクトの形はどのような姿勢になっている？
　・マップフレームの中でベースフレームはどのような姿勢を今とっている？
tfはディストリビューテッドシステムの中で操作できます。
これは、ロボットの座標系フレームに関するすべての情報はシステムの中のすべてのコンピュータのROSのコンポーネントで利用できることを意味します。
このtfの情報(transform information)にとっては中心のサーバーとなるものは存在しません。
（ROS　チュートリアルより）

/odom トピックか/odomフレームか？
ロボットの位置・姿勢を扱う時に、トピックで扱う方法とフレームで扱う方法が考えられます。

7.8 オドメトリを使ったOut and Backスクリプト

7.8.1 シミュレータによるオドメトリを使った Out and Back
7.8.2 実機によるオドメトリを使った Out and Back
7.8.3 オドメトリを使った Out-and-Back のPythonスクリプト

Out and Back スクリプトは7.6.2 Pythonスクリプトでロボットを動かしてみる（シミュレーション編）で使ったスクリプトです。

Out and Backはロボットに一定時間だけ速度を与え続けることで、目的の距離だけ移動します。

ここでは、ロボットのオドメトリ（自己位置）を利用してロボットを動かすことを考えます。オドメトリは必ずしも正確ではありませんが、速度を一定時間だけ決め打ち（？）で与えるよりは正確に移動できます。

ここでは、オドメトリを使ったodom_out_and_back.pyスクリプトを見ていきます。

1　#!/usr/bin/env python
2
3　import rospy
4　from geometry_msgs.msg import Twist, Point, Quaternion
5　import tf
6　from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
7　from math import radians, copysign, sqrt, pow, pi

geometry_msgsパッケージに含まれる、Twist,Point,Quaternionメッセージタイプを使えるようにする。

/odom と /base_link (あるいは /base_footprint) を変換するための tf メッセージタイプを使えるようにする。

quat_to_angleはquaternion を オイラー角 (yaw) に変換する関数、

normalize_angle はremoves the ambiguity between 180 and -180 degrees as well as 0 and 360 degreesする関数で、いずれもROS by Exampleのサンプルコードに含まれる。

9　　class OutAndBack():
10 　　def __init__(self):
11 　　　# Give the node a name
12 　　　rospy.init_node('out_and_back', anonymous=False)
13
14 　　　# Set rospy to execute a shutdown function when exiting
15 　　　rospy.on_shutdown(self.shutdown)
16
17 　　　# Publisher to control the robot's speed
18 　　　self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
19
20 　　　# How fast will we update the robot's movement?
21 　　　rate = 20
22
23 　　　# Set the equivalent ROS rate variable
24 　　　r = rospy.Rate(rate)

/cmd_velトピックにTwist型の型のメッセージ（コマンド？）を送る宣言をします。 一秒間に20回のタイミングでコマンドをアップデートします。

26 　　　# Set the forward linear speed to 0.2 meters per second
27 　　　linear_speed = 0.2
28
29 　　　# Set the travel distance in meters
30 　　　goal_distance = 1.0
31
32 　　　# Set the rotation speed in radians per second
33 　　　angular_speed = 1.0

直進速度は秒速0.2メートルでゴールまでの距離は1メートル。

回転角度は1秒間に1ラジアン。

35 　　　# Set the angular tolerance in degrees converted to radians
36 　　　angular_tolerance = radians(2.5)

angular_tolerance は角度の許容誤差をラジアンで表した変数です。

実機のロボットは目的角度にピッタリ止めるのは難しく、目的角度周辺で振動してしまいます（行き過ぎて、逆回転して、戻りすぎて、逆回転して…）。

そこで、angular_toleranceに入ったらOKにしましょう、という値です。

38 　　　# Set the rotation angle to Pi radians (180 degrees)
39 　　　goal_angle = pi

回転角度の目標値は180度です。

41 　　　# Initialize the tf listener
42 　　　self.tf_listener = tf.TransformListener()

TransformListenerオブジェクトを生成する。

44 　　　# Give tf some time to fill its buffer
45 　　　rospy.sleep(2)

tfはバッファにデータが溜まるのに時間がかかるので、2秒スリープする。

47 　　　# Set the odom frame
48 　　　self.odom_frame = '/odom'
49

ロボットの位置・姿勢を取得するために、 /base_footprint あるいは /base_link を /odm フレームの変換する。

TurtleBot の場合は/base_footprint フレームを使う。

Pi Robot や Maxwell の場合は /base_link フレームを使う。

50 　　　# Find out if the robot uses /base_link or /base_footprint51　　　 try:
52 　　　　self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
53 　　　　self.base_frame = '/base_footprint'
54 　　　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
55 　　　　try:
56 　　　　　self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
57 　　　　　self.base_frame = '/base_link'
58 　　　　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
59 　　　　　rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
60 　　　　　rospy.signal_shutdown("tf Exception")

try:

   /base_footprint が見つかるか？

except:

  try:

    /base_link が見つかるか？

  except:

    どちらも見つからないので、エラーを出力して終了

self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))

waitForTransform()には４つの引数がある:

　取得したい変換のfromにあたるframe（ここでは /odm）

　取得したい変換のtoにあたるframe（ここでは /base_footprint）

　指定する時間

　時間切れ: この最大の期間よりは長く待たない（ここでは1.0秒）

/odmとbase_footprint'との間のtransformが利用可能になるか、transformが利用可能でない場合、時間切れが来るまでブロックします。

self.base_frame = '/base_footprint'

以降はbase_frame を使います。

62 　　　# Initialize the position variable as a Point type
63 　　　position = Point()

ロボットのポジジョンの為の変数を初期化します

66 　　　for i in range(2):

スクリプトでは同じことを2回繰り返します（1st legと2nd leg） それぞれのlegでは、ロボットは1メートル進んだあとに180度回転します。

67 　　　　# Initialize the movement command
68 　　　　move_cmd = Twist()

ロボットに移動速度を指示する変数を初期化します

70 　　　　# Set the movement command to forward motion
71 　　　　move_cmd.linear.x = linear_speed

ロボットに移動速を指示する変数に直進速度を代入します

73 　　　　# Get the starting position values
74 　　　　(position, rotation) = self.get_odom()
75
76 　　　　x_start = position.x
77 　　　　y_start = position.y

現在のオドメトリの値を取得して、初期位置として設定します。

self.get_odom() 関数を先に説明します。（113行目から定義されています）

130 def get_odom(self):
131 　# Get the current transform between the odom and base frames
132 　try:
133 　　(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
134 　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
135 　　　rospy.loginfo("TF Exception")
136 　　　return
137
138 　return (Point(*trans), quat_to_angle(Quaternion(*rot)))

オドメトリとベースフレームの現在の変換を見るためにtf_listenerを使います。
　(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
tf_listener.lookupTrasform 関数によりtransとrot（位置と姿勢）が求められたら、Point型の位置情報とQuaternion型の姿勢情報を返します。

transとrotの前に付いている*記号はPythonスクリプトの記法で、transではx座標, y座標, z座標 、rotではx,y,z,wのQuaternion型のリストデータを返します。

詳しくはPython言語の解説書を御覧ください。

79 　　　　# Keep track of the distance traveled
80 　　　　distance = 0
81
82 　　　　# Enter the loop to move along a side
83 　　　　while distance < goal_distance and not rospy.is_shutdown():
84 　　　　　# Publish the Twist message and sleep 1 cycle
85 　　　　　self.cmd_vel.publish(move_cmd)
86
87 　　　　　r.sleep()
88
89 　　　　　# Get the current position
90　　　　　 (position, rotation) = self.get_odom()
91
92 　　　　　# Compute the Euclidean distance from the start
93 　　　　　distance = sqrt(pow((position.x - x_start), 2) +
94 　　　　　　　　　　　　　　 pow((position.y - y_start), 2))

83行目から94行目までのループでロボットは1メートル進みます。

Twistメッセージ（速度の指示）をパブリッシュしたら、必ず一定時間（ここでは1サイクル）スリープを入れるのを忘れないで下さい。

96 　　　　# Stop the robot before the rotation
97 　　　　move_cmd = Twist()
98 　　　　self.cmd_vel.publish(move_cmd)
99 　　　　rospy.sleep(1)

直進移動が終わったあと、回転する前にロボットを一時停止します。

停止した後に、1秒間スリープします。

101 　　　 # Set the movement command to a rotation
102 　　　 move_cmd.angular.z = angular_speed
104   # Track the last angle measured
105   last_angle = rotation

回転速度を設定して、現在の姿勢を設定します。

107   # Track how far we have turned
108   turn_angle = 0
109
110 　while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
111　　　# Publish the Twist message and sleep 1 cycle
112 　　self.cmd_vel.publish(move_cmd)
113 　　r.sleep()
114
115 　　# Get the current rotation
116 　　(position, rotation) = self.get_odom()
117
118 　　# Compute the amount of rotation since the last loop
119 　　delta_angle = normalize_angle(rotation - last_angle)
120
121 　　# Add to the running total
122 　　turn_angle += delta_angle
123 　　last_angle = rotation

オドメトリを監視しながら、ロボットが指定の姿勢（角度）になるまで回転させます。

125 　# Stop the robot before the next leg
126 　move_cmd = Twist()
127 　self.cmd_vel.publish(move_cmd)
128 　rospy.sleep(1)
129
130 # Stop the robot for good
131 self.cmd_vel.publish(Twist())

ROS by Example本では64ページの’プログラムのリストの125行目から131行目までのインデントが間違っているので注意。

付録のサンプルプログラムは正しいのでちゃんと動きます。

ここでは回転が終わったら次のlegの準備の為にロボットを静止させて、1秒間スリープします。

さらに1st、2nd のleg が終わったらロボットを停止させます。

7.7 オドメトリを使って自己位置を知る

移動ロボットが自由自在に動き回るには正確な自己位置を知ることが重要です。ロボットに装備された様々センサ（車輪の回転角度、画像、距離センサ、GPS…）を使い、自己位置を求める手法をオドメトリあるいはデッドレコニングと言います。

ROSでは nav_msgs/Odometry という型のメッセージでオドメトリを表現しています。
nav_msgs/Odometry 型の詳細は以下のコマンドで確認できます。

$rosmsg show nav_msgs/Odometry

Header header 

　uint32 seq 

　time stamp

　string frame_id 

string child_frame_id

geometry_msgs/PoseWithCovariance pose 

　geometry_msgs/Pose pose

　　geometry_msgs/Point position 

　　　float64 x

　　　float64 y 

　　　float64 z

　　geometry_msgs/Quaternion orientation 

　　　float64 x

　　　float64 y 

　　　float64 z 

　　　float64 w

　float64[36] covariance 

geometry_msgs/TwistWithCovariance twist

　geometry_msgs/Twist twist 

　　geometry_msgs/Vector3 linear

　　　float64 x 

　　　float64 y 

　　　float64 z

　　geometry_msgs/Vector3 angular 

　　　float64 x

　　　float64 y 

　　　float64 z

　float64[36] covariance

ROSのナビゲーションに関数チュートリアルは以下のページを参照してください。日本語訳が進行中（？）です。

http://wiki.ros.org/ja/navigation/Tutorials

7.6.2 Pythonスクリプトでロボットを動かしてみる（シミュレーション編）

timed_out_and_back.py を参考にPythonスクリプトでロボットを動かすプログラムを見てみましょう。

1 #!/usr/bin/env python
2
3　import rospy

この2行はPythonスクリプトでROSプログラム書くときのお決まりです

4　from geometry_msgs.msg import Twist5　from math import pi

Twist型のメッセージを使います

7　class OutAndBack():
8 　　def __init__(self):

OutAndBackクラスを作ります

9 　　　 # Give the node a name
10 　　　rospy.init_node('out_and_back', anonymous=False)
11
12 　　　# Set rospy to execute a shutdown function when exiting
13 　　　rospy.on_shutdown(self.shutdown)

out_and_backノードを初期化します。
スクリプトが終了するときに呼ばれるコールバックon_shutdown()を宣言します。例えば、CtrlCtrl+cキーが押された時に呼ばれます。

15 　　　# Publisher to control the robot's speed
16 　　　self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
17
18 　　　# How fast will we update the robot's movement?
19 　　　rate = 50
20
21 　　　# Set the equivalent ROS rate variable
22 　　　r = rospy.Rate(rate)

/cmd_velトピックにTwist型の型のメッセージ（コマンド？）を送る宣言をします。

一秒間に50回のタイミングでコマンドをアップデートします。

24 　　 # Set the forward linear speed to 0.2 meters per second
25 　　 linear_speed = 0.2
26
27 　　 # Set the travel distance to 1.0 meters
28 　　 goal_distance = 1.0
29
30 　　 # How long should it take us to get there?
31 　　 linear_duration = linear_distance / linear_speed

ロボットの前進速度を秒速0.2メートルに設定します。

ゴールまでの距離を1.0メートルに設定します。

ゴールまでの到達時間を求めます。1.0／0.2＝5秒

33 　　　# Set the rotation speed to 1.0 radians per second
34 　　　angular_speed = 1.0
35
36 　　　# Set the rotation angle to Pi radians (180 degrees)
37 　　　goal_angle = pi
38
39 　　　# How long should it take to rotate?
40 　　　angular_duration = angular_distance / angular_speed
この行は間違えなので、以下のように修正する。付属のスクリプトは正しい
40      angular_duration = goal_angle / angular_speed

同じように、ロボットの回転角速度を毎秒1.0ラジアンに設定し、

目標とする回転角度を180度（πラジアン）に設定すると、

目標とする回転角度までの時間は pi (3.14159263) / 1.0 = 3.14秒

42 　　　# Loop through the two legs of the trip
43 　　　for i in range(2):
44 　　　　# Initialize the movement command
45 　　　　move_cmd = Twist()
46
47 　　　　# Set the forward speed
48 　　　　move_cmd.linear.x = linear_speed
49
50 　　　　# Move forward for a time to go the desired distance
51 　　　　ticks = int(linear_duration * rate)
52
53 　　　　for t in range(ticks):
54 　　　　　self.cmd_vel.publish(move_cmd)
55 　　　　　r.sleep()

このループで実際にロボット動かします。一連の動きを２回繰り返します。

ロボットを１秒の間に、linear_speedの速度で、linear_distanceの距離だけ動かすために、1/rate 秒の間隔でTwist型のmove_cmd メッセージを出力し続けます。

 r.sleep()では非常に短い時間（1/rate）だけスリープします。1/rateという時間は、r=rospy.Rate(rate)で設定された値になります。

57 　　　　　# Stop the robot before the rotation
58 　　　　　move_cmd = Twist()
59 　　　　　self.cmd_vel.publish(move_cmd)
60 　　　　　rospy.sleep(1)

ここではループの後半に入る前に、ロボットを一旦停止させ、スクリプトを1秒間停止させます。

62 　　　　　　# Now rotate left roughly 180 degrees
63
64 　　　　　　# Set the angular speed
65 　　　　　　move_cmd.angular.z = angular_speed
66
67 　　　　　　# Rotate for a time to go 180 degrees
68 　　　　　　ticks = int(goal_angle * rate)
69
70 　　　　　　for t in range(ticks):
71 　　　　　　　　self.cmd_vel.publish(move_cmd)
72 　　　　　　　　r.sleep()

ループの後半はロボットを回転させます。

１秒間で180度回転します。

74 　　　　# Stop the robot before the next leg
75 　　　　move_cmd = Twist()
76 　　　　self.cmd_vel.publish(move_cmd)
77 　　　　rospy.sleep(1)

2度めのループに入る前に、ロボットを一旦停止させ、スクリプトを1秒間停止させます。

79 　　　# Stop the robot.
80 　　　self.cmd_vel.publish(Twist())

2度のループが終わったら、ロボットに空（すべてが0）のTwistメッセージ送る。

82 def shutdown(self):
83 　　# Always stop the robot when shutting down the node.
84 　　rospy.loginfo("Stopping the robot...")
85 　　self.cmd_vel.publish(Twist())
86 　　rospy.sleep(1)

何らかの理由でシャットダウン（Ctrl+cが押された）が実行された時はこのコールバック関数が実行される。

Tearminalウィンドウに”Stopping the robot..." が表示され、空のTwistメッセージが送られ、1秒間ループしたのちにスクリプトは終了する。

さて、timed_out_and_back.pyの動作をシミュレータで確認してみましょう。

$roslaunch rbx1_bringup fake_turtlebot.launch

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

$rosrun rbx1_nav timed_out_and_back.py

この3つのコマンドをそれぞれ別のTerminalウィンドウで実行して下さい。もう一つ別のウィンドウで $roscore を実行するのを忘れないでください。

7.6 ROSノードからTwistメッセージを発信する

7.6ではPythonスクリプトからTwistメッセージを/cmd_vel に配信することで、ロボットを動かしてみます。
　
7.6.1 時間あるいは速度により移動距離や回転角度を指定する
「ロボットを1m動かしたかったら、0.1m毎秒の速度を10秒間配信する」のようなプログラムを作成します。

実際に1m正確に動くかどうかはロボットのオドメトリの精度に影響されます。
このように、オドメトリだけでロボットを動かすのは、タイヤの滑りや障害物への衝突などの影響で正確な移動は望めません。
しかし、プログラムは簡単なので入門編ではまずこの方法で動かしてみましょう。

7.5 Twistメッセージを実機ロボットに送る

Twistメッセージはロボットに移動速度を与えるメッセージであり、頻繁に使用されます。
Terminal画面で以下のように実行すると詳細が表記されます。

$ rosmsg show geometry_msgs/Twist

6つのパラメータの前半3つは並進方向、後半3つは回転方向の速度を指示するように使い、
それぞれ、x軸方向の並進速度、y軸方向の並進速度、z軸方向の並進速度、
x軸回りの回転速度、y軸回りの回転速度、z軸回りの回転速度のように使います。

以前の記事で、シミュレータ上のロボットを動かすために以下のようなコマンドを実行しました。
この時使ったのがTwistメッセージです。
以下のコマンドでは、秒速0.5ラジアンで回転しながら、秒速0.2mの速度で前に進んでいるはずです。
$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

ここでは実機ロボット（Kobuki）に直接、Twistメッセージを送り、Kobukiが移動するのを確認します。

記事に順番が逆になりますが、「7.10.1 キーボートを使った実機ロボットの操縦」を読んで、Kobukiのパッケージやドライバーをインストールしておいてください。

$ roslaunch kobuki_node minimal.launch

Kobukiの制御ノードを立ち上げ、以下のようにTwistメッセージを送ります。

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

Kobukiを止めたい時は、Ctl+cキーを押して、以下のコマンドを入力してください。

$rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'

7.4 オドメトリのキャリブレーション

実機ロボットのオドメトリをより正確なものにするために、キャリブレーションを行ないます。
7.4.1 並進方向のキャリブレーションおよび7.4.2 回転方向のキャリブレーションを参考にキャリブレーション用のパラメータを求めたら、以下のように設定して下さい。

床の材質によってキャリブレーションの値は大きく変化します。場所ごとにキャリブレーションを行って、以下のlaunchファイルを複数用意しておくのがいいでしょう。

TurtleBotの場合

turtlebot_carpet_create.launch を編集して、以下のように数値を変更して下さい。

<launch>
  <param name="/use_sim_time" value="false" />
  <!-- Calibration parameters after calibration of an original Create TurtleBot on low-ply carpet -->
  <param name="turtlebot_node/gyro_measurement_range" value="150"/>
  <param name="turtlebot_node/gyro_scale_correction" value="1.43"/>
  <param name="turtlebot_node/odom_angular_scale_correction" value="1.0 "/>
  <param name="turtlebot_node/odom_linear_scale_correction" value="0.955"/>

  <include file="$(find rbx1_bringup)/launch/turtlebot_minimal_create.launch" />

</launch>

以降は、キャリブレーション済みの launchファイルを使って下さい。

$ roslaunch rbx1_bringup turtlebot_carpet_create.launch

Kobukiの場合

turtlebot_carpet_create.launch をコピーして、turtlebot_carpet_kobuki.launch を作成し、以下のように変更して下さい。

<launch>
  <param name="/use_sim_time" value="false" />
  <!-- Calibration parameters after calibration of an original Create TurtleBot on low-ply carpet -->
  <param name="turtlebot_node/gyro_measurement_range" value="150"/>
  <param name="turtlebot_node/gyro_scale_correction" value="1.43"/>
  <param name="turtlebot_node/odom_angular_scale_correction" value="1.0 "/>
  <param name="turtlebot_node/odom_linear_scale_correction" value="0.955"/>

  <include file="$(find rbx1_bringup)/launch/turtlebot_minimal_kobuki.launch" />

以降は、キャリブレーション済みの launchファイルを使って下さい。

$ roslaunch rbx1_bringup turtlebot_carpet_kobuki.launch

7.3.2 RVizによるロボットの可視化

RVizは強力な可視化ツールです。
ここでは、シミュレータを使い、Twistメッセージによりロボットが動く様子を可視化してみます。

シミュレータでTurtlebotを動かして

$roslaunch rbx1_bringup fake_turtlebot.launch

別のターミナルウィンドウで、rvizを動かします。

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

さらに別のターミナルウィンドウで以下のTwistメッセージを配信します

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

以下の図のように反時計方向に0.1ラジアン毎秒で回転しながら、0.1メートル毎秒で直進します。

rostopic の -r 10 オプションは10Hz（毎秒10回）の間隔でTwistメッセージを /cmd_velに発行することを意味します。

ROSでは「安全第一の設計」がされていて、Twistメッセージを送り続けないとロボットは動きません。

ロボットの動きを止めたい時は、以下のように空のTwistメッセージ発行してください。

$rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'

7.3 Twistメッセージによるロボットの移動

ROSでは base controller へのモーションコマンドとして Twist型のメッセージを使います。
頻繁に見かけるのは、以下の様な例です。
「”command velocities"と呼ばれる/cmd_vel ノードにTwistが型のメッセージを配信する」


7.3.1 Twist型メッセージの例

以下のコマンドでTwist型メッセージの詳細が表示されます

$ rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z

geometry_msgs/Twist 型は二つのサブメッセージから成り、それぞれ float64型が3つからなるベクター（配列）型です。

geometry_msgs/Vector3 linear  は並進方向の速度を意味し、それぞれ x, y, z 方向の速度をメートル毎秒で与えます。

また、geometry_msgs/Vector3 angular  は回転方向の速度を意味し、それぞれ x, y, z 軸回りの回転速度をラジアン毎秒で与えます。

例えば、ロボットを0.1メートル/秒で前進させたいときには、

'{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

※ここで、サブメッセージ内の3つの値のデリミタは「コロン＋スペース」なのを注意して下さい。

x: 0.1 (x + : +  スペース＋数値）です。：の後にスペースを忘れないように。

同様に、ロボットをその場で反時計方向に回転させるには以下のようにします。

'{linear: {x: 0.0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.1}}'

実際にROSのコマンドでロボットを移動させるには以下の様に入力します。

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

時計方向に0.5ラジアン毎秒で回転しながら、0.1メートル毎秒で移動します。

7.2 移動制御の抽象化のレベル

7.2 モーションコントロール
ROSでは「与えらえたゴールに移動する」というタスクに関して、モーターへの指示コマンド（抽象度が低い）から移動先のゴール（位置と姿勢）を与える（抽象度が高い）までを幾つかの抽象度に分解して実装しています。
以下の節では、抽象度の低い順に紹介されています。

7.2.1 モータ, 車輪, エンコーダ

移動ロボットの制御においては、オドメトリ（内部センサによる移動量の推定）が重要になり、エンコーダから得られるモータの回転数だけでなく、加速度計などの移動量を測定できるセンサを使うこともあります。

例えば、Turtlebotで使っているRoombaは2つのモータの回転数に加え、一軸のジャイロにより姿勢の推定精度を向上させています。

7.2.2 モータのコントローラとドライバ
移動ロボットの一番低レベルの制御は各モータのスピードを制御することです。
車輪のモータで速度を決めだけで無く、ステアリングのモータを制御して進行方向を変えたり、パンチルト台のモータを制御してカメラが見ている方向を変えることなども含まれます。このレベルは個々のロボット毎に大きく異なります。
モータの数や車輪の大きさはモータ毎に違うので、このレベルにおいてはロボット毎に異なるドライバを実装するのが普通です。

7.2.3 ロボット台車の制御
このレベルでは上位レベルから指示された通りに台車を制御することが要求されます。
例えば、「xxメータ/秒で動かす」「xxラジアン/秒で回転させる」などをいかに正確に実現するかが求められます。
指示された制御量にスムーズに追随するため、PD制御やPID制御と言われるドライバが実装されています。
例えば、あなたのロボットが静止状態にあり、「0.5m/sec（秒速50㎝）で前に進め」という指示をしたいとします。
その時、静止状態のロボットにいきなり0.5m/secの速度指定コマンドを送るのは現実的ではありません。急発進して、場合によっては倒れてしまうかもしれません。
そこで、ROSのBase Controllerノードでは、PID制御のような手法を使って、「スムーズに」指定された速度になるまでロボットの速度コマンドを細分化します。
例えば、以下のように一定時間間隔で徐々に速度を上げていきます。　
0.0m/sec ->0.05m/sec -> 0.1m/sec -> 0.15m/sec ..... -> 0.5m/sec

この base controller ノードと呼ばれるノードからは自己位置（オドメトリ）が /odom トピックとして配信され、/cmd_vel  ノードでは上位レベルからの移動速度を待っています。
同時に、コントローラノードは、一般的に（常にではないが）ベースフレーム（/ base_linkまたは/ base_footprintのいずれか）へ/odmを変換しています。

TurtleBotのようないくつかのロボットでは、ロボットの位置・姿勢のより正確な推定値を取得するために、車輪のオドメトリとジャイロデータを結合するrobot_pose_ekfパッケージを使用します。この場合には、robot_pose_ekfノードが/odom から/ base_footprintに変換しています。（ robot_pose_ekfパッケージは拡張カルマンフィルタにより実装されています。）

このように、自分のロボット専用の base controller ノードを作ってしまえば、後は、上位レベルのプログラミングに専念することが出来ます。

7.2.4 move_base ROS パッケージによるFrame-Base Motion
move_baseパッケージは目的地（一般的な移動ロボットでは移動先の位置・姿勢）を与えられたときに現在地から目的地まで、障害物を回避しながら経路を生成し、移動する機能を提供します。
move_baseパッケージは、非常に洗練された経路計画であり、ロボットが移動するためのパスを選択する際に、ローカルおよびグローバルなコストマップの両方とオドメトリデータを組み合わせを利用します。
7.2.5 gmapping と amcl パッケージによるSLAM
gmappingパッケージにより地図を作成します。一般に、SLAMで地図を作るにはレーザレンジファインのような正確な距離センサが必要ですが、Turtlebotに搭載のKinectやXtionでも十分な精度の地図を得ることが可能です。

地図が作れたら、amcl (adaptive Monte Carlo localization) パッケージを使い正確な自己位置・姿勢を得ることができます。
7.2.6 セマンティックゴール　Semantic Goals
抽象化の最上位のこのレベルでは、
「キッチンに行って、ビールを取ってきて」

とか、もっと曖昧に
「ビールを取ってきて」
のように、実現してもらいたいGoalをロボットに指示します。
7.2.7 まとめ

[Goal]
↓
[AMCL]
↓
[Path Planner]
↓
[move_base]
↓
[/cmd_vel + /odom]
↓
[Base Controller]
↓
[Motor Speeds]