Source: https://github.com/surfertas/turtlepi
This is a quick note to update the work done thus far while here at Idein
Inc. One of the short term objective was to create an
environment that would allow for the collection of data of a robot navigating in
a simulation environment based on a given policy. The expert policy was provided
by the ROS navigation stack, specifically the move_base found in the ROS
node. Since the robot will have access to a static map, we will be using the
AMCL, adaptive monte carlo localization algorithm as opposed to SLAM. [1] See
the figure below for the navigation stack setup. The move_base given the below setup
publishes control commands to the cmd_vel topic as messages of type
geometry_msgs/Twist allowing the robot to follow a path computed by
global_planner and local_planner to a goal specified independently.
A typical way of generating a target for navigation is by using RViz to specify a goal explicitly using the Navigation 2d feature. This is fine in the case where only a few sample runs are needed, but in our case we would like to automate the target generation process in order to efficiently generate a large amount of data in simulation.
Automated Target Generation System
The program logic associated with the automated target generation service
generate_nav_target is quite simple, though the integration requires work.
- We start by obtaining the meta data related to the map. In this case we are
using a static map, and assume that the map environment does not change during
an episode, where an episode is defined as a run from current location to the
target. The ROS framework already provides a service
static_mapthat is of typenav_msgs::GetMapand returns a response of typenav_msgs/OccupancyGrid. We can observe the raw message definition defined per below. In particular, we want to copy over the MapMetaData info andint8[]data for later use. Luckilyint8[]is of typestd::vector<int8_t>in c++ thus the=is overloaded and copying the vector is easy. NOTE: the data is in row-major form so we need to be sensitive when using the data. The map gets initialized once when the target generator service is launched.
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starts with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
int8[] data
- When the service is initiated, the callback
generateTargetServiceis called, and generates a new target. The service uses the cost map that was saved at initialization and randomly selects a point on the cost map. The grid point is converted to the world coordinates via a conversion as per below:
void TargetGenerator::mapToWorld(uint32_t mx, uint32_t my, double& wx, double&
wy)
{
wx = map_origin_x_ + (mx + 0.5) * map_resolution_;
wy = map_origin_y_ + (my + 0.5) * map_resolution_;
}
With the world coordinates in hand, a distance check is made against a threshold which I have set to 8m. We want to avoid targets that are too close to the current location. If and only if the threshold check passes and the selected point on grid is in the free space, which is associated with a cost map value of 0, then the generated point is selected as a valid target.
A couple of additional notes:
- I have used visualization markers to make target visualization in RViz easier.
You need to add a marker, and specify the appropriate topic to subscribe to.
In this case the topic is
/turtlepi_navigate/visualization_marker. - The implementation has much room for optimization. This implementation is a first pass. When considering a large number of calls to this service. It would be inefficient to randomly select a point from the entire cost map, when the valid points are only contained to the free space. I look to implement an algorithm that would return the free space points only, and the service can randomly select a point from the set of free points.
- Once the target goal is obtained the action server is used to communicate the goal and follow on communication with the robot until the target is reached or a failure event occurs.
Results
See the results of an example run below.
Usage
git clone https://github.com/surfertas/turtlepi.git
// On first terminal
cd ~/turtlepi_gazebo/launch/
roslaunch turtlepi.launch
// Wait until you see that odom received message.
// On second terminal
cd ~/turtlepi_navigate/launch/
roslaunch turtlepi_gentarget.launch
References
- http://robots.stanford.edu/papers/fox.aaai99.pdf