/**
* \brief Recusrive function performing DFS
*/
bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal,
uint32_t depth, std::vector<bool> &visited, bool in_obstacle_space) {
//std::cout << start.x << " " << start.y << std::endl;
// Termination crit
if (start == goal) {
return true;
}
if (depth == 0) {
return false;
}
uint32_t start_idx = MAP_IDX(map_.info.width, start.x, start.y);
visited[start_idx] = true;
std::vector<Point2d> neighbours;
getOrderedNeighbours(start, goal, visited, neighbours, in_obstacle_space);
for (size_t i = 0; i < neighbours.size(); ++i) {
Point2d& n = neighbours[i];
// Check if it has been visited again - quite likely that one of the
// previous loop iterations have covered this already
uint32_t n_idx = MAP_IDX(map_.info.width, n.x, n.y);
if (visited[n_idx]) {
continue;
}
bool success = searchForPath(n, goal, depth - 1, visited);
if (success)
return true;
}
return false; // disconnected components
}
bool locationsInDirectLineOfSight(const Point2f& pt1, const Point2f& pt2,
const nav_msgs::OccupancyGrid map) {
int x0 = lrint(pt1.x), y0 = lrint(pt1.y);
int x1 = lrint(pt2.x), y1 = lrint(pt2.y);
int dx = abs(x1-x0), dy = abs(y1-y0);
int sx = (x0 < x1) ? 1 : -1;
int sy = (y0 < y1) ? 1 : -1;
float err = dx - dy;
bool is_occupied = false;
while (!is_occupied) {
is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
if (x0 == x1 && y0 == y1) break;
float e2 = 2 * err;
if (e2 > -dy) {
err -= dy;
x0 += sx;
}
if (x0 == x1 && y0 == y1) {
is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
break;
}
if (e2 < dx) {
err += dx;
y0 += sy;
}
}
return !is_occupied;
}
void CostMapCalculation::sysMessageCallback(const std_msgs::String& string)
{
ROS_DEBUG("HectorCM: sysMsgCallback, msg contents: %s", string.data.c_str());
if (string.data == "reset")
{
ROS_INFO("HectorCM: reset");
// set default values
cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
// loop through starting area
min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
for (int v = min_index(1); v < max_index(1); ++v)
for (int u = min_index(0); u < max_index(0); ++u)
cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
received_elevation_map = false;
received_grid_map = false;
received_point_cloud = false;
}
}
/**
* \brief draws colored regions corresponding to each critical region
* onto a given image starting at (orig_x, orig_y)
*/
void TopologicalMapper::drawConnectedComponents(cv::Mat &image,
uint32_t orig_x, uint32_t orig_y) {
// Figure out different colors - 1st color should always be black
size_t num_colors = num_components_;
component_colors_.resize(num_colors);
for (size_t i = 0; i < num_colors; ++i) {
component_colors_[i] =
cv::Vec3b(160 + rand() % 64, 160 + rand() % 64, 160 + rand() % 64);
}
// component 0 is obstacles + background. don't draw?
// Now paint!
for (uint32_t j = 1; j < map_resp_.map.info.height; ++j) {
cv::Vec3b* image_row_j = image.ptr<cv::Vec3b>(j + orig_y);
for (uint32_t i = 0; i < map_resp_.map.info.width; ++i) {
size_t map_idx = MAP_IDX(map_resp_.map.info.width, i, j);
if (component_map_[map_idx] == -1)
continue;
cv::Vec3b& pixel = image_row_j[i + orig_x];
pixel[0] = component_colors_[component_map_[map_idx]][0];
pixel[1] = component_colors_[component_map_[map_idx]][1];
pixel[2] = component_colors_[component_map_[map_idx]][2];
}
}
}
/**
* \brief Gets neighbours for a given node iff they are also obstacles
* and have not been visited before
*/
void DirectedDFS::getOrderedNeighbours(const Point2d &from,
const Point2d &goal, const std::vector<bool> &visited,
std::vector<Point2d> &neighbours, bool in_obstacle_space) {
size_t neighbour_count = 8;
uint32_t x_offset[] = {-1, 0, 1, -1, 1, -1, 0, 1};
uint32_t y_offset[] = {-1, -1, -1, 0, 0, 1, 1, 1};
neighbours.clear();
for (size_t i = 0; i < neighbour_count; ++i) {
// Check if neighbours are still on map
Point2d p = from + Point2d(x_offset[i],y_offset[i]);
// covers negative case as well (unsigned)
if (p.x >= (int) map_.info.width || p.y >= (int) map_.info.height ||
p.x <= 0 || p.y <= 0) {
continue;
}
uint32_t map_idx = MAP_IDX(map_.info.width, p.x, p.y);
if (visited[map_idx] || (in_obstacle_space && map_.data[map_idx] == 0) ||
(!in_obstacle_space && map_.data[map_idx] != 0)) {
continue;
}
p.distance_from_ref = norm(p - goal);
neighbours.push_back(p);
}
std::sort(neighbours.begin(), neighbours.end(), Point2dDistanceComp());
}
void CostMapCalculation::updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data)
{
ROS_DEBUG("HectorCM: received new map meta data -> overwrite old parameters");
// check if new parameters differ and abort otherwise
if (cost_map.info.width == map_meta_data.width &&
cost_map.info.height == map_meta_data.height &&
cost_map.info.resolution == map_meta_data.resolution &&
cost_map.info.origin.position.x == map_meta_data.origin.position.x &&
cost_map.info.origin.position.y == map_meta_data.origin.position.y &&
cost_map.info.origin.position.z == map_meta_data.origin.position.z &&
cost_map.info.origin.orientation.x == map_meta_data.origin.orientation.x &&
cost_map.info.origin.orientation.y == map_meta_data.origin.orientation.y &&
cost_map.info.origin.orientation.z == map_meta_data.origin.orientation.z &&
cost_map.info.origin.orientation.w == map_meta_data.origin.orientation.w) {
return;
}
// set new parameters
cost_map.info.width = map_meta_data.width;
cost_map.info.height = map_meta_data.height;
cost_map.info.resolution = map_meta_data.resolution;
cost_map.info.origin.position.x = map_meta_data.origin.position.x;
cost_map.info.origin.position.y = map_meta_data.origin.position.y;
cost_map.info.origin.position.z = map_meta_data.origin.position.z;
cost_map.info.origin.orientation.x = map_meta_data.origin.orientation.x;
cost_map.info.origin.orientation.y = map_meta_data.origin.orientation.y;
cost_map.info.origin.orientation.z = map_meta_data.origin.orientation.z;
cost_map.info.origin.orientation.w = map_meta_data.origin.orientation.w;
world_map_transform.setTransforms(cost_map.info);
// allocate memory
cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
// loop through starting area
min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
for (int v = min_index(1); v < max_index(1); ++v)
for (int u = min_index(0); u < max_index(0); ++u)
cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
// update flags
received_elevation_map = false;
received_grid_map = false;
use_cloud_map = false;
}
void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>);
ROS_DEBUG("HectorCM: received new point cloud");
// converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
pcl::fromROSMsg(*cloud_msg, *cloud_sensor);
// transform cloud to /map frame
try
{
listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_msg->header.stamp,ros::Duration(1.0));
pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorCM: pointcloud transform failed");
return;
}
// compute region of intereset
if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world))
return;
Eigen::Vector2f world_coords;
// define a cube with two points in space:
Eigen::Vector4f minPoint;
world_coords = world_map_transform.getC1Coords(min_index.cast<float>());
minPoint[0]=world_coords(0); // define minimum point x
minPoint[1]=world_coords(1); // define minimum point y
minPoint[2]=slize_min_height; // define minimum point z
Eigen::Vector4f maxPoint;
world_coords = world_map_transform.getC1Coords(max_index.cast<float>());
maxPoint[0]=world_coords(0); // define max point x
maxPoint[1]=world_coords(1); // define max point y
maxPoint[2]=slize_max_height; // define max point z
pcl::CropBox<pcl::PointXYZ> cropFilter;
cropFilter.setInputCloud (cloud_base_link);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*sliced_cloud);
ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size());
pub_cloud_slice.publish(sliced_cloud);
cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
// iterate trough all points
for (unsigned int k = 0; k < sliced_cloud->size(); ++k)
{
const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k];
// allign grid points
Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL;
}
// set point cloud received flag
received_point_cloud = true;
// calculate cost map
if(received_grid_map)
{
if(received_elevation_map)
{
calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
}
else
{
calculateCostMap(USE_GRID_AND_CLOUD_MAP);
}
}
}
void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg)
{
ROS_DEBUG("HectorCM: received new elevation map");
// check header
if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) && // Magic number 1000 -> min grid size should not be less than 1mm
elevation_map_msg->info.height != cost_map.info.height &&
elevation_map_msg->info.width != cost_map.info.width)
{
ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!");
return;
}
// store elevation_map_msg in local variable
elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width);
// store elevation map zero level
elevation_zero_level = elevation_map_msg->info.zero_elevation;
// compute region of intereset
if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world))
return;
// loop through each element
int filtered_cell, filtered_cell_x, filtered_cell_y;
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
// compute cost_map_index
unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v);
// check if neighbouring cells are known
if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) ||
elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) ||
elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) ||
elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level))
continue;
// edge filter
filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1));
filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u));
if(filtered_cell_x > filtered_cell_y)
filtered_cell = filtered_cell_x;
else
filtered_cell = filtered_cell_y;
// check if cell is traversable
if(filtered_cell > max_delta_elevation/grid_res_z)
{
// cell is not traversable -> mark it as occupied
elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL;
}
else
{
// cell is traversable -> mark it as free
elevation_cost_map.data[cost_map_index] = FREE_CELL;
}
}
}
// set elevation map received flag
received_elevation_map = true;
// calculate cost map
if(received_grid_map)
{
if(received_point_cloud)
{
calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
}
else
{
calculateCostMap(USE_GRID_AND_ELEVATION_MAP);
}
}
else
{
calculateCostMap(USE_ELEVATION_MAP_ONLY);
}
}
CostMapCalculation::CostMapCalculation() : nHandle(), pnHandle("~")
{
int width, height;
double grid_res_xy;
pnHandle.param("elevation_resolution", grid_res_z, 0.01); //[m]
pnHandle.param("max_grid_size_x", width, 1024); //[cell]
cost_map.info.width = width;
pnHandle.param("max_grid_size_y", height, 1024); //[cell]
cost_map.info.height = height;
pnHandle.param("map_resolution", grid_res_xy, 0.05); //[m]
cost_map.info.resolution = grid_res_xy;
pnHandle.param("origin_x",cost_map.info.origin.position.x, -(double)cost_map.info.width*(double)cost_map.info.resolution/2.0); //[m]
pnHandle.param("origin_y",cost_map.info.origin.position.y, -(double)cost_map.info.height*(double)cost_map.info.resolution/2.0); //[m]
pnHandle.param("origin_z",cost_map.info.origin.position.z, 0.0); //[m]
pnHandle.param("orientation_x",cost_map.info.origin.orientation.x, 0.0); //[]
pnHandle.param("orientation_y",cost_map.info.origin.orientation.y, 0.0); //[]
pnHandle.param("orientation_z",cost_map.info.origin.orientation.z, 0.0); //[]
pnHandle.param("orientation_w",cost_map.info.origin.orientation.w, 1.0); //[]
pnHandle.param("initial_free_cells_radius", initial_free_cells_radius, 0.30); //[m]
pnHandle.param("update_radius", update_radius_world, 4.0); //[m]
pnHandle.param("max_delta_elevation", max_delta_elevation, 0.08); //[m]
pnHandle.param("map_frame_id", map_frame_id,std::string("map"));
pnHandle.param("local_transform_frame_id", local_transform_frame_id,std::string("base_footprint"));
pnHandle.param("cost_map_topic", cost_map_topic, std::string("cost_map"));
pnHandle.param("elevation_map_topic", elevation_map_topic, std::string("elevation_map_local"));
pnHandle.param("grid_map_topic", grid_map_topic, std::string("scanmatcher_map"));
pnHandle.param("use_elevation_map", use_elevation_map, true);
pnHandle.param("use_grid_map", use_grid_map, true);
pnHandle.param("use_cloud_map", use_cloud_map, false);
pnHandle.param("allow_elevation_map_to_clear_occupied_cells", allow_elevation_map_to_clear_occupied_cells, true);
pnHandle.param("max_clear_size", max_clear_size, 2);
pnHandle.param("point_cloud_topic", point_cloud_topic,std::string("openni/depth/points"));
pnHandle.param("slize_min_height", slize_min_height, 0.30); //[m]
pnHandle.param("slize_max_height", slize_min_height, 0.35); //[m]
pnHandle.param("costmap_pub_freq", costmap_pub_freq, 4.0); //[Hz]
pnHandle.param("sys_msg_topic", sys_msg_topic, std::string("syscommand"));
cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
world_map_transform.setTransforms(cost_map.info);
// loop through starting area
min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
for (int v = min_index(1); v < max_index(1); ++v)
for (int u = min_index(0); u < max_index(0); ++u)
cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
pub_cost_map = nHandle.advertise<nav_msgs::OccupancyGrid>(cost_map_topic, 1, true);
received_elevation_map = false;
received_grid_map = false;
received_point_cloud = false;
sliced_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
if(use_elevation_map)
sub_elevation_map = nHandle.subscribe(elevation_map_topic,10,&CostMapCalculation::callbackElevationMap,this);
if(use_grid_map)
sub_grid_map = nHandle.subscribe(grid_map_topic,10,&CostMapCalculation::callbackGridMap,this);
if(use_cloud_map)
sub_point_cloud = nHandle.subscribe(point_cloud_topic,10,&CostMapCalculation::callbackPointCloud,this);
sub_map_info = nHandle.subscribe("map_metadata",1,&CostMapCalculation::updateMapParamsCallback,this);
sub_sysMessage = nHandle.subscribe(sys_msg_topic, 10, &CostMapCalculation::sysMessageCallback, this);
dyn_rec_server_.setCallback(boost::bind(&CostMapCalculation::dynRecParamCallback, this, _1, _2));
timer = pnHandle.createTimer(ros::Duration(1.0/costmap_pub_freq), &CostMapCalculation::timerCallback,this);
ROS_INFO("HectorCM: is up and running.");
pub_cloud_slice = pnHandle.advertise<sensor_msgs::PointCloud2>("cloud_slice",1,true);
ros::spin();
}
bool
SlamKarto::updateMap()
{
boost::mutex::scoped_lock(map_mutex_);
karto::OccupancyGrid* occ_grid =
karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
if(!occ_grid)
return false;
if(!got_map_) {
map_.map.info.resolution = resolution_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
// Translate to ROS format
kt_int32s width = occ_grid->GetWidth();
kt_int32s height = occ_grid->GetHeight();
karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();
if(map_.map.info.width != (unsigned int) width ||
map_.map.info.height != (unsigned int) height ||
map_.map.info.origin.position.x != offset.GetX() ||
map_.map.info.origin.position.y != offset.GetY())
{
map_.map.info.origin.position.x = offset.GetX();
map_.map.info.origin.position.y = offset.GetY();
map_.map.info.width = width;
map_.map.info.height = height;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}
for (kt_int32s y=0; y<height; y++)
{
for (kt_int32s x=0; x<width; x++)
{
// Getting the value at position x,y
kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
switch (value)
{
case karto::GridStates_Unknown:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
break;
case karto::GridStates_Occupied:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
break;
case karto::GridStates_Free:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
break;
default:
ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
break;
}
}
}
// Set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = map_frame_;
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
delete occ_grid;
return true;
}
void
loadMapFromFile(dynamic_mapping::GetDynamicMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th, double* origin, uint32_t *dynamic_imp)
{
SDL_Surface* img;
unsigned char* pixels;
unsigned char* p;
int rowstride, n_channels;
unsigned int i,j;
int k;
double occ;
int color_sum;
double color_avg;
int fname_size = strlen(fname);
// Load the image using SDL. If we get NULL back, the image load failed.
if(!(img = IMG_Load(fname)))
{
std::string errmsg = std::string("failed to open image file \"") +
std::string(fname) + std::string("\"");
throw std::runtime_error(errmsg);
}
// Copy the image data into the map structure
resp->map.info.width = img->w;
resp->map.info.height = img->h;
resp->map.info.resolution = res;
resp->map.info.origin.position.x = *(origin);
resp->map.info.origin.position.y = *(origin+1);
resp->map.info.origin.position.z = 0.0;
tf::Quaternion q;
q.setRPY(0,0, *(origin+2));
resp->map.info.origin.orientation.x = q.x();
resp->map.info.origin.orientation.y = q.y();
resp->map.info.origin.orientation.z = q.z();
resp->map.info.origin.orientation.w = q.w();
// Allocate space to hold the data
resp->map.data.resize(resp->map.info.width * resp->map.info.height);
resp->map.dynamic.resize(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
n_channels = img->format->BytesPerPixel;
ROS_INFO("Bytes per Pixel: %d\n", n_channels);
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
int dyn_area = 1;
for(j = 0; j < resp->map.info.height; j++)
{
for (i = 0; i < resp->map.info.width; i++)
{
static unsigned char color[3];
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
color_sum = 0;
// ppm file rgb
if(n_channels == 3) {
color[0] = (int) *p; //red
color[1] = (int) *(p+1); //green
color[2] = (int) *(p+2); //blue
// blue pixel
if(color[1] < color[2]) {
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
}
// red pixel
else if(color[0] > color[1]) {
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 100 - ((color[1]) * 5 * 100) / 255;
resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = dynamic_imp[resp->map.info.height*j + i];
}
// grey valued pixel
else {
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 100 - ((color[1]) * 100) / 255;
resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = dynamic_imp[resp->map.info.height*j + i];
}
}
//pgm grey valued file
else {
color[0] = (int) *p; //grey
if(color[0] == 254)
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
else if(color[0] == 0)
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
else
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = (color[0] - 100);
resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
}
}
}
SDL_FreeSurface(img);
}
geometry_msgs::Pose BaseRobotPositioner::positionRobot(
const bwi_mapper::Point2f& from,
const bwi_mapper::Point2f& at,
const bwi_mapper::Point2f& to) {
geometry_msgs::Pose resp;
bwi_mapper::Point2f from_map =
bwi_mapper::toMap(from, map_info_);
bwi_mapper::Point2f at_map =
bwi_mapper::toMap(at, map_info_);
// Figure out if you want to stay on the outside angle or not
bool use_outside_angle = true;
float yaw1 = -atan2((to - at).y, (to - at).x);
float yaw2 = -atan2((from - at).y, (from - at).x);
bwi_mapper::Point2f yaw1_pt(cosf(yaw1),sinf(yaw1));
bwi_mapper::Point2f yaw2_pt(cosf(yaw2),sinf(yaw2));
bwi_mapper::Point2f yawmid_pt = 0.5 * (yaw1_pt + yaw2_pt);
if (bwi_mapper::getMagnitude(yawmid_pt) < 0.1) {
use_outside_angle = false;
}
size_t y_test = at.y - search_distance_ / map_info_.resolution;
float location_fitness = -1;
bwi_mapper::Point2f test_coords;
bwi_mapper::Point2f map_coords;
while(y_test < at.y + search_distance_ / map_info_.resolution) {
size_t x_test = at.x - search_distance_ / map_info_.resolution;
while (x_test < at.x + search_distance_ / map_info_.resolution) {
bwi_mapper::Point2f test(x_test, y_test);
// Check if x_test, y_test is free.
size_t map_idx = MAP_IDX(map_info_.width, x_test, y_test);
if (inflated_map_.data[map_idx] != 0) {
x_test++;
continue;
}
// Check if it is on the outside or not
if (use_outside_angle) {
if ((from + to - 2 * at).dot(test - at) > 0) {
x_test++;
continue;
}
}
bwi_mapper::Point2f test_loc(x_test, y_test);
float dist1 =
bwi_mapper::minimumDistanceToLineSegment(from, at, test_loc);
float dist2 =
bwi_mapper::minimumDistanceToLineSegment(at, to, test_loc);
float fitness = std::min(dist1, dist2);
if (fitness > location_fitness) {
test_coords = test_loc;
map_coords = bwi_mapper::toMap(test_loc, map_info_);
resp.position.x = map_coords.x;
resp.position.y = map_coords.y;
location_fitness = fitness;
}
x_test++;
}
y_test++;
}
// Calculate yaw
yaw1 = atan2(resp.position.y - at_map.y, resp.position.x - at_map.x);
yaw2 = atan2(resp.position.y - from_map.y, resp.position.x - from_map.x);
yaw1_pt = bwi_mapper::Point2f(cosf(yaw1),sinf(yaw1));
yaw2_pt = bwi_mapper::Point2f(cosf(yaw2),sinf(yaw2));
yawmid_pt = 0.5 * (yaw1_pt + yaw2_pt);
/* float resp_yaw = atan2f(yawmid_pt.y, yawmid_pt.x); */
float resp_yaw = yaw2;
resp.orientation = tf::createQuaternionMsgFromYaw(resp_yaw);
if (debug_) {
cv::Mat image;
mapper_->drawMap(image, inflated_map_);
cv::circle(image, from, 5, cv::Scalar(255, 0, 0), -1);
cv::circle(image, at, 5, cv::Scalar(255, 0, 255), 2);
cv::circle(image, to, 5, cv::Scalar(0, 0, 255), -1);
cv::circle(image, test_coords, 5, cv::Scalar(0, 255, 0), 2);
cv::circle(image, test_coords +
bwi_mapper::Point2f(20 * cosf(resp_yaw), 20 * sinf(resp_yaw)),
3, cv::Scalar(0, 255, 0), -1);
cv::namedWindow("Display window", CV_WINDOW_AUTOSIZE);
cv::imshow("Display window", image);
cv::waitKey(0);
}
return resp;
}
void TopologicalMapper::computeGraph(double merge_threshold) {
// First for each critical point, find out the neigbouring regions
std::vector<std::set<uint32_t> > point_neighbour_sets;
point_neighbour_sets.resize(critical_points_.size());
// Draw the critical lines as their indexes on the map
cv::Mat lines(map_resp_.map.info.height, map_resp_.map.info.width, CV_16UC1,
cv::Scalar((uint16_t)-1));
drawCriticalLines(lines, 0, 0, true);
// Go over all the pixels in the lines image and find neighbouring critical
// regions
for (int j = 0; j < lines.rows; ++j) {
uint16_t* image_row_j = lines.ptr<uint16_t>(j);
for (int i = 0; i < lines.cols; ++i) {
uint16_t pixel = image_row_j[i];
if (pixel == (uint16_t)-1) {
continue;
}
int x_offset[] = {0, -1, 1, 0};
int y_offset[] = {-1, 0, 0, 1};
size_t num_neighbours = 4;
for (size_t count = 0; count < num_neighbours; ++count) {
uint32_t x_n = i + x_offset[count];
uint32_t y_n = j + y_offset[count];
if (x_n >= (uint16_t) lines.cols || y_n >= (uint16_t) lines.rows) {
continue;
}
size_t map_idx = MAP_IDX(lines.cols, x_n, y_n);
if (component_map_[map_idx] >= 0 &&
component_map_[map_idx] < (int32_t) num_components_) {
point_neighbour_sets[pixel].insert(component_map_[map_idx]);
}
}
}
}
// Throw out any critical points that do not have 2 adjoining regions.
// This can happen if the regions are too small
std::vector<std::vector<uint32_t> > point_neighbours;
point_neighbours.resize(critical_points_.size());
std::vector<int> remove_crit_points;
for (size_t i = 0; i < critical_points_.size(); ++i) {
if (point_neighbour_sets[i].size() == 2) {
point_neighbours[i] =
std::vector<uint32_t>(point_neighbour_sets[i].begin(),
point_neighbour_sets[i].end());
} else {
remove_crit_points.push_back(i);
}
}
for (int i = remove_crit_points.size() - 1; i >=0 ; --i) {
critical_points_.erase(critical_points_.begin() + i);
point_neighbour_sets.erase(point_neighbour_sets.begin() + i);
point_neighbours.erase(point_neighbours.begin() + i);
}
// Find connectivity to remove any isolated sub-graphs
std::vector<bool> checked_region(critical_points_.size(), false);
std::vector<std::set<int> > graph_sets;
for (size_t i = 0; i < num_components_; ++i) {
if (checked_region[i]) {
continue;
}
std::set<int> open_set, closed_set;
open_set.insert(i);
while (open_set.size() != 0) {
int current_region = *(open_set.begin());
open_set.erase(open_set.begin());
closed_set.insert(current_region);
checked_region[current_region] = true;
for (int j = 0; j < critical_points_.size(); ++j) {
for (int k = 0; k < 2; ++k) {
if (point_neighbours[j][k] == current_region &&
std::find(closed_set.begin(), closed_set.end(),
point_neighbours[j][1-k]) ==
closed_set.end()) {
open_set.insert(point_neighbours[j][1-k]);
}
}
}
}
graph_sets.push_back(closed_set);
}
std::set<int> master_region_set;
if (graph_sets.size() != 1) {
std::cout << "WARNING: Master graph is fragmented into " <<
graph_sets.size() << " sub-graphs!!!" << std::endl;
int max_idx = 0;
int max_size = graph_sets[0].size();
for (size_t i = 1; i < graph_sets.size(); ++i) {
if (graph_sets[i].size() > max_size) {
max_idx = i;
max_size = graph_sets[i].size();
}
}
master_region_set = graph_sets[max_idx];
} else {
master_region_set = graph_sets[0];
}
// Create the region graph next
std::map<int, int> region_to_vertex_map;
int vertex_count = 0;
for (size_t r = 0; r < num_components_; ++r) {
if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
master_region_set.end()) {
region_to_vertex_map[r] = -1;
continue;
}
Graph::vertex_descriptor vi = boost::add_vertex(region_graph_);
// Calculate the centroid
uint32_t avg_i = 0, avg_j = 0, pixel_count = 0;
for (size_t j = 0; j < map_resp_.map.info.height; ++j) {
for (size_t i = 0; i < map_resp_.map.info.width; ++i) {
size_t map_idx = MAP_IDX(map_resp_.map.info.width, i, j);
if (component_map_[map_idx] == (int)r) {
avg_j += j;
avg_i += i;
pixel_count++;
}
}
}
region_graph_[vi].location.x = ((float) avg_i) / pixel_count;
region_graph_[vi].location.y = ((float) avg_j) / pixel_count;
region_graph_[vi].pixels = floor(sqrt(pixel_count));
// This map is only required till the point we form edges on this graph
region_to_vertex_map[r] = vertex_count;
++vertex_count;
}
// Now that region_to_vertex_map is complete,
// forward critical points to next graph
std::map<int, std::map<int, VoronoiPoint> >
region_vertex_crit_points;
for (size_t r = 0; r < num_components_; ++r) {
if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
master_region_set.end()) {
region_to_vertex_map[r] = -1;
continue;
}
// Store the critical points corresponding to this vertex
for (int j = 0; j < critical_points_.size(); ++j) {
for (int k = 0; k < 2; ++k) {
if (point_neighbours[j][k] == r) {
int region_vertex = region_to_vertex_map[r];
int other_region_vertex = region_to_vertex_map[point_neighbours[j][1-k]];
if (region_vertex == 192 && other_region_vertex == 202) {
std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
}
if (region_vertex == 202 && other_region_vertex == 192) {
std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
}
region_vertex_crit_points[region_vertex]
[region_to_vertex_map[point_neighbours[j][1-k]]] =
critical_points_[j];
}
}
}
}
// Create 1 edge per critical point
for (size_t i = 0; i < critical_points_.size(); ++i) {
int region1 = region_to_vertex_map[point_neighbours[i][0]];
int region2 = region_to_vertex_map[point_neighbours[i][1]];
if (region1 == -1) {
// part of one of the sub-graphs that was discarded
continue;
}
int count = 0;
Graph::vertex_descriptor vi,vj;
vi = boost::vertex(region1, region_graph_);
vj = boost::vertex(region2, region_graph_);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::edge(vi, vj, region_graph_);
if (!b) {
boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_);
}
/* boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_); */
// region_graph_[e].weight = bwi_mapper::getMagnitude(
// region_graph_[vi].location - region_graph_[vj].location);
}
// Refine the region graph into the point graph:
int pixel_threshold = merge_threshold / map_resp_.map.info.resolution;
enum {
PRESENT = 0,
REMOVED_REGION_VERTEX = 1,
CONVERT_TO_CRITICAL_POINT = 2,
MERGE_VERTEX = 3
};
Graph::vertex_iterator vi, vend;
// PASS 1 - resolve all vertices that are too big and have more than 2
// critical points to their underlying critical points
std::cout << std::endl << "==============================" << std::endl;
std::cout << "PASS 1" << std::endl;
std::cout << "==============================" << std::endl << std::endl;
Graph pass_0_graph = region_graph_;
Graph pass_1_graph;
int pass_0_count = 0;
int pass_1_count = 0;
std::vector<int> pass_0_vertex_status(
boost::num_vertices(pass_0_graph), PRESENT);
std::map<int, int> pass_0_vertex_to_pass_1_map;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
std::cout << "Analyzing pass 0 graph vertex: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
// See if the area of this region is too big to be directly pushed into
// the pass 3 graph
if (pass_0_graph[*vi].pixels >= pixel_threshold &&
adj_vertices.size() > 2) {
pass_0_vertex_status[pass_0_count] = CONVERT_TO_CRITICAL_POINT;
std::cout << " - throwing it out (needs to be resolved to CPs)" <<
std::endl;
continue;
}
// Otherwise insert this as is into the point graph
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi] = pass_0_graph[*vi];
pass_0_vertex_to_pass_1_map[pass_0_count] = pass_1_count;
++pass_1_count;
}
// Now for each vertex that needs to be resolved into critical points,
// check neighbours recursively to convert these vertices into critical
// points
std::map<int, std::map<int, int> > pass_0_vertex_to_cp_map;
pass_0_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
if (pass_0_vertex_status[pass_0_count] != CONVERT_TO_CRITICAL_POINT) {
continue;
}
std::cout << "Converting pass1 vtx to CPs: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
std::vector<int> connect_edges;
BOOST_FOREACH(size_t vtx, adj_vertices) {
if (pass_0_vertex_status[vtx] == PRESENT) {
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi].location =
region_vertex_crit_points[pass_0_count][vtx];
std::cout << " - added vtx at " << pass_1_graph[point_vi].location
<< std::endl;
int pass_1_vertex = pass_0_vertex_to_pass_1_map[vtx];
std::cout << " - connecting vtx to pass_1_vertex: " << pass_1_vertex
<< " (pass0 = " << vtx << ")" << std::endl;
Graph::vertex_descriptor vj;
vj = boost::vertex(pass_1_vertex, pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(point_vi, vj, pass_1_graph);
connect_edges.push_back(pass_1_count);
++pass_1_count;
} else if (vtx > pass_0_count) {
// The CP is shared, but has not been added
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi].location =
region_vertex_crit_points[pass_0_count][vtx];
std::cout << " - added shared cp with " << vtx << " at "
<< pass_1_graph[point_vi].location << std::endl;
pass_0_vertex_to_cp_map[pass_0_count][vtx] = pass_1_count;
connect_edges.push_back(pass_1_count);
++pass_1_count;
} else {
// Retrieve existing CP
int cp_vtx = pass_0_vertex_to_cp_map[vtx][pass_0_count];
connect_edges.push_back(cp_vtx);
}
}
/* Connect all the edges */
for (int i = 0; i < connect_edges.size(); ++i) {
for (int j = 0; j < i; ++j) {
Graph::vertex_descriptor vi, vj;
vi = boost::vertex(connect_edges[i], pass_1_graph);
vj = boost::vertex(connect_edges[j], pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
}
}
}
// Connect all the edges as is from pass 1
pass_0_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
if (pass_0_vertex_status[pass_0_count] != PRESENT) {
continue;
}
std::cout << "Adding pass 1 edges for: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
BOOST_FOREACH(size_t vtx, adj_vertices) {
if (pass_0_vertex_status[vtx] == PRESENT && vtx < pass_0_count) {
Graph::vertex_descriptor vi, vj;
vi = boost::vertex(
pass_0_vertex_to_pass_1_map[pass_0_count], pass_1_graph);
vj = boost::vertex(
pass_0_vertex_to_pass_1_map[vtx], pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
}
}
}
pass_1_graph_ = pass_1_graph;
// PASS 2 - remove any vertices that are adjacent to only 2 other vertices,
// where both those vertices are visible to each other
std::cout << std::endl << "==============================" << std::endl;
std::cout << "PASS 2" << std::endl;
std::cout << "==============================" << std::endl << std::endl;
Graph pass_2_graph;
std::vector<int> pass_1_vertex_status(boost::num_vertices(pass_1_graph), PRESENT);
std::map<int, int> pass_1_vertex_to_pass_2_vertex_map;
std::vector<std::pair<int, int> > rr_extra_edges;
std::map<int, std::pair<int, int> > removed_vertex_map;
pass_1_count = 0;
int pass_2_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
++vi, ++pass_1_count) {
std::cout << "Analyzing pass_1 graph vertex: " << pass_1_count << std::endl;
if (pass_1_vertex_status[pass_1_count] != PRESENT) {
std::cout << " - the vertex has been thrown out already " << std::endl;
continue;
}
// See if this only has 2 neighbours, and the 2 neighbours are visible to
// each other
std::vector<size_t> open_vertices;
int current_vertex = pass_1_count;
getAdjacentNodes(current_vertex, pass_1_graph, open_vertices);
std::vector<int> removed_vertices;
std::pair<int, int> edge;
if (open_vertices.size() == 2 &&
pass_1_vertex_status[open_vertices[0]] == PRESENT &&
pass_1_vertex_status[open_vertices[1]] == PRESENT) {
while(true) {
std::cout << " - has 2 adjacent vertices " << open_vertices[0] << ", "
<< open_vertices[1] << std::endl;
// Check if the 2 adjacent vertices are visible
Point2f location1 =
getLocationFromGraphId(open_vertices[0], pass_1_graph);
Point2f location2 =
getLocationFromGraphId(open_vertices[1], pass_1_graph);
bool locations_visible =
locationsInDirectLineOfSight(location1, location2, map_resp_.map);
if (locations_visible) {
std::cout << " - the 2 adjacent vertices are visible "
<< "- removing vertex." << std::endl;
pass_1_vertex_status[current_vertex] = REMOVED_REGION_VERTEX;
removed_vertices.push_back(current_vertex);
edge = std::make_pair(open_vertices[0], open_vertices[1]);
bool replacement_found = false;
for (int i = 0; i < 2; ++i) {
std::vector<size_t> adj_vertices;
getAdjacentNodes(open_vertices[i], pass_1_graph,
adj_vertices);
if (adj_vertices.size() == 2) {
size_t new_vertex = (adj_vertices[0] == current_vertex) ?
adj_vertices[1] : adj_vertices[0];
if (pass_1_vertex_status[new_vertex] == PRESENT) {
current_vertex = open_vertices[i];
std::cout << " - neighbours may suffer from the same problem"
<< ". checking vertex " << current_vertex << std::endl;
open_vertices[i] = new_vertex;
replacement_found = true;
break;
}
}
}
if (!replacement_found) {
// Both neighbours on either side had 1 or more than 2 adjacent
// vertices
break;
}
} else {
// left and right vertices not visible
break;
}
}
}
BOOST_FOREACH(int removed_vertex, removed_vertices) {
removed_vertex_map[removed_vertex] = edge;
}
if (removed_vertices.size() != 0) {
// Add the extra edge
std::cout << " - adding extra edge between " << edge.first <<
" " << edge.second << std::endl;
rr_extra_edges.push_back(edge);
continue;
}
// Otherwise insert this as is into the point graph
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_2_graph);
pass_2_graph[point_vi] = pass_1_graph[*vi];
pass_1_vertex_to_pass_2_vertex_map[pass_1_count] =
pass_2_count;
++pass_2_count;
}
// Insert all edges that can be inserted
// Add edges from the pass_1 graph that can be placed as is
pass_1_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
++vi, ++pass_1_count) {
if (pass_1_vertex_status[pass_1_count] == PRESENT) {
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_1_count, pass_1_graph, adj_vertices);
BOOST_FOREACH(size_t adj_vertex, adj_vertices) {
if (pass_1_vertex_status[adj_vertex] == PRESENT &&
adj_vertex > pass_1_count) {
Graph::vertex_descriptor vi,vj;
int vertex1 =
pass_1_vertex_to_pass_2_vertex_map[pass_1_count];
int vertex2 = pass_1_vertex_to_pass_2_vertex_map[adj_vertex];
vi = boost::vertex(vertex1, pass_2_graph);
vj = boost::vertex(vertex2, pass_2_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_2_graph);
}
}
}
}
void
loadMapFromFile(nav_msgs::GetMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th, double* origin,
bool trinary)
{
SDL_Surface* img;
unsigned char* pixels;
unsigned char* p;
unsigned char value;
int rowstride, n_channels, avg_channels;
unsigned int i,j;
int k;
double occ;
int alpha;
int color_sum;
double color_avg;
// Load the image using SDL. If we get NULL back, the image load failed.
if(!(img = IMG_Load(fname)))
{
std::string errmsg = std::string("failed to open image file \"") +
std::string(fname) + std::string("\"");
throw std::runtime_error(errmsg);
}
// Copy the image data into the map structure
resp->map.info.width = img->w;
resp->map.info.height = img->h;
resp->map.info.resolution = res;
resp->map.info.origin.position.x = *(origin);
resp->map.info.origin.position.y = *(origin+1);
resp->map.info.origin.position.z = 0.0;
tf::Quaternion q;
q.setRPY(0,0, *(origin+2));
resp->map.info.origin.orientation.x = q.x();
resp->map.info.origin.orientation.y = q.y();
resp->map.info.origin.orientation.z = q.z();
resp->map.info.origin.orientation.w = q.w();
// Allocate space to hold the data
resp->map.data.resize(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
n_channels = img->format->BytesPerPixel;
if (trinary || n_channels == 1)
avg_channels = n_channels;
else
avg_channels = n_channels - 1;
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
for(j = 0; j < resp->map.info.height; j++)
{
for (i = 0; i < resp->map.info.width; i++)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
color_sum = 0;
for(k=0;k<avg_channels;k++)
color_sum += *(p + (k));
color_avg = color_sum / (double)avg_channels;
if (n_channels == 1)
alpha = 1;
else
alpha = *(p+n_channels-1);
// If negate is true, we consider blacker pixels free, and whiter
// pixels free. Otherwise, it's vice versa.
if(negate)
occ = color_avg / 255.0;
else
occ = (255 - color_avg) / 255.0;
// Apply thresholds to RGB means to determine occupancy values for
// map. Note that we invert the graphics-ordering of the pixels to
// produce a map with cell (0,0) in the lower-left corner.
if(occ > occ_th)
value = +100;
else if(occ < free_th)
value = 0;
else if(trinary || alpha < 1.0)
value = -1;
else {
double ratio = (occ - free_th) / (occ_th - free_th);
value = 99 * ratio;
}
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
}
}
SDL_FreeSurface(img);
}
bool CostMapCalculation::calculateCostMap_old(char map_level)
{
switch(map_level)
{
case USE_GRID_MAP_ONLY:
{
// cost map based on grid map only
ROS_DEBUG("HectorCM: compute costmap based on grid map");
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
// check if cell is known
if((char)grid_map_.data[index] != UNKNOWN_CELL)
{
if(grid_map_.data[index] == OCCUPIED_CELL)
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
else
{
// cell is not occupied
cost_map.data[index] = FREE_CELL;
}
}
}
}
break;
}
case USE_ELEVATION_MAP_ONLY:
{
// cost map based on elevation map only
ROS_DEBUG("HectorCM: compute costmap based on elevation map");
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
// check if cell is known
if(elevation_cost_map.data[index] != UNKNOWN_CELL)
{
if(elevation_cost_map.data[index] == OCCUPIED_CELL)
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
else
{
// cell is not occupied
cost_map.data[index] = FREE_CELL;
}
}
}
}
break;
}
case USE_GRID_AND_ELEVATION_MAP:
{
// cost map based on elevation and grid map
ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map");
int checksum_grid_map;
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
// check if cell is known
if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
{
if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
{
checksum_grid_map = 0;
checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
checksum_grid_map += grid_map_.at<int8_t>(v, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
{
if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
{
// cell is free
cost_map.data[index] = FREE_CELL;
}
else
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
}
else
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
}
else
{
cost_map.data[index] = FREE_CELL;
}
}
}
}
break;
}
case USE_GRID_AND_CLOUD_MAP:
{
// cost map based on cloud map and grid map
ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map");
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
// check if cell is known
if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
{
if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL)
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
else
{
cost_map.data[index] = FREE_CELL;
}
}
}
}
break;
}
case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP:
{
// cost map based on elevation, cloud and grid map
ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map");
int checksum_grid_map;
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
// check if cell is known
if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
{
if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
{
checksum_grid_map = 0;
checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
checksum_grid_map += grid_map_.at<int8_t>(v, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
{
if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
{
// cell is free
cost_map.data[index] = FREE_CELL;
}
else
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
}
else
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
}
else
{
if(cloud_cost_map.data[index] == OCCUPIED_CELL)
{
// cell is occupied
cost_map.data[index] = OCCUPIED_CELL;
}
else
{
// cell is free
cost_map.data[index] = FREE_CELL;
}
}
}
}
}
break;
}
}
ROS_DEBUG("HectorCM: computed a new costmap");
return true;
}
void ElevationMapping::timerCallback(const ros::TimerEvent& event)
{
tf::StampedTransform local_map_transform;
// get local map transform
try
{
listener.waitForTransform(map_frame_id, local_map_frame_id,ros::Time(0),ros::Duration(1.0));
listener.lookupTransform(map_frame_id, local_map_frame_id,ros::Time(0), local_map_transform);
}
catch (tf::TransformException ex)
{
ROS_DEBUG("HectorEM: localmap transform in timer callback failed");
ROS_ERROR("%s",ex.what());
return;
}
// allign grid points
Eigen::Vector2f index_world(local_map_transform.getOrigin().x(), local_map_transform.getOrigin().y());
Eigen::Vector2f index_map = world_map_transform.getC2Coords(index_world);
// check if elevation of current robot position is known, otherwise cancel pose update
if(global_elevation_map.data[MAP_IDX(elevation_map_meta.width, (int)index_map(0), (int)index_map(1))] != (int16_t)-elevation_map_meta.zero_elevation)
{
geometry_msgs::PoseWithCovarianceStamped cell_height_average;
int error_level = 0;
int pattern_cell_quantity = 4*poseupdate_used_pattern_size*poseupdate_used_pattern_size;
/// todo check if min/max index is inside map
// include neighbours
for(int x=index_map(0)-poseupdate_used_pattern_size;x<index_map(0)+poseupdate_used_pattern_size;x++)
{
for(int y=index_map(1)-poseupdate_used_pattern_size;y<index_map(1)+poseupdate_used_pattern_size;y++)
{
int cell_index = MAP_IDX(elevation_map_meta.width, x, y);
if(global_elevation_map.data[cell_index] == (int16_t)-elevation_map_meta.zero_elevation)
{
// unknown cell
error_level++;
}
else
{
// cell is knwon, accumulate cell hight
cell_height_average.pose.pose.position.z += (global_elevation_map.data[cell_index]-elevation_map_meta.zero_elevation)*elevation_map_meta.resolution_z;
}
}
}
// only publish pose update, if more than 1/2 of pattern cells are known
if(error_level < pattern_cell_quantity/2)
{
pattern_cell_quantity -= error_level;
cell_height_average.pose.pose.position.z = cell_height_average.pose.pose.position.z/(double)pattern_cell_quantity;
cell_height_average.header.frame_id = map_frame_id;
cell_height_average.header.stamp = ros::Time::now();
cell_height_average.pose.covariance.at(0) = 0.0; //no x-position information
cell_height_average.pose.covariance.at(7) = 0.0; //no y-position information
cell_height_average.pose.covariance.at(14) = 1.0/poseupdate_height_covariance;
pub_height_update.publish(cell_height_average);
ROS_DEBUG("HectorEM: published height update %f",cell_height_average.pose.pose.position.z);
}
}
}
bool CostMapCalculation::calculateCostMap(char map_level)
{
if (!map_level)
{
ROS_WARN("Invalid map selection was given to cost map calculation!");
return false;
}
if (map_level & GRID_MAP) ROS_DEBUG("HectorCM: compute costmap based on grid map");
if (map_level & ELEVATION_MAP) ROS_DEBUG("HectorCM: compute costmap based on elevation map");
if (map_level & CLOUD_MAP) ROS_DEBUG("HectorCM: compute costmap based on cloud map");
// loop through each element
for (int v = min_index(1); v < max_index(1); ++v)
{
for (int u = min_index(0); u < max_index(0); ++u)
{
unsigned int index = MAP_IDX(cost_map.info.width, u, v);
int checksum_grid_map = 0;
cost_map.data[index] = UNKNOWN_CELL;
// grid map
if (map_level & GRID_MAP)
{
switch (grid_map_.data[index])
{
case OCCUPIED_CELL:
if (map_level & ELEVATION_MAP && allow_elevation_map_to_clear_occupied_cells)
{
checksum_grid_map += grid_map_.at<int8_t>(v-1, u );
checksum_grid_map += grid_map_.at<int8_t>(v+1, u );
checksum_grid_map += grid_map_.at<int8_t>(v, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
}
cost_map.data[index] = OCCUPIED_CELL;
break;
case FREE_CELL: cost_map.data[index] = FREE_CELL; break;
}
}
// point cloud
if (map_level & CLOUD_MAP)
{
if (cost_map.data[index] != OCCUPIED_CELL)
{
switch (cloud_cost_map.data[index])
{
case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
case FREE_CELL: cost_map.data[index] = FREE_CELL; break;
}
}
}
// elevation map
if (map_level & ELEVATION_MAP)
{
if (cost_map.data[index] != OCCUPIED_CELL || (allow_elevation_map_to_clear_occupied_cells && checksum_grid_map <= max_clear_size*OCCUPIED_CELL))
{
switch (elevation_cost_map.data[index])
{
case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
case FREE_CELL: cost_map.data[index] = FREE_CELL; break;
}
}
}
}
}
ROS_DEBUG("HectorCM: computed a new costmap");
return true;
}
void ElevationMapping::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_sensor_msg)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud2_map_pcl (new pcl::PointCloud<pcl::PointXYZ> ()),
pointcloud2_sensor_pcl (new pcl::PointCloud<pcl::PointXYZ> ());
tf::StampedTransform local_map_transform;
ROS_DEBUG("HectorEM received a point cloud.");
// converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
pcl::fromROSMsg(*pointcloud2_sensor_msg, *pointcloud2_sensor_pcl);
// transform cloud to /map frame
try
{
listener.waitForTransform(map_frame_id, pointcloud2_sensor_msg->header.frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
pcl_ros::transformPointCloud(map_frame_id,*pointcloud2_sensor_pcl,*pointcloud2_map_pcl,listener);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorEM: pointcloud transform failed");
return;
}
// get local map transform
try
{
listener.waitForTransform(map_frame_id, local_map_frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
listener.lookupTransform(map_frame_id, local_map_frame_id, pointcloud2_sensor_msg->header.stamp, local_map_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorEM: localmap transform in cloud callback failed");
return;
}
bool local_map_is_subscribed = (pub_local_map.getNumSubscribers () > 0);
bool global_map_is_subscribed = (pub_global_map.getNumSubscribers () > 0);
if(local_map_is_subscribed)
local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
unsigned int size = (unsigned int)pointcloud2_map_pcl->points.size();
// iterate trough all points
for (unsigned int k = 0; k < size; ++k)
{
const pcl::PointXYZ& pt_cloud = pointcloud2_map_pcl->points[k];
double measurement_distance = pointcloud2_sensor_pcl->points[k].z;
// check for invalid measurements
if (isnan(pt_cloud.x) || isnan(pt_cloud.y) || isnan(pt_cloud.z))
continue;
// check max distance (manhatten norm)
if(max_observable_distance < measurement_distance)
continue;
// check min/max height
if(elevation_map_meta.min_elevation+local_map_transform.getOrigin().z() > pt_cloud.z || elevation_map_meta.max_elevation+local_map_transform.getOrigin().z() < pt_cloud.z)
continue;
// allign grid points
Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
unsigned int cell_index = MAP_IDX(elevation_map_meta.width, (int)round(index_map(0)), (int)round(index_map(1)));
int16_t* pt_local_map = &local_elevation_map.data[cell_index];
int16_t* pt_global_map = &global_elevation_map.data[cell_index];
double* pt_var = &cell_variance[cell_index];
if(local_map_is_subscribed)
{
// elevation in current cell in meter
double cell_elevation = elevation_map_meta.resolution_z*(*pt_local_map-elevation_map_meta.zero_elevation);
// store maximum of each cell
if(pt_cloud.z > cell_elevation)
*pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// filter each cell localy
// double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
// if(*pt_local_map == (int16_t)-elevation_map_meta.zero_elevation)
// {
// // unknown cell -> use current measurement
// *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// *pt_var = measurement_variance;
// }
// else
// {
// // fuse cell_elevation with measurement
// *pt_local_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
// }
}
if(publish_poseupdate || global_map_is_subscribed)
{
// fuse new measurements with existing map
// elevation in current cell in meter
double cell_elevation = elevation_map_meta.resolution_z*(*pt_global_map-elevation_map_meta.zero_elevation);
// measurement variance
double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
// mahalanobis distance
double mahalanobis_distance = sqrt((pt_cloud.z - cell_elevation)*(pt_cloud.z - cell_elevation)/(measurement_variance*measurement_variance));
if(pt_cloud.z > cell_elevation && (mahalanobis_distance > 5.0))
{
*pt_global_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
*pt_var = measurement_variance;
continue;
}
if((pt_cloud.z < cell_elevation) && (mahalanobis_distance > 5.0))
{
*pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
//*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
*pt_var = measurement_variance;
continue;
}
*pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
}
}
if(local_map_is_subscribed)
{
// set the header information on the map
local_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
local_elevation_map.header.frame_id = map_frame_id;
pub_local_map.publish(local_elevation_map);
}
if(global_map_is_subscribed)
{
// set the header information on the map
global_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
global_elevation_map.header.frame_id = map_frame_id;
pub_global_map.publish(global_elevation_map);
}
}
void
SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
boost::mutex::scoped_lock map_lock (map_mutex_);
GMapping::ScanMatcher matcher;
double* laser_angles = new double[scan.ranges.size()];
double theta = angle_min_;
for(unsigned int i=0; i<scan.ranges.size(); i++)
{
if (gsp_laser_angle_increment_ < 0)
laser_angles[scan.ranges.size()-i-1]=theta;
else
laser_angles[i]=theta;
theta += gsp_laser_angle_increment_;
}
matcher.setLaserParameters(scan.ranges.size(), laser_angles,
gsp_laser_->getPose());
delete[] laser_angles;
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];
std_msgs::Float64 entropy;
entropy.data = computePoseEntropy();
if(entropy.data > 0.0)
entropy_publisher_.publish(entropy);
if(!got_map_) {
map_.map.info.resolution = delta_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;
center.y=(ymin_ + ymax_) / 2.0;
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
delta_);
ROS_DEBUG("Trajectory tree:");
for(GMapping::GridSlamProcessor::TNode* n = best.node;
n;
n = n->parent)
{
ROS_DEBUG(" %.3f %.3f %.3f",
n->pose.x,
n->pose.y,
n->pose.theta);
if(!n->reading)
{
ROS_DEBUG("Reading is NULL");
continue;
}
matcher.invalidateActiveArea();
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
}
// the map may have expanded, so resize ros message as well
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
// so we must obtain the bounding box in a different way
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;
ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
xmin_, ymin_, xmax_, ymax_);
map_.map.info.width = smap.getMapSizeX();
map_.map.info.height = smap.getMapSizeY();
map_.map.info.origin.position.x = xmin_;
map_.map.info.origin.position.y = ymin_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
}
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
}
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;
//make sure to set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
}
void ElevationVisualization::visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform)
{
int current_height_level;
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
map_marker_array_msg.markers[i].points.clear();
}
map_marker_array_msg.markers.clear();
// each array stores all cubes of one height level:
map_marker_array_msg.markers.resize(max_height_levels+1);
for (int index_y = 0; index_y < (int)elevation_map.info.height; ++index_y)
{
for (int index_x = 0; index_x < (int)elevation_map.info.width; ++index_x)
{
// visualize only known cells
if(elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)] != (int16_t)-elevation_map.info.zero_elevation)
{
geometry_msgs::Point cube_center;
Eigen::Vector2f index_map(index_x, index_y);
Eigen::Vector2f index_world = world_map_transform.getC1Coords(index_map);
cube_center.x = index_world(0);//+elevation_map.info.resolution_xy/2.0;
cube_center.y = index_world(1);//+elevation_map.info.resolution_xy/2.0;
cube_center.z = (elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)]-elevation_map.info.zero_elevation)*elevation_map.info.resolution_z;
current_height_level = max_height_levels/2+(int)round(std::min(std::max((double)cube_center.z+local_map_transform.getOrigin().z(), -(double)max_height), (double)max_height)*(double)max_height_levels/((double)max_height*2.0f));
map_marker_array_msg.markers[current_height_level].points.push_back(cube_center);
if(use_color_map)
{
double h = (1.0 - std::min(std::max((cube_center.z-min_height)/ (max_height - min_height), 0.0), 1.0)) *color_factor;
map_marker_array_msg.markers[current_height_level].colors.push_back(heightMapColor(h));
}
}
}
}
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
std::stringstream ss;
ss <<"Level "<<i;
map_marker_array_msg.markers[i].ns = ss.str();
map_marker_array_msg.markers[i].id = i;
map_marker_array_msg.markers[i].header.frame_id = "/map";
map_marker_array_msg.markers[i].header.stamp = elevation_map.header.stamp;
map_marker_array_msg.markers[i].lifetime = ros::Duration();
map_marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
map_marker_array_msg.markers[i].scale.x = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.y = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.z = elevation_map.info.resolution_z;
map_marker_array_msg.markers[i].color = marker_color;
if (map_marker_array_msg.markers[i].points.size() > 0)
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
else
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::DELETE;
}
}