waypoint_nav

このノードは、waypoint_saverで記録したウェイポイントを基にナビゲーションを行なうためのものです。move_base の Action API を利用しているため、別途 move_base を起動しておく必要があります。 また、速度調整を実現するために velocity_controller と同時に起動することを前提としています。 オプションで、tandem_run_manager を起動することで、縦列に対応することができます。

1. Subscribed Topics

• cmd_vel (geometry_msgs/Twist)
  ロボットへの速度指令。ロボットが動いているか、静止しているかをチェックする。

• scan (sensor_msgs/LaserScan)
  ウェイポイント周辺に障害物があるかチェックする。

2. Published Topics

• waypoint_num (std_msgs/UInt16)
  システム全体で、何番目のウェイポイントに向かっているかを共有する。

• max_vel (std_msgs/Float32)
  velocity_controller に最大速度を設定する。

• waypoints (geometry_msgs/PoseArray)
  rvizにウェイポイントを表示する。

3. Services

• start_wp_nav (std_srvs/Trigger)
  ナビゲーションの開始。

• stop_wp_nav (std_srvs/Trigger)
  ナビゲーションの一時停止。

• resume_nav (std_srvs/Trigger)
  ナビゲーションの再開。

start_wp_nav, resume_nav は rviz_plugins によりrviz上に表示されるボタンからコールされる。 stop_wp_nav は、システム内部からコールされる。

4. Parameters

• ~robot_frame (string, default: "base_footprint")
  ロボット本体のフレーム名。

• ~world_frame (string, default: "map")
  マップのフレーム名

• ~max_update_rate (float, default: 10.0)
  メインループのレート。

• ~filename (string, default: "")
  ウェイポイントを記述したyamlファイルのパス。

• ~StartFromTheMiddle (bool, default: false)
  近くのウェイポイントから始めるかどうか。

• ~min_dist_err (float, default: 0.3)
  ウェイポイントのパラメータ stop により停止するときの、到達判定距離(m)。

• ~min_yaw_err (float, default: 0.3)
  ウェイポイントのパラメータ stop により停止するときの、到達判定角度(rad)。

5. Required tf Transforms

• (value of "~world_frame" parameter) → (value of "~robot_frame" parameter)

6. Required YAML Format

基本的に、waypoint_nav により出力したYAMLファイルを利用します。

waypoints:
- point: {x: 1.0, y: 0.0, z: 0.0, vel: 1.0, rad: 0.8, stop: false}
- point: {x: 2.0, y: 0.0, z: 0.0, vel: 1.0, rad: 0.8, stop: true}
- point: {x: 3.0, y: 0.0, z: 0.0, vel: 1.0, rad: 0.8, stop: false}
finish_pose:
  header: {seq: 0, stamp: 113.132, frame_id: map}
  pose:
    position: {x: 4.0, y: 0.0, z: 0.0}
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}

各ウェイポイントには、要素 x, y, z が必須となります。 これに加え、以下の3つの要素を追加することができます。

1. vel (float, default: 1.0)
   そのウェイポイントに向かうまでの速度を、velocity_controller のパラメータ max_vel_param に指定した値 * vel に制限できます。

2. rad (float, default: 0.8)
   そのウェイポイントへの到達判定距離(m)を指定できます。

3. stop (bool, default: false)
   そのウェイポイント上で停止するかを指定できます。このとき、到達判定距離にはパラメータ ~min_dist_err の値が使用され、上記の rad の値は無視されます。 ナビゲーションを再開するには、resume_nav サービスを利用します。