map.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, 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 "rve_render_client/mesh.h"
#include "rve_render_client/mesh_instance.h"
#include "rve_render_client/texture.h"
#include "rve_render_client/box.h"

#include <rve_common_transformers/map.h>
#include <rve_transformer/transformer_manager.h>
#include <rve_transformer/frame_manager.h>
#include <rve_properties/property_node.h>
#include <rve_properties/messages.h>

#include <tf2/exceptions.h>

#include <rve_msgs/make_vector3.h>
#include <rve_msgs/make_quaternion.h>
#include <rve_msgs/Mesh.h>
#include <rve_common/eigen_conversions.h>
#include <geometry_msgs/TransformStamped.h>

namespace rve_common_transformers
{
  
Map::Map()
{
}

Map::~Map()
{
  getPropertyNode()->removeChangeCallback(this);
}

void Map::onInit()
{
  //getPropertyNode()->addChangeCallback(boost::bind(&Map::onPropertyChanged, this, _1, _2, _3), this, getCallbackQueue());

  ros::NodeHandle nh;
  map_sub_ = nh.subscribe("/map", 1, &Map::receiveOccupancyGrid, this);
}
  
void Map::makeMesh( sensor_msgs::ImagePtr &image )
{
  ROS_INFO( "makeMesh()" );
  map_texture_ = rve_render_client::createTexture();
  map_texture_->setFromImage( image );
  addObject(map_texture_);

  rve_msgs::MeshPtr mesh(new rve_msgs::Mesh);
  mesh->submeshes.resize(1);

  rve_msgs::SubMesh& sm = mesh->submeshes[0];
  sm.positions.push_back(rve_msgs::makeVector3(0, map_height_meters_, 0));
  sm.positions.push_back(rve_msgs::makeVector3(map_width_meters_, map_height_meters_, 0));
  sm.positions.push_back(rve_msgs::makeVector3(0, 0, 0));
  sm.positions.push_back(rve_msgs::makeVector3(map_width_meters_, 0, 0));
  sm.indices.push_back(0);
  sm.indices.push_back(1);
  sm.indices.push_back(2);
  sm.indices.push_back(2);
  sm.indices.push_back(1);
  sm.indices.push_back(3);

  sm.normals.push_back(rve_msgs::makeVector3(0.0, 0.0, 1.0));
  sm.normals.push_back(rve_msgs::makeVector3(0.0, 0.0, 1.0));
  sm.normals.push_back(rve_msgs::makeVector3(0.0, 0.0, 1.0));
  sm.normals.push_back(rve_msgs::makeVector3(0.0, 0.0, 1.0));

  sm.tex_coords.resize(1);
  rve_msgs::SubMesh::_tex_coords_type& tc = sm.tex_coords;
  tc[0].dims = 2;
  tc[0].array.resize(4);
  tc[0].array[0].uvw[0] = 0.0;
  tc[0].array[0].uvw[1] = 1.0;

  tc[0].array[1].uvw[0] = 1.0;
  tc[0].array[1].uvw[1] = 1.0;

  tc[0].array[2].uvw[0] = 0.0;
  tc[0].array[2].uvw[1] = 0.0;

  tc[0].array[3].uvw[0] = 1.0;
  tc[0].array[3].uvw[1] = 0.0;

  sm.material_index = 0;

  mesh->materials.resize(1);
  rve_msgs::Material& mat = mesh->materials[0];
  mat.id = rve_common::UUID::generate();
  mat.opacity = 1.0;
  mat.has_texture = true;
  mat.texture = "texture://" + map_texture_->getID().toString();
  mat.disable_shading = true;
  mat.cull_mode = rve_msgs::Material::CULL_NONE;

  rve_render_client::MeshPtr m = rve_render_client::createMesh(mesh);
  mesh_instance_ = rve_render_client::createMeshInstance(m);
  mesh_instance_->setPosition(position_);
  mesh_instance_->setOrientation(orientation_);
  ROS_INFO("map mesh instance scene object id: %s", mesh_instance_->getID().toString().c_str());
  printf("map mesh instance scene object pointer: %p\n", mesh_instance_.get());
  addObject(mesh_instance_);
  addObject(m);
}

void Map::receiveOccupancyGrid(const nav_msgs::OccupancyGridConstPtr &msg)
{
#if 0
  if (!validateFloats(*msg))
  {
    ROS_ERROR("Message contained invalid floating point values (nans or infs)");
    return;
  }
#endif
  if (msg->info.width * msg->info.height == 0)
  {
    ROS_ERROR("Map is zero-sized (%dx%d)", msg->info.width, msg->info.height);
    return;
  }

//  clear();

  ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
           msg->info.width,
           msg->info.height,
           msg->info.resolution);

  // TODO: check if resolution or position or orientation etc have
  // changed, and call makeMesh() again if so
  map_ = msg;
  position_.x = msg->info.origin.position.x;
  position_.y = msg->info.origin.position.y;
  position_.z = msg->info.origin.position.z;
  orientation_.w = msg->info.origin.orientation.w;
  orientation_.x = msg->info.origin.orientation.x;
  orientation_.y = msg->info.origin.orientation.y;
  orientation_.z = msg->info.origin.orientation.z;
  map_width_pixels_ = msg->info.width;
  map_height_pixels_ = msg->info.height;
  map_width_meters_ = map_width_pixels_ * msg->info.resolution;
  map_height_meters_ = map_height_pixels_ * msg->info.resolution;
  resolution_ = msg->info.resolution;
  frame_ = msg->header.frame_id;
  if (frame_.empty())
  {
    frame_ = "/map";
  }

  // Convert to image.
  int pixels_size = map_width_pixels_ * map_height_pixels_;
  sensor_msgs::Image::_data_type pixels( pixels_size, 255 );

  for(unsigned int j=0;j<msg->info.height;j++)
  {
    for(unsigned int i=0;i<msg->info.width;i++)
    {
      unsigned char val;
      if(msg->data[j*msg->info.width+i] == 100)
      {
        val = 0;
      }
      else if(msg->data[j*msg->info.width+i] == 0)
        val = 255;
      else
        val = 127;

      int pidx = (j*map_width_pixels_ + i);
      pixels[pidx] = val;
    }
  }

  map_image_ = sensor_msgs::ImagePtr( new sensor_msgs::Image() );
  map_image_->data = pixels;
  map_image_->height = map_height_pixels_;
  map_image_->width = map_width_pixels_;
  map_image_->step = map_width_pixels_;
  map_image_->encoding = "mono8";
  
  if( !map_texture_ ) {
    makeMesh( map_image_ );
  }
  map_texture_->setFromImage( map_image_ );
}

void Map::onUpdate()
{
  if(mesh_instance_)
  {
    try
    {
      geometry_msgs::TransformStamped transform = getManager()->getFrameManager()->lookupTransform(frame_, ros::Time());
      const geometry_msgs::Vector3& v = transform.transform.translation;
      const geometry_msgs::Quaternion& q = transform.transform.rotation;
      position_.x = v.x;
      position_.y = v.z;
      position_.z = v.z;
      orientation_.x = q.x;
      orientation_.y = q.y;
      orientation_.z = q.z;
      orientation_.w = q.w;
      mesh_instance_->setPosition(position_);
      mesh_instance_->setOrientation(orientation_);
    }
    catch (tf2::TransformException& e)
    {
    }
  }
}

void Map::onPropertyChanged(const rve_properties::PropertyNodePtr& node, const std::string& path, rve_properties::PropertyChangeType type)
{
  if (type != rve_properties::PropertyChanged)
  {
    return;
  }
}

} // namespace rve_common_transformers