spawn_object_model.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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 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.
 *
 * $Id: acquire_data.cpp 32074 2010-02-25 20:16:36Z rusu $
 *
 */

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>

#include <boost/filesystem.hpp> 
#include <boost/thread.hpp> 
#include <boost/make_shared.hpp> 
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <nav_msgs/Odometry.h>
#include <tf/tfMessage.h>
#include <rosgraph_msgs/Clock.h>
// Image
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
// Interface to model database
#include <household_objects_database/objects_database.h>
#include <household_objects_database/database_original_model.h>
#include <household_objects_database/database_mesh.h>

// Interface to Gazebo
#include <gazebo_msgs/DeleteModel.h>
#include <gazebo_msgs/SpawnModel.h>
#include <gazebo_msgs/SetModelState.h>

using namespace std;
using namespace boost;
using namespace ros;
using namespace rosgraph_msgs;
using namespace tf;
using namespace sensor_msgs;
using namespace nav_msgs;
using namespace image_transport;
using namespace household_objects_database;

class ObjectDataAcquisitionGazebo
{
  private:
    // ROS
    NodeHandle nh_;
    ServiceClient gazebo_model_remove_serv_, gazebo_model_spawn_serv_, gazebo_camera_set_pose_serv_;
    CallbackQueue tf_queue_, clock_queue_;
    thread tf_thread_, clock_thread_;
    mutex m_mutex_;

    // Model database
    shared_ptr<ObjectsDatabase> database_;
    std::string model_dae_path_;

    // Topics
    string t_camera_base_, t_ptu_base_, t_pan_tilt_, t_image_raw_l_, t_image_raw_r_, t_cam_info_l_, t_cam_info_r_;

    // Output BAG file
    string output_file_;

    double scale_factor_;
    bool log_file_open_, init_failed_, alternate_mode_, artificial_texture_, alternate_artificial_texture_;


  public:
    ObjectDataAcquisitionGazebo (int texture, int model_id) : nh_ ("~"),
                                                              log_file_open_ (false), 
                                                              init_failed_ (false), alternate_mode_ (false), 
                                                              artificial_texture_ (true), 
                                                              alternate_artificial_texture_ (false)
    {
      // Select the correct texturing mode
      switch (texture)
      {
        case 0: artificial_texture_ = false; break;
        case 1: artificial_texture_ = true; break;
        case 2: alternate_artificial_texture_ = true; alternate_mode_ = true; break;
      }

      nh_.param ("scale_factor", scale_factor_, 1.0);
      if (scale_factor_ != 1.0)
        ROS_INFO ("Using a scale factor of %f.", scale_factor_);

      tf_thread_    = thread (bind (&ObjectDataAcquisitionGazebo::tfThread, this));
      clock_thread_ = thread (bind (&ObjectDataAcquisitionGazebo::clockThread, this));

      // Gazebo
      gazebo_model_remove_serv_ = nh_.serviceClient<gazebo_msgs::DeleteModel>("/gazebo/delete_model");
      gazebo_model_spawn_serv_  = nh_.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_gazebo_model");
      gazebo_camera_set_pose_serv_ = nh_.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");

      gazebo_model_remove_serv_.waitForExistence();
      gazebo_model_spawn_serv_.waitForExistence();
      gazebo_camera_set_pose_serv_.waitForExistence();

      // Init the model database
      database_ = make_shared<ObjectsDatabase> ("wgs36", "5432", "willow", "willow", "household_objects-0.2");
      if (!database_->isConnected ())
      {
        ROS_ERROR ("Failed to connect to database");
        init_failed_ = true;
        return;
      }
      //load the metadata for the model; not strictly needed but used to check that the model exists
      household_objects_database::DatabaseOriginalModel db_model;
      db_model.id_.get() = model_id;
      if (! database_->loadFromDatabase(&db_model.model_) || db_model.model_.get().empty())
      {
        ROS_ERROR("Model with id %d not found in database", model_id);
        init_failed_ = true;
        return;
      }
      //load the geometry for the model
      household_objects_database::DatabaseMesh mesh;
      mesh.id_.data() = model_id;
      if ( !database_->loadFromDatabase(&mesh.triangles_) || !database_->loadFromDatabase(&mesh.vertices_) )
      {
        ROS_ERROR("Failed to load mesh from database for original model %d", mesh.id_.data());
        init_failed_ = true;
        return;
      }
      //we are done
      ROS_INFO("Loaded a mesh with %ud vertices and %ud triangles", 
               (unsigned int)mesh.vertices_.data().size(), (unsigned int)mesh.triangles_.data().size() / 3);
      // mesh.vertices_.data()  is a std::vector<double> containing the vertices
      // mesh.triangles_.data() is a std::vector<int> containing the triangles
    }
    
