ロボットカートのナビゲーション (ROS) | ROS、ROS2 (ロボットオペレーティングシステム）

概要

ロボットカート（ロボット台車）のナビゲーションを行います。

１）ロボットカートを使って部屋の2D地図を作成します。
２）地図上で目標位置・向きを指定して、そこまでロボットカートを自動運転させます。経路上に障害物があっても、それを避けてロボットカートが移動します。

３）パラメータ調整のためシミュレーションを使用します。

動画

ロボットカートをROSのナビゲーションで動かす

ロボットカートをROSのナビゲーションで動かす（その２）

ロボットカートを動かし、地図上にLiDARの測距データを表示する

ロボットカートのナビゲーションをROSでシミュレーションする

ロボットカートをジョイパッドで操縦する

地図作成

ロボットカートが移動する部屋の地図を作成します。

ロボットカートをジョイパッドで操縦し、部屋を一周します。

Launchファイルを起動します。

<launch>
  <!-- Load the HajimeCart URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find hajimecart_description)/urdf/hajimecart_robothw.urdf" />
  <!-- Convert /joint_states messages published by Gazebo to /tf messages,
       e.g., for rviz-->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
</launch>

roslaunch hajimecart_description hajimecart_robothw.launch

ros_serial関連のノードをまとめて起動します。

hajimecart_rosserial_all.launch

<launch>
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="230400"/>
  </node>
  <node name="cmdvel_cmdmotorspeed" pkg="cmdvel_cmdmotorspeed" type="cmdvel_cmdmotorspeed"/>
  <node name="odometry_publisher" pkg="odometry_publisher" type="odometry_publisher"/>
</launch>

roslaunch teleop_joy hajimecart_rosserial_all.launch

ジョイパッド関連のノードをまとめて起動します。

hajimecart_teleop.launch

<launch>
  <arg name="dev" default="/dev/input/js0"/>
  <node name="joy_node" pkg="joy" type="joy_node">
    <param name="dev" value="$(arg dev)"/>
  </node>
  <node name="teleop_joy" pkg="teleop_joy" type="teleop_joy"/>
</launch>

roslaunch teleop_joy hajimecart_teleop.launch dev:=/dev/input/js0

360度測距センサーのLiDARを起動します。

roslaunch rplidar_ros rplidar.launch

rosbagを起動して、これから測定するデータを記録していきます。

rosbag record -a -O sakamotobag

ロボットカートをジョイパッドで操縦して、ロボットカートに部屋を一周させます。

ロボットカートをジョイパッドで操縦する

記録を終了するには、rosbagをCTRL-Cで終了させます。

記録したbagファイルの情報を確認するには以下のコマンドです。

rosbag info sakamotobag.bag

path:        sakamotobag.bag
version:     2.0
duration:    1:46s (106s)
start:       Sep 13 2019 13:41:36.38 (1568349696.38)
end:         Sep 13 2019 13:43:22.47 (1568349802.47)
size:        7.3 MB
messages:    24918
compression: none [10/10 chunks]
types:       diagnostic_msgs/DiagnosticArray [60810da900de1dd6ddd437c3503511da]
             geometry_msgs/Twist             [9f195f881246fdfa2798d1d3eebca84a]
             nav_msgs/Odometry               [cd5e73d190d741a2f92e81eda573aca7]
             rosgraph_msgs/Log               [acffd30cd6b6de30f120938c17c593fb]
             sensor_msgs/JointState          [3066dcd76a6cfaef579bd0f34173e9fd]
             sensor_msgs/Joy                 [5a9ea5f83505693b71e785041e67a8bb]
             sensor_msgs/LaserScan           [90c7ef2dc6895d81024acba2ac42f369]
             std_msgs/Float32MultiArray      [6a40e0ffa6a17a503ac3f8616991b1f6]
             std_msgs/Int32MultiArray        [1d99f79f8b325b44fee908053e9c945b]
             tf2_msgs/TFMessage              [94810edda583a504dfda3829e70d7eec]
