差動２輪移動ロボットのナビゲーション

前ページで作った（差動２輪移動ロボットの作り方）ロボットのナビゲーションを行う．

移動ロボットは，速度司令として（x方向速度，y方向速度，theta回転速度）を与える必要がある．

詳しくは本家のHPを見てください．

移動ロボットのナビゲーションには，move_baseノードの機能を利用する．

move_baseは主に，global_plannerとlocal_plannerと呼ばれる大域的な経路計画と局所的な経路計画の２つの機能から成り立つ．それぞれ，地図中のコストを管理するglobal cost mapとlocal cost map に接続して周囲の状況を判断する．

move_baseはSimpleActionServerを備えており，目的位置を指示するだけで目的位置までのナビゲーションを行ってくれる．

move_baseがインストールされていない場合は下記のコマンドでインストール．

$ sudo apt-get install ros-indigo-navigation (indigoの場合）

$ sudo apt-get install ros-kinetic-navigation (kineticの場合）

移動ロボットのナビゲーションを行うには地図が必要なので，まずgmappingを使って地図を作成する．

下記コマンドで，作動型移動ロボットを作ったディレクトリのlaunchディレクトに地図作成のためのlaunchファイルを作成する．

$ cd ~/catkin_ws/src/diff_mobile_robot/launch

$ gedit gmapping.launch

gmapping.launchの中身は下記の通り．

gmapping.launch

<launch>

<arg name="scan_topic" default="scan" />

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

<param name="base_frame" value="base_footprint"/>

<param name="odom_frame" value="odom"/>

<param name="map_update_interval" value="5.0"/>

<param name="maxUrange" value="6.0"/>

<param name="maxRange" value="8.0"/>

<param name="sigma" value="0.05"/>

<param name="kernelSize" value="1"/>

<param name="lstep" value="0.05"/>

<param name="astep" value="0.05"/>

<param name="iterations" value="5"/>

<param name="lsigma" value="0.075"/>

<param name="ogain" value="3.0"/>

<param name="lskip" value="0"/>

<param name="minimumScore" value="100"/>

<param name="srr" value="0.01"/>

<param name="srt" value="0.02"/>

<param name="str" value="0.01"/>

<param name="stt" value="0.02"/>

<param name="linearUpdate" value="0.5"/>

<param name="angularUpdate" value="0.436"/>

<param name="temporalUpdate" value="-1.0"/>

<param name="resampleThreshold" value="0.5"/>

<param name="particles" value="80"/>

<!--

<param name="xmin" value="-50.0"/>

<param name="ymin" value="-50.0"/>

<param name="xmax" value="50.0"/>

<param name="ymax" value="50.0"/>

make the starting size small for the benefit of the Android client's memory...

-->

<param name="xmin" value="-1.0"/>

<param name="ymin" value="-1.0"/>

<param name="xmax" value="1.0"/>

<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>

<param name="llsamplerange" value="0.01"/>

<param name="llsamplestep" value="0.01"/>

<param name="lasamplerange" value="0.005"/>

<param name="lasamplestep" value="0.005"/>

<remap from="scan" to="$(arg scan_topic)"/>

</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

<rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />

<rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />

<rosparam file="$(find diff_mobile_robot)/param/local_costmap_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/global_costmap_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/base_local_planner_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/dwa_local_planner_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/move_base_params.yaml" command="load" />

<!-- <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>

<remap from="odom" to="$(arg odom_topic)"/> -->

</node>

</launch>

作ったlaunchをすぐに実行したいところだが，launchファイル内で参照しているパラメータのファイルを作っていないので下記のように作成．

$ cd ~/catkin_ws/src/diff_mobile_robot

$ mkdir param

$ cd param

$ gedit costmap_common_params.yaml

costmap_common_params.yaml

# Customized for ArloBot

max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot

obstacle_range: 2.5

raytrace_range: 3.0

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)

#robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18)

robot_radius: 0.4509 # ArloBot

# footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular

inflation_radius: 0.50 # max. distance from an obstacle at which costs are incurred for planning paths.

cost_scaling_factor: 5 # exponential rate at which the obstacle cost drops off (default: 10)

# voxel map configuration; z-voxels 0 are filled by bumpers and 1 by laser scan (kinect)

map_type: costmap

transform_tolerance: 5 # seconds

origin_z: 0.0

z_resolution: 0.2

z_voxels: 2

publish_voxel_map: false

observation_sources: scan bump

#observation_sources: scan

# scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}

# Our lasers (Xtion and fake) either needs to publish a height, or set min_obstacle_height to 0.0:

# http://wiki.ros.org/navigation/Troubleshooting#Missing_Obstacles_in_Costmap2D

# Note taht the max_obstacle_height is very important too!

scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.0, max_obstacle_height: 3}

