概要
ロボットカート（ロボット台車）のナビゲーションを行います。
１）ロボットカートを使って部屋の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
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
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
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
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のノードとトピック