    void
      clockThread ()
    {
      static const double timeout = 0.01;
      while (nh_.ok ())
        clock_queue_.callAvailable (WallDuration (timeout));
    }

    void
      tfThread ()
    {
      static const double timeout = 0.01;
      while (nh_.ok ())
        tf_queue_.callAvailable (WallDuration (timeout));
    }

    virtual ~ObjectDataAcquisitionGazebo ()
    {
      // Clear and disable the queues
      tf_queue_.clear ();    tf_queue_.disable ();
      clock_queue_.clear (); clock_queue_.disable ();
      // Shut down ROS
      requestShutdown ();
      // Join the threads
      tf_thread_.join ();
      clock_thread_.join ();

      if (init_failed_)
        return;
    }


    string
      assembleModelGazebo (string model_path, bool artificial_texture)
    {
      string model = 
        "<?xml version=\"1.0\" ?>\n"
         "<model:physical name=\"object_1\">\n"
         "<xyz>   0.0    0.0    0.0</xyz>\n"
         "<rpy>   0.0    0.0    0.0</rpy>\n"
         "<static>false</static>\n"
         "<body:trimesh name=\"object_body\">\n"
         "  <turnGravityOff>false</turnGravityOff>\n"
         "  <massMatrix>true</massMatrix>\n"
         "  <mass>0.01</mass>\n"
         "  <ixx>0.0001</ixx>\n"
         "  <ixy>0.0</ixy>\n"
         "  <ixz>0.0</ixz>\n"
         "  <iyy>0.0001</iyy>\n"
         "  <iyz>0.0</iyz>\n"
         "  <izz>0.0001</izz>\n"
         "  <cx>0.0</cx>\n"
         "  <cy>0.0</cy>\n"
         "  <cz>0.07</cz>\n"
         "  <geom:trimesh name=\"object_geom\">\n"
         "    <xyz>0 0 0</xyz>\n"
         "    <laserRetro>2000.0</laserRetro>\n"
         "    <kp>100000000.0</kp>\n"
         "    <kd>1000000.0</kd>\n"
         "    <scale>0.001 0.001 0.001</scale>\n"
         "    <mesh>"+ model_path + "</mesh>\n"
         "    <visual>\n"
         "      <xyz>0 0 0</xyz>\n"
         "      <scale>0.001 0.001 0.001</scale>\n";
      if (artificial_texture)
        model += string ("      <material>Gazebo/Grey</material>\n");
      model += string (
         "      <mesh>"+ model_path + "</mesh>\n"
         "    </visual>\n"
         "  </geom:trimesh>\n"
         "</body:trimesh>\n"
       "</model:physical>");
      return (model);
    }

    void
      spin ()
    {
      if (init_failed_) return;

      // Call the Gazebo service request to remove the existing model (if any)
      gazebo_msgs::DeleteModel dm; dm.request.model_name = "object_1";
      gazebo_model_remove_serv_.call (dm);
      
      // Assemble the model to be sent to Gazebo
      string gazebo_model = assembleModelGazebo (model_dae_path_, artificial_texture_);
      
      // Call the Gazebo service request to add the current model
      gazebo_msgs::SpawnModel sm; 
      sm.request.model_name = "object_1"; 
      sm.request.model_xml = gazebo_model; 
      sm.request.robot_namespace = "/";
      sm.request.initial_pose.position.x = .7; // center of table is shifted x by 1
      sm.request.initial_pose.position.z = 0.55; // table top level
      gazebo_model_spawn_serv_.call (sm);
      ROS_INFO ("wait...");
      usleep(1000000);
    }
};

/* ---[ */
int
  main (int argc, char** argv)
{
  ROS_INFO("Syntax is: %s model_id [artificial_texture = 0/1/2 "
           "(0 = disabled, use the real object texture / 1 = enabled* / 2 = alternate mode, save both)] ", argv[0]);

  
  int model_i;
  if (argc > 1) model_i = atoi (argv[1]);
  else 
  {
    ROS_ERROR("Can not spawn database object: model_id not specified");
    return (0);
  }

  int param = 1;
  if (argc > 2) param = atoi (argv[2]);

  // ROS init
  init (argc, argv, "object_data_acquisition_gazebo");

  ObjectDataAcquisitionGazebo p (param, model_i);
  p.spin ();

  return (0);
}