# Can we just set up two of these here?

bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15}

ほかにも，同じディレクトリに次の5つのファイルを作成する．

local_costmap_params.yaml, global_costmap_params.yaml,base_local_planner_params.yaml,

dwa_local_planner_params.yaml, move_base_params.yaml

local_costmap_params.yaml

local_costmap:

global_frame: odom

robot_base_frame: /base_footprint

update_frequency: 5.0

publish_frequency: 2.0

static_map: false

rolling_window: true

width: 4.0

height: 4.0

resolution: 0.05

transform_tolerance: 0.5

global_costmap_params.yaml

global_costmap:

global_frame: /map

robot_base_frame: /base_footprint

update_frequency: 1.0

publish_frequency: 0.5

static_map: true

transform_tolerance: 0.5

base_local_planner_params.yaml

TrajectoryPlannerROS:

# Robot Configuration Parameters

max_vel_x: 0.3

min_vel_x: 0.1

max_vel_theta: 1.0

min_vel_theta: -1.0

min_in_place_vel_theta: 0.6

acc_lim_x: 0.5

acc_lim_theta: 1.0

# Goal Tolerance Parameters

yaw_goal_tolerance: 0.3

xy_goal_tolerance: 0.15

# Forward Simulation Parameters

sim_time: 3.0

vx_samples: 6

vtheta_samples: 20

# Trajectory Scoring Parameters

meter_scoring: true

pdist_scale: 0.6

gdist_scale: 0.8

occdist_scale: 0.01

heading_lookahead: 0.325

dwa: true

# Oscillation Prevention Parameters

oscillation_reset_dist: 0.05

# Differential-drive robot configuration

holonomic_robot: false

max_vel_y: 0.0

min_vel_y: 0.0

acc_lim_y: 0.0

vy_samples: 1

dwa_local_planner_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters - Kobuki

max_vel_x: 0.5 # 0.55

min_vel_x: 0.0

max_vel_y: 0.0 # diff drive robot

min_vel_y: 0.0 # diff drive robot

max_trans_vel: 0.5 # choose slightly less than the base's capability

min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity

trans_stopped_vel: 0.1

# Warning!

# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities

# are non-negligible and small in place rotational velocities will be created.

max_rot_vel: 5.0 # choose slightly less than the base's capability

min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity

rot_stopped_vel: 0.4

acc_lim_x: 1.0 # maximum is theoretically 2.0, but we

acc_lim_theta: 2.0

acc_lim_y: 0.0 # diff drive robot

# Goal Tolerance Parameters

yaw_goal_tolerance: 0.3 # 0.05

xy_goal_tolerance: 0.15 # 0.10

# latch_xy_goal_tolerance: false

# Forward Simulation Parameters

sim_time: 1.0 # 1.7

vx_samples: 6 # 3

vy_samples: 1 # diff drive robot, there is only one sample

vtheta_samples: 20 # 20

# Trajectory Scoring Parameters

path_distance_bias: 64.0 # 32.0 - weighting for how much it should stick to the global path plan

goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal

occdist_scale: 0.50 # 0.01 - weighting for how much the controller should avoid obstacles

forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point

stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj.

scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint

max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed.

# Oscillation Prevention Parameters

oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags

# Debugging

publish_traj_pc : true

publish_cost_grid_pc: true

global_frame_id: odom

# Differential-drive robot configuration - necessary?

# holonomic_robot: false

move_base_params.yaml

# Move base node parameters. For full documentation of the parameters in this file, please see

#

# http://www.ros.org/wiki/move_base

#

shutdown_costmaps: false

controller_frequency: 5.0

controller_patience: 3.0

planner_frequency: 1.0

planner_patience: 5.0

oscillation_timeout: 10.0

oscillation_distance: 0.2

# local planner - default is trajectory rollout

base_local_planner: "dwa_local_planner/DWAPlannerROS"

