plannerMapServer.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *      * Redistributions of source code must retain the above copyright
 *              notice, this list of conditions and the following disclaimer.
 *      * Redistributions in binary form must reproduce the above copyright
 *              notice, this list of conditions and the following disclaimer in the
 *              documentation and/or other materials provided with the distribution.
 *      * Neither the name of the Willow Garage, Inc. nor the names of its
 *              contributors may be used to endorse or promote products derived from
 *              this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <fstream>

#include <youbot_overhead_localization/plannerMapServer.h>

//TODO: Temporary include until image resizing is moved to vision
#include <opencv2/imgproc/imgproc.hpp>

using namespace std;

plannerMapServer::plannerMapServer()
{
  mapfname = "";
  std::string fname;
  n.getParam("/planner_map_server/map_file_yaml", fname);
  n.param("frameID", frameID, std::string("map"));

  std::ifstream fin(fname.c_str());
  if (fin.fail())
  {
    ROS_ERROR("Map_server could not open %s.", fname.c_str());
    exit(-1);
  }
  YAML::Parser parser(fin);
  YAML::Node doc;
  parser.GetNextDocument(doc);
  try
  {
    doc["resolution"] >> resolution;
  }
  catch (YAML::InvalidScalar &)
  {
    ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
    exit(-1);
  }
  try
  {
    doc["negate"] >> negate;
  }
  catch (YAML::InvalidScalar &)
  {
    ROS_ERROR("The map does not contain a negate tag or it is invalid.");
    exit(-1);
  }
  try
  {
    doc["occupied_thresh"] >> occThresh;
  }
  catch (YAML::InvalidScalar &)
  {
    ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
    exit(-1);
  }
  try
  {
    doc["free_thresh"] >> freeThresh;
  }
  catch (YAML::InvalidScalar &)
  {
    ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
    exit(-1);
  }
  try
  {
    doc["origin"][0] >> origin[0];
    doc["origin"][1] >> origin[1];
    doc["origin"][2] >> origin[2];
  }
  catch (YAML::InvalidScalar &)
  {
    ROS_ERROR("The map does not contain an origin tag or it is invalid.");
    exit(-1);
  }

  service = n.advertiseService("static_map", &plannerMapServer::mapCallback, this);

  // Latched publisher for metadata
  metadataPublisher = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  metadataPublisher.publish(metaDataMessage);

  // Latched publisher for data
  mapPublisher = n.advertise<nav_msgs::OccupancyGrid>("/map", 1, true);

  //Subscriber to update the map from new images
  mapImageSubscriber = n.subscribe("/image_calibration/image_cal", 1, &plannerMapServer::mapUpdateCallback, this);
}

void plannerMapServer::mapUpdateCallback(const sensor_msgs::Image& mapImg)
{
  cv_bridge::CvImagePtr cvPtr = cv_bridge::toCvCopy(mapImg, "bgr8");
  cv::flip(cvPtr->image, cvPtr->image, 0);
  loadMapFromMat(&mapResponse, &cvPtr->image, resolution, negate, occThresh, freeThresh, origin);

  mapResponse.map.info.map_load_time = ros::Time::now();
  mapResponse.map.header.frame_id = frameID;
  mapResponse.map.header.stamp = ros::Time::now();
  metaDataMessage = mapResponse.map.info;

  metadataPublisher.publish(metaDataMessage);
  mapPublisher.publish(mapResponse.map);
}

bool plannerMapServer::mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
  // = operator is overloaded to make deep copy (tricky!)
  res = mapResponse;
  ROS_INFO("Sending map");

  return true;
}

void plannerMapServer::loadMapFromMat(nav_msgs::GetMap::Response* resp, cv::Mat *opencvimg, double res, bool negate,
                                      double occThresh, double freeThresh, double* origin)
{
  unsigned char* pixels;
  unsigned char* p;
  int rowstride, n_channels;
  unsigned int i, j;
  int k;
  double occ;
  int color_sum;
  double color_avg;

  // Copy the image data into the map structure
  resp->map.info.width = opencvimg->cols;
  resp->map.info.height = opencvimg->rows;
  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 = opencvimg->step;
  n_channels = opencvimg->channels();

  // Copy pixel data into the map structure
  pixels = (unsigned char*)(opencvimg->data);
  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 < n_channels; k++)
        color_sum += *(p + (k));
      color_avg = color_sum / (double)n_channels;

      // 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 (convert ipl to sdl0,0) in the lower-left corner.
      if (occ > occThresh)
        resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = +100;
      else if (occ < freeThresh)
        resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
      else
        resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
    }
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "planner_map_server");

  plannerMapServer ms;

  ros::Rate loopRate(30);
  while (ros::ok())
  {
    ros::spinOnce();
    loopRate.sleep();
  }

  return 0;
}