topics:      /cmd_vel         10601 msgs    : geometry_msgs/Twist            
             /cmdmotorspeed    2658 msgs    : std_msgs/Float32MultiArray     
             /diagnostics        92 msgs    : diagnostic_msgs/DiagnosticArray
             /encoder          2646 msgs    : std_msgs/Int32MultiArray       
             /joint_states     1066 msgs    : sensor_msgs/JointState         
             /joy               688 msgs    : sensor_msgs/Joy                
             /odom             2653 msgs    : nav_msgs/Odometry              
             /rosout             14 msgs    : rosgraph_msgs/Log               (4 connections)
             /rosout_agg          2 msgs    : rosgraph_msgs/Log              
             /scan              774 msgs    : sensor_msgs/LaserScan          
             /tf               3723 msgs    : tf2_msgs/TFMessage              (2 connections)
             /tf_static           1 msg     : tf2_msgs/TFMessage

ジョイパッドでロボットカート操縦中のROSのノードとトピック

測定データを記録したbagファイルを使用して地図作成にかかります。

地図作成のためのプログラムgmappingを起動します。

hajimecart_gmapping.launch

<launch>
    <param name="use_sim_time" value="true"/>
    <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_frame" value="map"/>
      <param name="map_update_interval" value="2.0"/>
      <param name="maxUrange" value="6.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="0.0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.2"/>
      <param name="temporalUpdate" value="1.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="100"/>
      <param name="xmin" value="-10.0"/>
      <param name="ymin" value="-10.0"/>
      <param name="xmax" value="10.0"/>
      <param name="ymax" value="10.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"/>
    </node>
</launch>

roslaunch hajimecart_navigation hajimecart_gmapping.launch

bagファイルを再生します。

rosbag play --clock sakamotobag.bag

地図を保存します。

rosrun map_server map_saver -f sakamotomap

gmappingで作成した地図

sakamotomap.pgm

作成した地図

作成した地図の情報ファイル

sakamotomap.yaml

image: sakamotomap.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

ナビゲーション（実機）

ナビゲーションのためにロボットカートを起動します。

roslaunch hajimecart_description hajimecart_robothw.launch
roslaunch teleop_joy hajimecart_rosserial_all.launch
roslaunch rplidar_ros rplidar.launch

ナビゲーション用のLaunchファイルで、map_server、amcl、move_baseなどをまとめて起動します。
hajimecart_robothw_navigation.launch

<launch>
  <!-- Load the HajimeCart URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find hajimecart_description)/urdf/hajimecart_robothw.urdf"/>
  <!-- Convert /joint_states messages published by Gazebo to /tf messages,
       e.g., for rviz-->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
        type="robot_state_publisher" output="screen"/>
  <node name="map_server" pkg="map_server" type="map_server"
        args="$(find hajimecart_navigation)/maps/sakamotomap.yaml" output="screen"/>
  <include file="$(find hajimecart_navigation)/launch/amcl.launch"/>
  <include file="$(find hajimecart_navigation)/launch/move_base.launch"/>
</launch>

amcl.launch

<launch>
  <!-- AMCL -->
  <node pkg="amcl" type="amcl" name="amcl">
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <param name="initial_pose_x"            value="0.0"/>
    <param name="initial_pose_y"            value="0.0"/>
    <param name="initial_pose_a"            value="0.0"/>
    <param name="gui_publish_rate"          value="50.0"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>
    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha1"               value="0.6"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    <param name="odom_frame_id"             value="odom"/>
    <param name="base_frame_id"             value="base_footprint"/>
  </node>
</launch>

move_base.launch

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find hajimecart_navigation)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find hajimecart_navigation)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find hajimecart_navigation)/params/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find hajimecart_navigation)/params/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find hajimecart_navigation)/params/move_base_params.yaml" command="load" />
    <rosparam file="$(find hajimecart_navigation)/params/dwa_local_planner_params.yaml" command="load" />
  </node>
</launch>

costmap_common_params.yaml

obstacle_range: 2.0
raytrace_range: 2.5
footprint: [[0.340, 0.160], [0.340, -0.16], [-0.100, -0.16], [-0.100, 0.16]]
inflation_radius: 0.5
cost_scaling_factor: 3.0
map_type: costmap
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: base_scan, observation_persistence: 0.0, max_obstacle_height: 0.3, min_obstacle_height: 0.0, data_type: LaserScan, topic: scan, marking: true, clearing: true}

local_costmap_params.yaml

local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint
  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 1.0
  static_map: false
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 1.0
  static_map: true

move_base_params.yaml

shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2

dwa_local_planner_params.yaml

DWAPlannerROS:
# Robot Configuration Parameters
  max_vel_x: 0.2
  min_vel_x: -0.2
  max_vel_y: 0.0
  min_vel_y: 0.0
