a monolithic mobile robot navigation system as a replacement for the navigation stack
- ROS 1 version: https://github.com/ryuichiueda/value_iteration
$ sudo apt install ros-humble-grid-map*
$ sudo apt install ... (I will write later about required packages. )
$ cd <workspace>
$ git clone https://github.com/ryuichiueda/emcl2_ros2.git #Please use this version due to a problem of launch file.
$ git clone https://github.com/ryuichiueda/value_iteration2.git
$ cd <workspace>
$ colcon build
$ source install/setup.bash
$ source install/local_setup.bash
$ ros2 launch value_iteration2 turtle.launch.py
This node executes value iteration.
Give the goal to topic /goal_pose
. The planner immediately starts working and giving the velocity of the robot through /cmd_vel
. The action server is not implemented in this stage.
- static_map (nav_msgs/srv/GetMap)
- Initiate the map for value iteration.
- goal_pose (geometry_msgs/msg/PoseStamped)
- goal position and orientation
- scan (sensor_msgs/msg/LaserScan)
- laser scans
- tf (tf2_msgs/msg/tfMessage)
- transforms
- /cmd_vel (geometry_msgs/msg/Twist)
- the control order to the robot; published only when the parameter
online
is true
- the control order to the robot; published only when the parameter
- action_list (still not imported from ROS 1)
- Currently,
vi_node
requires an action list like the following.onestep_forward_m
andonestep_rotation_deg
mean the forward and rotational velocities respectively.vi_node
calculates the optimal value function and the oplimal policy based on this action list.
- Currently,
<rosparam> <!-- in the case of ROS 1 -->
vi_node:
action_list:
- name: forward
onestep_forward_m: 0.3
onestep_rotation_deg: 0.0
- name: back
onestep_forward_m: -0.2
onestep_rotation_deg: 0.0
- name: right
onestep_forward_m: 0.0
onestep_rotation_deg: -20.0
- name: left
onestep_forward_m: 0.0
onestep_rotation_deg: 20.0
</rosparam>
- ~online (bool, defalut: true)
- flag for using vi_node as a real-time planner
- ~theta_cell_num (int, default: 60)
- number of intervals of the discrete state space on theta-axis
- ~global_thread_num (int, default: 2)
- number of threads used on value iteration
- ~goal_margin_radius (double, default: 0.2[m])
- radius of the goal on xy-plane
- ~goal_margin_theta (int, default: 10[deg])
- radius of the goal on theta-axis
- ~map_type (string, "cost" or "occupancy", default: occupancy, "cost" is not imported to this version)
- choice of map for setting immediate costs and occupancy (please read the Maps section)
We can choose two types of maps for initializing parametes of states.
A cost map is created from an occupancy grid map. Occupied cells are marked as roped-off area. Moreover, cells near occupied ones are given immediate penalty. The near cells and the value of the penalty are controlled through the following parametes.
- parapeters
- ~safety_radius (double, default: 0.2[m])
- distance that should be kept between the center of the robot and an occupancy grid
- ~safety_radius_penalty (double, default: 30[s], max: 1,000,000,000[s])
- immediate penality (negative immediate reward in the field of reinforcement learning) when the robot invades the safety radius.
- ~safety_radius (double, default: 0.2[m])
We can also use a "cost map," which contains the immediate cost of every cell. This map should be written with "Raw mode". In a map, cells given 255 are regarded as roped-off cells. Cells with other values are free cells but are given the values as their immediate costs.
There are an information file (cost.yaml
) and a map file (cost.pgm
) in the maps
directory. The information file must contains the line mode: raw
as shown below.
image: ./cost.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65 #not used
free_thresh: 0.196 #not used
mode: raw
vi_node
reads a cost map from a service cost_map
. In launch/vi_turtle_online.launch
, you can find the following lines so as to provice the cost map through cost_map
.
<node pkg="map_server" name="cost_map" type="map_server" args="$(find value_iteration)/maps/cost.yaml">
<remap from="static_map" to="cost_map" />
<remap from="map" to="cost_map_for_vi" /> <!-- This line avoids this node to provide the map to RViz. -->
</node>
It is the sum of costs from a pose (position + orientation) to a point of goal. The cost means the time. Therefore, it means the time required to reach the goal if immediate penarties don't exist. As shown in the parameter list, we can define some kinds of immediate penalties, which are quantified in units of seconds.
- R. Ueda, L. Tonouchi, T. Ikebe, and Y. Hayashibara, “Implementation of Brute-Force Value Iteration for Mobile Robot Path Planning and Obstacle Bypassing,” J. Robot. Mechatron., Vol.35 No.6, pp. 1489-1502, 2023.
- This software is developped on the support of JSPS KAKENHI JP20K04382.
- A custom simulation environment in this repository is derived from ROBOTIS-GIT/turtlebot3_simulations, which is licensed with Apache License 2.0.