2D Navigation of a Mobile Robot in ROS
Navigation can be 2D or 3D. For a ground mobile robot, the navigation is typically 2D since the mobile robot does not have a degree of freedom in the vertical direction. Even for other types of mobile robots such as unmanned aerial vehicles (UAVs), the navigation can be simplified to 2D. This is performed by decoupling the motion control into 2D horizontal motion and 1D vertical motion (i.e. altitude control). See the following example of 2D navigation applied to a UAV: https://www.wilselby.com/research/ros-integration/2d-mapping-navigation/
There are five things required to navigate a mobile robot:
- Map
- Robot localization
- Sensing the environment
- Motion planning
- User Interface
- Motion control
The first four things above are handled by a navigation package, whereas the last thing (i.e. the motion control) should be handled by the robot controller.
Here we are going to create a 2D navigation package for a mobile robot. The package has a structure like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
my_navigation_package: |--launch |--amcl.launch.xml |--my_navigation_launch.launch |--maps |--my_map.pgm |--my_map.yaml |--param |--move_base_params.yaml |--dwa_planner_params.yaml |--costmap_common_params.yaml |--global_costmap_params.yaml |--local_costmap_params.yaml |--rviz |--my_navigation_rviz.rviz |--src |--my_navigation_client.cpp |--script |--my_navigation_client.py |--package.xml |--CMakeLists.txt |
All the launch files described below should be combined into a single launch file called “my_navigation_launch.launch” as shown in the file structure above. This is the launch file which should be launched to load the navigation configuration, after the robot (or the robot simulator) is launched. This launch file performs several tasks including:
- running the map_server and loading the map
- running AMCL node (used to localize the robot)
- running move_base node (used to navigate the robot from the current pose to the goal pose).
A similar navigation package can be found here: https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation
Map
There are two cases in the navigation of a mobile robot with regard to map:
- The map is available
- The map is not available
The availability of map represents an apriori knowledge about the environment. A map can be drawn manually if the environment is know in advance. If the map is not available, simultaneous localization and mapping (SLAM) is typically performed to create the map while the robot is exploring the environment. A simple way to perform SLAM is by teleoperating the robot and observing the obstacles by using the robot exteroceptive sensors while creating the map using a SLAM algorithm. Similar to navigation, a map can be either a 2D map or a 3D map.
In ROS, a map should be represented by two files: a pgm file and a YAML file. The pgm file is the visual representation of the environment, which can be presented as an occupancy grid map with gray scale ranging from 0 (black) to 255 (white), but typically to be classified into binary scale: either occupied (black) or free (white). The YAML file contains some information about the map. A YAML file called “my_map.yaml” corresponding to a map called “my_map.pgm” looks like this:
|
1 2 3 4 5 6 |
image: my_map.pgm resolution: 0.050000 origin: [-10.000000, -10.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 |
The resolution is given in meters per pixel. In the example above, it means that 1 pixel equals 0.05 meters. In other words, 100 pixel equals 5 meters, i.e. 1 pixel equals 5/100 meters = 5 cm. The origin described above means that the lower left of the map is at x = -10 meters, y = -10 meters. Negate means whether black and white colors are inverted. If negate is 0, it means black remains black and white remains white. The threshold to consider a grayscale occupied is 0.65 whereas the threshold to consider a grayscale free is 0.196. This threshold is in the range of 0 to 1.
To load a map, run the “map_server” node and load the map. It can be performed by using a launch file like this:
|
1 2 3 4 5 6 |
<launch> <!-- Map server --> <arg name="map_file" default="$(find my_navigation_package)/maps/my_map.yaml"/> <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"> </node> </launch> |
Robot localization
There are several methods to localize the robot, including:
- Kalman Filter (KF), including EKF and UKF.
- Particle Filter (PF).
A well-known PF-based localization algorithm is Monte Carlo Localization (MCL). The improved version of MCL is the Adaptive Monte Carlo Localization (AMCL). The AMCL node can be easily launched by including it in a launch file like this:
|
1 2 3 |
<launch> <include file="$(find my_navigation_package)/launch/amcl.launch.xml"/> </launch> |
where the “amcl.launch.xml” looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 |
<launch> <!-- if true, AMCL receives map topic instead of service call. --> <arg name="use_map_topic" default="false"/> <!-- topic name for the sensor values from the distance sensor. --> <arg name="scan_topic" default="scan"/> <!-- used as the initial x-coordinate value of the Gaussian distribution in initial pose estimation.--> <arg name="initial_pose_x" default="0.0"/> <!-- used as the initial y-coordinate value of the Gaussian distribution in the initial pose estimation.--> <arg name="initial_pose_y" default="0.0"/> <!-- used as the initial yaw coordinate value of the Gaussian distribution in the initial pose estimation. --> <arg name="initial_pose_a" default="0.0"/> <!-- execute the amcl node by referring to the parameter settings below. --> <node pkg="amcl" type="amcl" name="amcl"> <!-- filter related parameters --> <!-- min number of particles allowed --> <param name="min_particles" value="500"/> <!-- max number of particles allowed (the higher the better; set based on PC performance) --> <param name="max_particles" value="3000"/> <!-- max error between the actual distribution and the estimated distribution --> <param name="kld_err" value="0.02"/> <!-- translational motion required for filter update (meter) --> <param name="update_min_d" value="0.2"/> <!-- rotational motion required for filter update (radian) --> <param name="update_min_a" value="0.2"/> <!-- resampling interval --> <param name="resample_interval" value="1"/> <!-- conversion allowed time (by sec) --> <param name="transform_tolerance" value="0.5"/> <!-- index drop rate(slow average weight filter), deactivated if 0.0 --> <param name="recovery_alpha_slow" value="0.0"/> <!-- index drop rate(fast average weight filter), deactivated if 0.0 --> <param name="recovery_alpha_fast" value="0.0"/> <!-- refer to above initial_pose_x --> <param name="initial_pose_x" value="$(arg initial_pose_x)"/> <!-- refer to above initial_pose_y --> <param name="initial_pose_y" value="$(arg initial_pose_y)"/> <!-- refer to above initial_pose_a --> <param name="initial_pose_a" value="$(arg initial_pose_a)"/> <!-- max period to visually displaying scan and path info --> <!-- example: 10Hz = 0.1sec, deactivated if -1.0 --> <param name="gui_publish_rate" value="50.0"/> <!-- same as the explanation for use_map_topic --> <param name="use_map_topic" value="$(arg use_map_topic)"/> <!--distance sensor parameter --> <!-- change the sensor topic name --> <remap from="scan" to="$(arg scan_topic)"/> <!-- max distance of laser sensing distance (meter) --> <param name="laser_max_range" value="3.5"/> <!-- max number of laser beams used during filter update --> <param name="laser_max_beams" value="180"/> <!-- z_hit mixed weight of sensor model (mixture weight) --> <param name="laser_z_hit" value="0.5"/> <!-- z_short mixed weight of sensor model (mixture weight) --> <param name="laser_z_short" value="0.05"/> <!-- z_max mixed weight of sensor model (mixture weight) --> <param name="laser_z_max" value="0.05"/> <!-- x_rand mixed weight of sensor model (mixture weight) --> <param name="laser_z_rand" value="0.5"/> <!-- standard deviation of Gaussian model using z_hit of sensor --> <param name="laser_sigma_hit" value="0.2"/> <!-- index drop rate parameter for z_short of sensor --> <param name="laser_lambda_short" value="0.1"/> <!-- max distance and obstacle for likelihood_field method sensor --> <param name="laser_likelihood_max_dist" value="2.0"/> <!-- sensor type(select likelihood_field or beam) --> <param name="laser_model_type" value="likelihood_field"/> <!-- parameter related to odometry --> <!-- robot driving methods. "diff" or "omni" can be selected --> <param name="odom_model_type"value="diff"/> <!-- estimated rotational motion noise of the odometry during rotational motion --> <param name="odom_alpha1" value="0.1"/> <!-- estimated rotational motion noise of the odometry during translation motion --> <param name="odom_alpha2" value="0.1"/> <!-- estimated translation motion noise of the odometry during translation motion --> <param name="odom_alpha3" value="0.1"/> <!-- estimated translation motion noise of the odometry during rotational motion --> <param name="odom_alpha4" value="0.1"/> <!-- odometry frame --> <param name="odom_frame_id" value="odom"/> <!-- robot base frame --> <param name="base_frame_id" value="base_footprint"/> </node> </launch> |
Please notice that the AMCL node requires the determination of the initial pose of the robot.
Another ROS package commonly used for robot localization is robot_localization. This package is capable to fuse data from several sensors such as odometry, IMU, and GPS to improve the localization. The pose estimation is performed by using EKF and UKF.
The robot pose is published by the “robot_state_publisher” node. Hence, it is necessary to run the “robot_state_publisher” node if it is not running yet. This can be done by using a launch file like this:
|
1 2 3 4 5 |
<launch> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"> <param name="publish_frequency" type="double" value="50.0" /> </node> </launch> |
Motion planning
A very common ROS package used to navigate a mobile robot in ROS is move_base. It consists of a global planner and a local planner. The global planner is based on the global costmap, whereas the local planner is based on the local costmap. Both the costmaps are presented as occupancy grid map in grayscale, ranging from 0 to 255:
- 000: Free area where robot can move freely
- 001~127: Areas of low collision probability
- 128~252: Areas of high collision probability
- 253~254: Collision area
- 255: Occupied area where robot can not move
The global costmap is indicated by grayscale representation around the obstacles on the loaded map, whereas the local costmap is indicated by grayscale representation around the robot body.
The “move_base” node can be conveniently launched by using a launch file like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
<launch> <!-- move_base --> <arg name="cmd_vel_topic" default="/cmd_vel" /> <arg name="odom_topic" default="odom" /> <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 my_navigation_package)/param/move_base_params.yaml" command="load" /> <rosparam file="$(find my_navigation_package)/param/dwa_local_planner_params.yaml" command="load" /> <rosparam file="$(find my_navigation_package)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find my_navigation_package)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find my_navigation_package)/param/global_costmap_params.yaml" command="load" /> <rosparam file="$(find my_navigation_package)/param/local_costmap_params.yaml" command="load" /> <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> </node> </launch> |
As seen above, there are some YAML files for the “move_base” node which should be written:
- move_base_params.yaml
- dwa_local_planner_params.yaml
- costmap_common_params.yaml
- global_costmap_params.yaml
- local_costmap_params.yaml
The “move_base_params.yaml” looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
# choosing whether to stop the costmap node when move_base is inactive shutdown_costmaps: false # cycle of control iteration (in Hz) that orders the speed command to the robot base controller_frequency: 3.0 # maximum time (in seconds) that the controller will listen for control information before the space-clearing operation is performed controller_patience: 1.0 # repetition cycle of global plan (in Hz) planner_frequency: 2.0 # maximum amount of time (in seconds) to wait for an available plan before the space-clearing operation is performed planner_patience: 1.0 # time (in sec) allowed to allow the robot to move back and forth before executing the recovery behavior. oscillation_timeout: 10.0 # oscillation_timeout is initialized if you move the distance below the distance (in meter) that the robot should move so that it does not move back and forth. oscillation_distance: 0.2 # Obstacles farther away from fixed distance are deleted on the map during costmap initialization of the restore operation conservative_reset_dist: 0.1 |
The “dwa_local_planner_params.yaml” looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 |
DWAPlannerROS: # Robot Parameters max_vel_x: 0.18 # max velocity for x axis(meter/sec) min_vel_x:-0.18 # min velocity for x axis (meter/sec) max_vel_y: 0.0 # Not used. applies to omni directional robots only min_vel_y: 0.0 # Not used. applies to omni directional robots only max_trans_vel: 0.18 # max translational velocity(meter/sec) min_trans_vel: 0.05 # min translational velocity (meter/sec), negative value for reverse # trans_stopped_vel: 0.01 # translation stop velocity(meter/sec) max_rot_vel: 1.8 # max rotational velocity(radian/sec) min_rot_vel: 0.7 # min rotational velocity (radian/sec) # rot_stopped_vel: 0.01 # rotation stop velocity (radian/sec) acc_lim_x: 2.0 # limit for x axis acceleration(meter/sec^2) acc_lim_y: 0.0 # limit for y axis acceleration (meter/sec^2) acc_lim_theta: 2.0 # theta axis angular acceleration limit (radian/sec^2) # Target point error tolerance yaw_goal_tolerance: 0.15 # yaw axis target point error tolerance (radian) xy_goal_tolerance: 0.05 # x, y distance Target point error tolerance (meter) # Forward Simulation Parameter sim_time: 3.5 # forward simulation trajectory time vx_samples: 20 # number of sample in x axis velocity space vy_samples: 0 # number of sample in y axis velocity space vtheta_samples: 40 # number of sample in theta axis velocity space # Trajectory scoring parameter (trajectory evaluation) # Score calculation used for the trajectory evaluation cost function is as follows. # cost = # path_distance_bias * (distance to path from the endpoint of the trajectory in meters) # + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) # + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254)) path_distance_bias: 32.0 # weight value of the controller that follows the given path goal_distance_bias: 24.0 # weight value for the goal pose and control velocity occdist_scale: 0.04 # weight value for the obstacle avoidance forward_point_distance: 0.325 # distance between the robot and additional scoring point (meter) stop_time_buffer: 0.2 # time required for the robot to stop before collision (sec) scaling_speed: 0.25 # scaling Speed (meter/sec) max_scaling_factor: 0.2 # max scaling factor # Oscillation motion prevention paramter # distance the robot must move before the oscillation flag is reset oscillation_reset_dist: 0.05 # Debugging publish_traj_pc: true # debugging setting for the movement trajectory publish_cost_grid_pc: true # debugging setting for costmap global_frame_id: odom # ID setting for global frame |
The “costmap_common_params.yaml” looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
# Indicate the object as an obstacle when the distance between the robot and obstacle is within this range. obstacle_range: 2.5 # sensor value that exceeds this range will be indicated as a freespace raytrace_range: 3.5 # external dimension of the robot is provided as polygons in several points footprint: [[-0.110, -0.090], [-0.110, 0.090], [0.041, 0.090], [0.041, -0.090]] # radius of the inflation area to prevent collision with obstacles inflation_radius: 0.15 # scaling variable used in costmap calculation. Calculation formula is as follows. # exp(-1.0 * cost_scaling_factor *(distance_from_obstacle - inscribed_radius)) *(254 - 1) cost_scaling_factor: 0.5 # select costmap to use between voxel(voxel-grid) and costmap(costmap_2d) map_type: costmap # tolerance of relative coordinate conversion time between tf transform_tolerance: 0.2 # specify which sensor to use observation_sources: scan # set data type and topic, marking status, minimum obstacle for the laser scan scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true} |
The “global_costmap_params.yaml” looks like this:
|
1 2 3 4 5 6 7 |
global_costmap: global_frame: /map # set map frame robot_base_frame: /base_footprint # set robot's base frame update_frequency: 2.0 # update frequency publish_frequency: 0.1 # publish frequency static_map: true # setting whether or not to use given map transform_tolerance: 1.0 # transform tolerance time |
The “local_costmap_params.yaml” looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 |
local_costmap: global_frame: /odom # set map frame robot_base_frame: /base_footprint # set robot's base frame update_frequency: 2.0 # update frequency publish_frequency: 0.5 # publish frequency static_map: false # setting whether or not to use given map rolling_window: true # local map window setting width: 3.5 # area of local map window height: 3.5 # height of local map window resolution: 0.05 # resolution of local map window (meter/cell) transform_tolerance: 1.0 # transform tolerance time |
User Interface
A user can provide inputs for navigation in two ways:
- Using RViz where a user can initialize the pose of the robot for the AMCL node and determine a goal pose of the robot for the move_base node.
- Using client node. This can be written in C++ or Python. By using such a client node, one can programmatically determine a goal pose of the robot. Below we can see simple client nodes written in C++ and Python to move the robot 2 meters forward.
A client node written in C++ looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 |
#include <ros/ros.h> #include <move_base_msgs/MoveBaseAction.h> #include <actionlib/client/simple_action_client.h> typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; int main(int argc, char** argv){ ros::init(argc, argv, "simple_navigation_goals"); //tell the action client that we want to spin a thread by default MoveBaseClient ac("move_base", true); //wait for the action server to come up while(!ac.waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } move_base_msgs::MoveBaseGoal goal; //we'll send a goal to the robot to move 2 meters in the X+ direction of the map goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = 2.0; goal.target_pose.pose.orientation.w = 1.0; ROS_INFO("Sending goal"); ac.sendGoal(goal); ac.waitForResult(); if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Hooray, the base moved 2 meters forward"); else ROS_INFO("The base failed to move forward 2 meters for some reason"); return 0; } |
If the robot is intended to move 2 meters forward with respect to the robot’s frame, let’s say base_link frame, you just need to change the reference frame of the motion:
|
1 |
goal.target_pose.header.frame_id = "base_link"; |
A client node written in Python looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 |
#!/usr/bin/env python # license removed for brevity import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal def movebase_client(): # Create an action client called "move_base" with action definition file "MoveBaseAction" client = actionlib.SimpleActionClient('move_base',MoveBaseAction) # Waits until the action server has started up and started listening for goals. client.wait_for_server() # Creates a new goal with the MoveBaseGoal constructor goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() # Move 2 meters in the X+ direction of the map goal.target_pose.pose.position.x = 2.0 # No rotation of the mobile base frame w.r.t. map frame goal.target_pose.pose.orientation.w = 1.0 # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. wait = client.wait_for_result() # If the result doesn't arrive, assume the Server is not available if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: # Result of executing the action return client.get_result() # If the python node is executed as main process (sourced directly) if __name__ == '__main__': try: # Initializes a rospy node to let the SimpleActionClient publish and subscribe rospy.init_node('movebase_client_py') result = movebase_client() if result: rospy.loginfo("Goal execution done!") except rospy.ROSInterruptException: rospy.loginfo("Navigation test finished.") |
If the robot is intended to move 2 meters forward with respect to the robot’s frame, let’s say base_link frame, you just need to change the reference frame of the motion:
|
1 |
goal.target_pose.header.frame_id = "base_link" |
Motion control
Motion control of the mobile robot is handled by the robot controller, such as ros_control and friends. Since the “move_base” node publishes the velocity command (/cmd_vel topic) which is a geometry_msgs/Twists message (containing the linear and angular velocities), this velocity command is fed to the robot controller, such as the “diff_drive_controller” which is one of the controller in ros_controller.
Official documentation