これで準備が整ったので，下記のコマンドでgmapping.launchを起動．

gmappingが入っていない可能性があるので

下記コマンドを先に実行してインストール確認

$ sudo apt-get install ros-kinetic-gmapping

$ roslaunch diff_mobile_robot diff_mobile_gazebo.launch (１つ目の端末で実行：シミュレータ起動用）

$ roslaunch diff_mobile_robot gmapping.launch （２つ目の端末で実行：地図作成用）

$ roslaunch diff_mobile_robot keyboard_teleop.launch（3つ目の端末で実行：移動ロボットコントロール用）

最初にgazeboを立ち上げたら，周りに障害物がないので下記の画面のようにgazeboの画面の左側にある「Insert」タブを選んで，その中にある「Willow Garage」を選ぶ．そうすると，建物のフロアのオブジェクトが設置される．

毎回設置場所が異なると困る場合は，ロボットアームのgazeboでのシミュレーション例でガソリンスタンドを置いたようにファイル内に記述する必要がある．

建物の設置が終わったら，キーボードで移動ロボットを動かしてみる．rviz内にmapをAddして表示しておくと，地図が更新されていく様子が確認できる．

課題

１．Willow Garageの建物が毎回同じ位置に生成されるようにGazeboによるマニピュレータの動力学シミュレーションのページを参考に修正せよ．

２．地図をある程度作成した後に，地図データを下記コマンドで保存せよ．

$ cd ~/catkin_ws/src/diff_mobile_robot

$ mkdir map

$ cd map

$ rosrun map_server map_saver -f testmap

最後のtestmapは保存するファイル名．

AMCLによる移動ロボットの自己位置推定

下記のように自己位置推定のノードを使うためのlaunchファイルを生成する．

$ cd ~/catkin_ws/src/diff_mobile_robot/launch

$ mkdir includes

$ cd includes

$ gedit move_base.launch.xml

move_base.launch.xml

<!--

ROS navigation stack with velocity smoother and safety (reactive) controller

-->

<launch>

<!--

<include file="$(find diff_mobile_robot)/launch/includes/velocity_smoother.launch.xml"/>

<include file="$(find diff_mobile_robot)/launch/includes/safety_controller.launch.xml"/>

-->

<arg name="odom_topic" default="odom" />

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

<rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />

<rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />

<rosparam file="$(find diff_mobile_robot)/param/local_costmap_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/global_costmap_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/base_local_planner_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/dwa_local_planner_params.yaml" command="load" />

<rosparam file="$(find diff_mobile_robot)/param/move_base_params.yaml" command="load" />

<!--

<remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>

<remap from="odom" to="$(arg odom_topic)"/>

-->

</node>

</launch>

同じディレクトリにgmapping.launch.xmlを作る．

gmapping.launch.xml

<launch>

<arg name="scan_topic" default="scan" />

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

<param name="base_frame" value="base_footprint"/>

<param name="odom_frame" value="odom"/>

<param name="map_update_interval" value="5.0"/>

<param name="maxUrange" value="6.0"/>

<param name="maxRange" value="8.0"/>

<param name="sigma" value="0.05"/>

<param name="kernelSize" value="1"/>

<param name="lstep" value="0.05"/>

<param name="astep" value="0.05"/>

<param name="iterations" value="5"/>

<param name="lsigma" value="0.075"/>

<param name="ogain" value="3.0"/>

<param name="lskip" value="0"/>

<param name="minimumScore" value="100"/>

<param name="srr" value="0.01"/>

<param name="srt" value="0.02"/>

<param name="str" value="0.01"/>

<param name="stt" value="0.02"/>

<param name="linearUpdate" value="0.5"/>

<param name="angularUpdate" value="0.436"/>

<param name="temporalUpdate" value="-1.0"/>

<param name="resampleThreshold" value="0.5"/>

<param name="particles" value="80"/>

<!--

<param name="xmin" value="-50.0"/>

<param name="ymin" value="-50.0"/>

<param name="xmax" value="50.0"/>

<param name="ymax" value="50.0"/>

make the starting size small for the benefit of the Android client's memory...

-->

<param name="xmin" value="-1.0"/>

<param name="ymin" value="-1.0"/>

<param name="xmax" value="1.0"/>

<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>

<param name="llsamplerange" value="0.01"/>

<param name="llsamplestep" value="0.01"/>

<param name="lasamplerange" value="0.005"/>

<param name="lasamplestep" value="0.005"/>

<remap from="scan" to="$(arg scan_topic)"/>

</node>

</launch>

さらに，同じディレクトリにamcl.launch.xmlを作る．

amcl.launch.xml

<launch>

<arg name="use_map_topic" default="false"/>

<arg name="scan_topic" default="scan"/>

<arg name="initial_pose_x" default="0.0"/>

<arg name="initial_pose_y" default="0.0"/>

<arg name="initial_pose_a" default="0.0"/>

<node pkg="amcl" type="amcl" name="amcl">

<param name="use_map_topic" value="$(arg use_map_topic)"/>

<!-- Publish scans from best pose at a max of 10 Hz -->

<param name="odom_model_type" value="diff"/>

<param name="odom_alpha5" value="0.1"/>

<param name="gui_publish_rate" value="10.0"/>

<param name="laser_max_beams" value="60"/>

<param name="laser_max_range" value="12.0"/>

<param name="min_particles" value="500"/>

<param name="max_particles" value="2000"/>

<param name="kld_err" value="0.05"/>

<param name="kld_z" value="0.99"/>

<param name="odom_alpha1" value="0.2"/>

<param name="odom_alpha2" value="0.2"/>

<!-- translation std dev, m -->

<param name="odom_alpha3" value="0.2"/>

<param name="odom_alpha4" value="0.2"/>

<param name="laser_z_hit" value="0.5"/>

<param name="laser_z_short" value="0.05"/>

<param name="laser_z_max" value="0.05"/>

<param name="laser_z_rand" value="0.5"/>

<param name="laser_sigma_hit" value="0.2"/>

<param name="laser_lambda_short" value="0.1"/>

<param name="laser_model_type" value="likelihood_field"/>

<!-- <param name="laser_model_type" value="beam"/> -->

<param name="laser_likelihood_max_dist" value="2.0"/>

<param name="update_min_d" value="0.25"/>

<param name="update_min_a" value="0.2"/>

<param name="odom_frame_id" value="odom"/>

<param name="base_frame_id" value="base_footprint"/>

<param name="resample_interval" value="1"/>

<!-- Increase tolerance because the computer can get quite busy -->

<param name="transform_tolerance" value="1.0"/>

<param name="recovery_alpha_slow" value="0.0"/>

<param name="recovery_alpha_fast" value="0.0"/>

<param name="initial_pose_x" value="$(arg initial_pose_x)"/>

<param name="initial_pose_y" value="$(arg initial_pose_y)"/>

<param name="initial_pose_a" value="$(arg initial_pose_a)"/>

<remap from="scan" to="$(arg scan_topic)"/>

</node>

</launch>

最後にlaunchファイルを生成

$ cd ~/catkin_ws/src/diff_mobile_robot/launch

$ gedit amcl.launch

amcl.launch

<launch>

<!-- https://github.com/ros-planning/navigation/issues/206 -->

<!-- <rosparam command="delete" ns="move_base" /> -->

<!-- Map server -->

<arg name="map_file" default="$(find diff_mobile_robot)/map/testmap.yaml"/>

<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

<arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->

<arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->

<arg name="initial_pose_a" default="0.0"/>

<include file="$(find diff_mobile_robot)/launch/includes/amcl.launch.xml">

<arg name="initial_pose_x" value="0"/>

<arg name="initial_pose_y" value="0"/>

<arg name="initial_pose_a" value="0"/>

</include>

<include file="$(find diff_mobile_robot)/launch/includes/move_base.launch.xml"/>

</launch>

amcl.launchを実行してみる．

amclが入っていない可能性があるので

下記コマンドを先に実行してインストール確認

$ sudo apt-get install ros-kinetic-amcl

$ roslaunch diff_mobile_robot diff_mobile_gazebo.launch (１つ目の端末で実行：シミュレータ起動用）

$ roslaunch diff_mobile_robot amcl.launch （２つ目の端末で実行：ナビゲーション用）

$ rviz rviz （３つ目の端末で実行：移動場所指示用）

課題

１．Willow Garageの建物内の経路を２．１のナビゲーションで習ったWaypoitを記録していく方法で作成し，目標位置までナビゲーションをおこなプログラムを作成せよ．