A compilation of concepts I want to remember...

Navigation
 » Home
 » About Me
 » Github

Turtlepi #7: Automatic Target Generation for the Turtlebot

23 May 2017 » ros

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.

nav_stack

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.

  1. 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_map that is of type nav_msgs::GetMap and returns a response of type nav_msgs/OccupancyGrid. We can observe the raw message definition defined per below. In particular, we want to copy over the MapMetaData info and int8[] data for later use. Luckily int8[] is of type std::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
  1. When the service is initiated, the callback generateTargetService is 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

  1. http://robots.stanford.edu/papers/fox.aaai99.pdf