The mapping node is responsible for updating the world map with sensor information. Initially it receives data from the robot base node (bumper hits) and the laser scanner. It can also load data from a bitmap at startup.

mapper_cu is responsible for accumulating sensor data in a map. It provides a service that allows other nodes to request and receive a copy of the current map. It also publishes map updates, thus allowing other nodes to maintain a current version of the map with limited overhead.


API Documentation

mapper_cu

mapper_cu subscribes to sensor data from the hokuyo_listener_cu and irobot_create_cu nodes, and accumulates it in a map. It provides a map service and publishes map updates. The former sends the entire map to a requesting node, while the latter publishes recent updates to a topic. The map service and map update messages are sent in the map coordinate system scaled by the map resolution. Therefore, they are technically NOT in the /map_cu frame. This is done to facilitate speedy map updates between mapper_cu and base_planner_cu (and also visualization_cu), who use similar internal map representations.

Two probabilistic occupancy grid maps are used (one each for laser and bumper data) and then merged before being sent to the rest of the system. The maximum probability of obstacle from either map is used during the merge. The laser map uses a predefined number of reading about a particular grid to calculated its obstacle probability. The bumper map associates a predefined obstacle probability with a bumper hit, and also resets a grid's obstacle probability to 0 whenever the robot occupies it.

Subscribed Topics

/cu/laser_scan_cu (hokuyo_listener_cu/PointCloudWithOrigin)
  • This is a list of laser scan hits, along with the pose of the laser scanner when it captured them. Coordinates are in the map coordinate system (/map_cu).
/cu/pose_cu (geometry_msgs/PoseStamped)
  • The robot's best estimate of where it is, given all pose data, in map coordinates (/map_cu).
/cu/bumper_pose_cu (geometry_msgs/PoseStamped)
  • The position of a bumper hit in map coordinates (/map_cu).

Published Topics

/cu/map_changes_cu (sensor_msgs/PointCloud)
  • Where each point in the cloud is a triple (x,y,z). x and y are used to denote which map grid is changed (in map coordinates scaled to resolution) and z is the new cost value of that grid. Note: map updates must be positive and non-zero.

Services

/cu/get_map_cu (nav_msgs/GetMap)
  • Provides a map in map coordinates scaled by resolution.

Services Called

/cu/get_pose_cu (localization_cu/GetPose)
  • Waits for this service at boot so that mapper_cu knows localization_cu is up and running before using data provided by it.

Parameters

~map_x_size (double, default: 20 (m))
  • Map goes from 0 to this distance (meters) in the x direction.
~map_y_size (double, default: 20 (m))
  • Map goes from 0 to this distance (meters) in the y direction.
~map_resolution (double, default: 0.08 (m))
  • Each map grid spans this much real-world distance (meters).
~obstacle_prior (double, default: 0.2)
  • The prior_probability of an obstacle in the laser map.
~map_attention_span (int, default: 100)
  • The last map_attention_span readings of a particular grid are used to calculate probability of obstacle there.
~scanner_range (double, default: 5.5 (m))
  • The range of the laser scanner in meters (used to identify safe grids within range and also prune bad data).
~bumper_obstacle_posterior (double, default: 1.0)
  • The probability of obstacle associated with a bumper hit.
~robot_radius (double, default: 0.2 (m))
  • Grids within this distance (meters) of the robot origin are labeled as safe.
~global_map_x_offset (double, default: 0 (m))
  • The map (/map_cu) coordinate system is this far off of the world coordinate system (/world_cu) in the x direction. Units are in meters.
~global_map_y_offset (double, default: 0 (m))
  • The map (/map_cu) coordinate system is this far off of the world coordinate system (/world_cu) in the y direction. Units are in meters.
~global_map_theta_offset (double, default: 0 (rad))
  • the map (/map_cu) coordinate system is this far off of the world coordinate system (/world_cu) rotationally. Units are in radians.
~map_file (string, default: ../blank.bmp)
  • A bitmap in this file is loaded as the initial bumper map.
prairiedog/using_tf (bool, default: true)
  • When true, the tf ROS package is used to facilitate coordinate transformations.

Provided tf Transforms

/world_cu/map_cu
  • Transform between world and map coordinates.

Wiki: mapper_cu (last edited 2010-02-23 19:21:10 by MeloneeWise)