# The velocity when robot is moving in a straight line
  max_trans_vel:  0.2
  min_trans_vel:  0.04
  max_rot_vel: 0.3
  min_rot_vel: 0.06
  acc_lim_x: 1.0
  acc_lim_y: 0.0
  acc_lim_theta: 1.0 
# Goal Tolerance Parametes
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.2
  latch_xy_goal_tolerance: false
# Forward Simulation Parameters
  sim_time: 4.0
  vx_samples: 20
  vy_samples: 0
  vth_samples: 40
  controller_frequency: 10.0
# Trajectory Scoring Parameters
  path_distance_bias: 32.0
  goal_distance_bias: 20.0
  occdist_scale: 0.02
  forward_point_distance: 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2
# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05
# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true

Launchファイルを起動します。

roslaunch hajimecart_navigation hajimecart_robothw_navigation.launch

ナビゲーション用のGUIのrvizを起動します。起動時に設定ファイルurdf_map.rvizを読み込みます。

display_map.launch

<launch>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find hajimecart_description)/launch/urdf_map.rviz" />
  <param name="use_gui" value="$(arg gui)"/>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" output="screen" />
</launch>

roslaunch hajimecart_description display_map.launch

rviz画面の地図上で、目標位置と向きをマウスで指定すると、ロボットカートは動き出します。

移動の経路上に障害物があればそれを避けて通り、目標に到達します。目標位置と向きを合わせてロボットカートは停止します。

LiDARで測定された測距データが地図上に表示されています。

本来の地図にはなかった、青色のサイコロもLiDARで検出されて、障害物として表示されています。（下の写真）

ロボットカートをナビゲーションで動かす

ナビゲーション中のrvizの画面

ナビゲーションでロボットカートが移動中（rvizの画面）

ナビゲーション中のROSのノードとトピック

ナビゲーション（シミュレーション）

ナビゲーションで設定するパラメータは多いため、シミュレーションを使用して、パラメータを調整します。

willowgarageの世界でシミュレータgazeboを起動します。
hajimecart_gazebo_willow.launch

<launch>
  <!-- Start Gazebo with an empty world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="worlds/willowgarage.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>
  <!-- Load the HajimeCart URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find hajimecart_description)/urdf/hajimecart.urdf" />
  <!-- Spawn a HajimeCart in Gazebo, taking the description from the
       parameter server -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
        args="-param robot_description -urdf -model hajimecart -x 8 -y -8" output="screen"/>
</launch>

roslaunch hajimecart_description hajimecart_gazebo_willow.launch

willowgarageの地図でmap_serverを起動します。
hajimecart_gazebo_willow_navigation.launch

<launch>
  <!-- Load the HajimeCart URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find hajimecart_description)/urdf/hajimecart.urdf"/>
  <!-- Convert /joint_states messages published by Gazebo to /tf messages,
       e.g., for rviz-->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
        type="robot_state_publisher" output="screen"/>
  <node name="map_server" pkg="map_server" type="map_server"
        args="$(find hajimecart_navigation)/maps/willow.yaml" output="screen"/>
  <include file="$(find hajimecart_navigation)/launch/amcl.launch"/>
  <include file="$(find hajimecart_navigation)/launch/move_base.launch"/>
</launch>

roslaunch hajimecart_navigation hajimecart_gazebo_willow_navigation.launch

ナビゲーション用のGUIのrvizを起動します。
display_map.launch

<launch>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find hajimecart_description)/launch/urdf_map.rviz" />
  <param name="use_gui" value="$(arg gui)"/>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" output="screen" />
</launch>

roslaunch hajimecart_description display_map.launch

左側ウインドウのrviz画面の地図上で、目標位置と向きをマウス操作で与えると、右側ウインドウのgazebo画面でロボットカートは動き出します。
移動の経路上に障害物があればそれを避けて通り、目標に到達します。目標位置と向きを合わせてロボットカートは停止します。

ロボットカートのナビゲーションのシミュレーション

ROSのパラメータを調整するために、rqt_reconfigureを起動します。

rosrun rqt_reconfigure rqt_reconfigure

rqt_reconfigureを使うと、パラメータ変更がリアルタイムに反映されます。
ロボットカートのシミュレーションの動きを確認しながら、ナビゲーションのパラメータを調整していきます。

rqt_reconfigureの画面

ナビゲーションのシミュレーション中のROSのノードとトピック