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