00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00046 #include <Eigen/Core>
00047 #include <Eigen/LU>
00048 #include <Eigen/Geometry>
00049
00050 #include <boost/filesystem.hpp>
00051 #include <boost/thread.hpp>
00052 #include <boost/make_shared.hpp>
00053 #include <ros/ros.h>
00054 #include <ros/callback_queue.h>
00055 #include <nav_msgs/Odometry.h>
00056 #include <tf/tfMessage.h>
00057 #include <roslib/Clock.h>
00058
00059 #include <image_transport/image_transport.h>
00060 #include <image_transport/subscriber_filter.h>
00061 #include <message_filters/subscriber.h>
00062 #include <message_filters/time_synchronizer.h>
00063
00064 #include <household_objects_database/objects_database.h>
00065 #include <household_objects_database/database_original_model.h>
00066 #include <household_objects_database/database_mesh.h>
00067
00068
00069 #include <gazebo/DeleteModel.h>
00070 #include <gazebo/SpawnModel.h>
00071 #include <gazebo/SetModelState.h>
00072
00073 using namespace std;
00074 using namespace boost;
00075 using namespace ros;
00076 using namespace roslib;
00077 using namespace tf;
00078 using namespace sensor_msgs;
00079 using namespace nav_msgs;
00080 using namespace image_transport;
00081 using namespace household_objects_database;
00082
00083 class ObjectDataAcquisitionGazebo
00084 {
00085 private:
00086
00087 NodeHandle nh_;
00088 ServiceClient gazebo_model_remove_serv_, gazebo_model_spawn_serv_, gazebo_camera_set_pose_serv_;
00089 CallbackQueue tf_queue_, clock_queue_;
00090 thread tf_thread_, clock_thread_;
00091 mutex m_mutex_;
00092
00093
00094 shared_ptr<ObjectsDatabase> database_;
00095 std::string model_dae_path_;
00096
00097
00098 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_;
00099
00100
00101 string output_file_;
00102
00103 double scale_factor_;
00104 bool log_file_open_, init_failed_, alternate_mode_, artificial_texture_, alternate_artificial_texture_;
00105
00106
00107 public:
00109 ObjectDataAcquisitionGazebo (int texture, int model_id) : nh_ ("~"),
00110 log_file_open_ (false),
00111 init_failed_ (false), alternate_mode_ (false),
00112 artificial_texture_ (true),
00113 alternate_artificial_texture_ (false)
00114 {
00115
00116 switch (texture)
00117 {
00118 case 0: artificial_texture_ = false; break;
00119 case 1: artificial_texture_ = true; break;
00120 case 2: alternate_artificial_texture_ = true; alternate_mode_ = true; break;
00121 }
00122
00123 nh_.param ("scale_factor", scale_factor_, 1.0);
00124 if (scale_factor_ != 1.0)
00125 ROS_INFO ("Using a scale factor of %f.", scale_factor_);
00126
00127 tf_thread_ = thread (bind (&ObjectDataAcquisitionGazebo::tfThread, this));
00128 clock_thread_ = thread (bind (&ObjectDataAcquisitionGazebo::clockThread, this));
00129
00130
00131 gazebo_model_remove_serv_ = nh_.serviceClient<gazebo::DeleteModel>("/gazebo/delete_model");
00132 gazebo_model_spawn_serv_ = nh_.serviceClient<gazebo::SpawnModel>("/gazebo/spawn_gazebo_model");
00133 gazebo_camera_set_pose_serv_ = nh_.serviceClient<gazebo::SetModelState>("/gazebo/set_model_state");
00134
00135 gazebo_model_remove_serv_.waitForExistence();
00136 gazebo_model_spawn_serv_.waitForExistence();
00137 gazebo_camera_set_pose_serv_.waitForExistence();
00138
00139
00140 database_ = make_shared<ObjectsDatabase> ("wgs36", "5432", "willow", "willow", "household_objects-0.2");
00141 if (!database_->isConnected ())
00142 {
00143 ROS_ERROR ("Failed to connect to database");
00144 init_failed_ = true;
00145 return;
00146 }
00147
00148 household_objects_database::DatabaseOriginalModel db_model;
00149 db_model.id_.get() = model_id;
00150 if (! database_->loadFromDatabase(&db_model.model_) || db_model.model_.get().empty())
00151 {
00152 ROS_ERROR("Model with id %d not found in database", model_id);
00153 init_failed_ = true;
00154 return;
00155 }
00156
00157 household_objects_database::DatabaseMesh mesh;
00158 mesh.id_.data() = model_id;
00159 if ( !database_->loadFromDatabase(&mesh.triangles_) || !database_->loadFromDatabase(&mesh.vertices_) )
00160 {
00161 ROS_ERROR("Failed to load mesh from database for original model %d", mesh.id_.data());
00162 init_failed_ = true;
00163 return;
00164 }
00165
00166 ROS_INFO("Loaded a mesh with %ud vertices and %ud triangles",
00167 (unsigned int)mesh.vertices_.data().size(), (unsigned int)mesh.triangles_.data().size() / 3);
00168
00169
00170 }
00171
00173 void
00174 clockThread ()
00175 {
00176 static const double timeout = 0.01;
00177 while (nh_.ok ())
00178 clock_queue_.callAvailable (WallDuration (timeout));
00179 }
00180
00182 void
00183 tfThread ()
00184 {
00185 static const double timeout = 0.01;
00186 while (nh_.ok ())
00187 tf_queue_.callAvailable (WallDuration (timeout));
00188 }
00189
00191 virtual ~ObjectDataAcquisitionGazebo ()
00192 {
00193
00194 tf_queue_.clear (); tf_queue_.disable ();
00195 clock_queue_.clear (); clock_queue_.disable ();
00196
00197 requestShutdown ();
00198
00199 tf_thread_.join ();
00200 clock_thread_.join ();
00201
00202 if (init_failed_)
00203 return;
00204 }
00205
00207
00211 string
00212 assembleModelGazebo (string model_path, bool artificial_texture)
00213 {
00214 string model =
00215 "<?xml version=\"1.0\" ?>\n"
00216 "<model:physical name=\"object_1\">\n"
00217 "<xyz> 0.0 0.0 0.0</xyz>\n"
00218 "<rpy> 0.0 0.0 0.0</rpy>\n"
00219 "<static>false</static>\n"
00220 "<body:trimesh name=\"object_body\">\n"
00221 " <turnGravityOff>false</turnGravityOff>\n"
00222 " <massMatrix>true</massMatrix>\n"
00223 " <mass>0.01</mass>\n"
00224 " <ixx>0.0001</ixx>\n"
00225 " <ixy>0.0</ixy>\n"
00226 " <ixz>0.0</ixz>\n"
00227 " <iyy>0.0001</iyy>\n"
00228 " <iyz>0.0</iyz>\n"
00229 " <izz>0.0001</izz>\n"
00230 " <cx>0.0</cx>\n"
00231 " <cy>0.0</cy>\n"
00232 " <cz>0.07</cz>\n"
00233 " <geom:trimesh name=\"object_geom\">\n"
00234 " <xyz>0 0 0</xyz>\n"
00235 " <laserRetro>2000.0</laserRetro>\n"
00236 " <kp>100000000.0</kp>\n"
00237 " <kd>1000000.0</kd>\n"
00238 " <scale>0.001 0.001 0.001</scale>\n"
00239 " <mesh>"+ model_path + "</mesh>\n"
00240 " <visual>\n"
00241 " <xyz>0 0 0</xyz>\n"
00242 " <scale>0.001 0.001 0.001</scale>\n";
00243 if (artificial_texture)
00244 model += string (" <material>Gazebo/Grey</material>\n");
00245 model += string (
00246 " <mesh>"+ model_path + "</mesh>\n"
00247 " </visual>\n"
00248 " </geom:trimesh>\n"
00249 "</body:trimesh>\n"
00250 "</model:physical>");
00251 return (model);
00252 }
00253
00255 void
00256 spin ()
00257 {
00258 if (init_failed_) return;
00259
00260
00261 gazebo::DeleteModel dm; dm.request.model_name = "object_1";
00262 gazebo_model_remove_serv_.call (dm);
00263
00264
00265 string gazebo_model = assembleModelGazebo (model_dae_path_, artificial_texture_);
00266
00267
00268 gazebo::SpawnModel sm;
00269 sm.request.model_name = "object_1";
00270 sm.request.model_xml = gazebo_model;
00271 sm.request.robot_namespace = "/";
00272 sm.request.initial_pose.position.x = .7;
00273 sm.request.initial_pose.position.z = 0.55;
00274 gazebo_model_spawn_serv_.call (sm);
00275 ROS_INFO ("wait...");
00276 usleep(1000000);
00277 }
00278 };
00279
00280
00281 int
00282 main (int argc, char** argv)
00283 {
00284 ROS_INFO("Syntax is: %s model_id [artificial_texture = 0/1/2 "
00285 "(0 = disabled, use the real object texture / 1 = enabled* / 2 = alternate mode, save both)] ", argv[0]);
00286
00287
00288 int model_i;
00289 if (argc > 1) model_i = atoi (argv[1]);
00290 else
00291 {
00292 ROS_ERROR("Can not spawn database object: model_id not specified");
00293 return (0);
00294 }
00295
00296 int param = 1;
00297 if (argc > 2) param = atoi (argv[2]);
00298
00299
00300 init (argc, argv, "object_data_acquisition_gazebo");
00301
00302 ObjectDataAcquisitionGazebo p (param, model_i);
00303 p.spin ();
00304
00305 return (0);
00306 }
00307