ed_gui_server
gui_server_plugin.cpp
Go to the documentation of this file.
1 #include "gui_server_plugin.h"
2 
3 #include <ros/console.h>
4 #include <ros/advertise_service_options.h>
5 #include <ros/node_handle.h>
6 
7 #include <ed/world_model.h>
8 #include <ed/entity.h>
9 #include <ed/measurement.h>
10 #include <ed/rendering.h>
11 #include <ed/io/filesystem/write.h>
12 #include <ed/error_context.h>
13 #include <ed/models/shape_loader.h>
14 
15 #include <geolib/datatypes.h>
16 #include <geolib/Shape.h>
17 #include <geolib/CompositeShape.h>
18 #include <geolib/ros/msg_conversions.h>
19 #include <geolib/sensors/DepthCamera.h>
20 
21 #include <rgbd/ros/conversions.h>
22 
23 #include <tue/config/configuration.h>
24 #include <tue/config/loaders/yaml.h>
25 #include <tue/config/reader.h>
26 
27 #include <ed_gui_server_msgs/EntityInfos.h>
28 #include <ed_gui_server_msgs/Mesh.h>
29 
30 #include <boost/filesystem.hpp>
31 
32 #include <opencv2/highgui/highgui.hpp>
33 
34 #include <memory>
35 #include <vector>
36 #include <sstream>
37 
38 void getPersonShape(geo::CompositeShapePtr& composite)
39 {
40  ed::ErrorContext errc("getPersonShape");
41  if (!composite)
42  composite.reset(new geo::CompositeShape);
43  geo::ShapePtr shape(new geo::Shape);
44  ed::models::createCylinder(*shape, 0.25, 1.4, 15);
45  composite->addShape(*shape, geo::Pose3D(0, 0, 0.7));
46  shape.reset(new geo::Shape);
47  ed::models::createSphere(*shape, 0.25);
48  composite->addShape(*shape, geo::Pose3D(0, 0, 1.525));
49 }
50 
58 void minMaxMesh(const geo::Mesh& mesh, const geo::Pose3D& pose, geo::Vec2& p_min, geo::Vec2& p_max)
59 {
60  ed::ErrorContext errc("minMaxMesh");
61  const std::vector<geo::Vector3>& vertices = mesh.getPoints();
62  for(unsigned int i = 0; i < vertices.size(); ++i)
63  {
64  const geo::Vector3& p = pose * vertices[i];
65  p_min.x = std::min(p.x, p_min.x);
66  p_min.y = std::min(p.y, p_min.y);
67 
68  p_max.x = std::max(p.x, p_max.x);
69  p_max.y = std::max(p.y, p_max.y);
70  }
71 }
72 
73 
74 void shapeToMesh(const geo::ShapeConstPtr& shape, ed_gui_server_msgs::Mesh& mesh)
75 {
76  ed::ErrorContext errc("shapeToMesh");
77  const std::vector<geo::Vector3>& vertices = shape->getMesh().getPoints();
78 
79  // Triangles
80  const std::vector<geo::TriangleI>& triangles = shape->getMesh().getTriangleIs();
81  unsigned int current_mesh_size = mesh.vertices.size();
82  mesh.vertices.resize(current_mesh_size + triangles.size() * 9);
83  for(unsigned int i = 0; i < triangles.size(); ++i)
84  {
85  const geo::TriangleI& t = triangles[i];
86  const geo::Vector3& v1 = vertices[t.i1_];
87  const geo::Vector3& v2 = vertices[t.i2_];
88  const geo::Vector3& v3 = vertices[t.i3_];
89 
90  unsigned int i9 = current_mesh_size + i * 9;
91 
92  mesh.vertices[i9] = v1.x;
93  mesh.vertices[i9 + 1] = v1.y;
94  mesh.vertices[i9 + 2] = v1.z;
95  mesh.vertices[i9 + 3] = v2.x;
96  mesh.vertices[i9 + 4] = v2.y;
97  mesh.vertices[i9 + 5] = v2.z;
98  mesh.vertices[i9 + 6] = v3.x;
99  mesh.vertices[i9 + 7] = v3.y;
100  mesh.vertices[i9 + 8] = v3.z;
101  }
102 }
103 
104 void CompositeShapeToMesh(const geo::CompositeShapeConstPtr& composite, ed_gui_server_msgs::Mesh& mesh)
105 {
106  ed::ErrorContext errc("CompositeShapeToMesh");
107  const std::vector<std::pair<geo::ShapePtr, geo::Transform> >& sub_shapes = composite->getShapes();
108 
109  for (std::vector<std::pair<geo::ShapePtr, geo::Transform> >::const_iterator it = sub_shapes.begin();
110  it != sub_shapes.end(); ++it)
111  {
112  geo::ShapePtr shape(new geo::Shape);
113  shape->setMesh(it->first->getMesh().getTransformed(it->second.inverse()));
114  geo::ShapeConstPtr ShapeC = std::const_pointer_cast<geo::Shape>(shape);
115  shapeToMesh(ShapeC, mesh);
116  }
117 }
118 
119 void GUIServerPlugin::entityToMsg(const ed::EntityConstPtr& e, ed_gui_server_msgs::EntityInfo& msg)
120 {
121  ed::ErrorContext errc("entityToMsg");
122 
123  msg.id = e->id().str();
124  msg.type = e->type();
125  msg.existence_probability = e->existenceProbability();
126  msg.visual_revision = e->visualRevision();
127  msg.volumes_revision = e->volumesRevision();
128 
129  if (e->hasType("person") && e->visualRevision() == 0)
130  msg.visual_revision = 1;
131 
132  if (e->has_pose())
133  {
134  geo::convert(e->pose(), msg.pose);
135  msg.has_pose = true;
136  }
137  else
138  {
139  msg.has_pose = false;
140  }
141 
142  if (!e->visual())
143  {
144  const ed::ConvexHull& ch = e->convexHull();
145 
146  if (!ch.points.empty() && e->has_pose())
147  {
148  const geo::Pose3D& pose = e->pose();
149 
150  geo::convert(pose, msg.pose);
151  msg.has_pose = true;
152 
153  msg.polygon.z_min = ch.z_min;
154  msg.polygon.z_max = ch.z_max;
155 
156  unsigned int size = ch.points.size();
157  msg.polygon.xs.resize(size);
158  msg.polygon.ys.resize(size);
159 
160  for(unsigned int i = 0; i < size; ++i)
161  {
162  msg.polygon.xs[i] = ch.points[i].x;
163  msg.polygon.ys[i] = ch.points[i].y;
164  }
165  }
166  }
167 
168  tue::config::Reader config(e->data());
169 
170  if (config.readGroup("color"))
171  {
172  double r, g, b;
173  if (config.value("red", r) && config.value("green", g) && config.value("blue", b))
174  {
175  msg.color.r = 255 * r;
176  msg.color.g = 255 * g;
177  msg.color.b = 255 * b;
178  msg.color.a = 255;
179  }
180  config.endGroup();
181  }
182 
183  if (e->hasFlag("highlighted"))
184  {
185  msg.color.a = 255;
186  msg.color.r = 255;
187  msg.color.g = 0;
188  msg.color.b = 0;
189  }
190 
191  if (e->hasType("person") || e->hasFlag("possible_human"))
192  {
193  msg.color.a = 255;
194  msg.color.r = 255;
195  msg.color.g = 255;
196  msg.color.b = 0;
197  }
198 }
199 
200 
201 
202 // ----------------------------------------------------------------------------------------------------
203 
204 GUIServerPlugin::GUIServerPlugin()
205 {
206  ed::ErrorContext errc("constructor");
207  geo::CompositeShapePtr person_composite;
208  getPersonShape(person_composite);
209  person_shape_ = *person_composite;
210 }
211 
212 // ----------------------------------------------------------------------------------------------------
213 
214 GUIServerPlugin::~GUIServerPlugin()
215 {
216  ed::ErrorContext errc("destructor");
217 }
218 
219 // ----------------------------------------------------------------------------------------------------
220 
221 void GUIServerPlugin::initialize(ed::InitData& init)
222 {
223  ed::ErrorContext errc("initialize");
224 
225  tue::Configuration& config = init.config;
226 
227  std::string robot_name;
228  if (config.value("robot_name", robot_name, tue::config::OPTIONAL))
229  {
230  std::string urdf_rosparam, tf_prefix;
231  config.value("urdf_rosparam", urdf_rosparam);
232  tf_prefix = "";
233  config.value("tf_prefix", tf_prefix, tue::config::OPTIONAL);
234  robot_ = ed::make_shared<gui::Robot>(tf_buffer_);
235  robot_->initialize(robot_name, urdf_rosparam, tf_prefix);
236  }
237 
238  ros::NodeHandle nh("~/gui");
239 
240  ros::AdvertiseServiceOptions opt_srv_entities =
241  ros::AdvertiseServiceOptions::create<ed_gui_server_msgs::QueryEntities>(
242  "query_entities", boost::bind(&GUIServerPlugin::srvQueryEntities, this, _1, _2),
243  ros::VoidPtr(), &cb_queue_);
244 
245  srv_query_entities_ = nh.advertiseService(opt_srv_entities);
246 
247  ros::AdvertiseServiceOptions opt_srv_meshes =
248  ros::AdvertiseServiceOptions::create<ed_gui_server_msgs::QueryMeshes>(
249  "query_meshes", boost::bind(&GUIServerPlugin::srvQueryMeshes, this, _1, _2),
250  ros::VoidPtr(), &cb_queue_);
251 
252  srv_query_meshes_ = nh.advertiseService(opt_srv_meshes);
253 
254  ros::AdvertiseServiceOptions opt_srv_get_entity_info =
255  ros::AdvertiseServiceOptions::create<ed_gui_server_msgs::GetEntityInfo>(
256  "get_entity_info", boost::bind(&GUIServerPlugin::srvGetEntityInfo, this, _1, _2),
257  ros::VoidPtr(), &cb_queue_);
258 
259  srv_get_entity_info_ = nh.advertiseService(opt_srv_get_entity_info);
260 
261  ros::AdvertiseServiceOptions opt_srv_interact =
262  ros::AdvertiseServiceOptions::create<ed_gui_server_msgs::Interact>(
263  "interact", boost::bind(&GUIServerPlugin::srvInteract, this, _1, _2),
264  ros::VoidPtr(), &cb_queue_);
265 
266  srv_interact_ = nh.advertiseService(opt_srv_interact);
267 
268  ros::AdvertiseServiceOptions opt_srv_map =
269  ros::AdvertiseServiceOptions::create<ed_gui_server_msgs::Map>(
270  "map", boost::bind(&GUIServerPlugin::srvMap, this, _1, _2),
271  ros::VoidPtr(), &cb_queue_);
272 
273  srv_map_ = nh.advertiseService(opt_srv_map);
274 
275  pub_entities_ = nh.advertise<ed_gui_server_msgs::EntityInfos>("entities", 1);
276 }
277 
278 // ----------------------------------------------------------------------------------------------------
279 
280 void GUIServerPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& /*req*/)
281 {
282  ed::ErrorContext errc("process");
283 
284  world_model_ = &world;
285  cb_queue_.callAvailable();
286 
287  ed_gui_server_msgs::EntityInfos entities_msg;
288 
289  entities_msg.header.stamp = ros::Time::now();
290  entities_msg.header.frame_id = "map";
291 
292  entities_msg.entities.resize(world_model_->numEntities());
293 
294  unsigned int i = 0;
295  for (ed::WorldModel::const_iterator it = world_model_->begin(); it != world_model_->end(); ++it)
296  {
297  const ed::EntityConstPtr& e = *it;
298  entityToMsg(e, entities_msg.entities[i++]);
299  }
300 
301  if (robot_)
302  robot_->getEntities(entities_msg.entities);
303 
304  pub_entities_.publish(entities_msg);
305 }
306 
307 // ----------------------------------------------------------------------------------------------------
308 
309 bool GUIServerPlugin::srvQueryEntities(const ed_gui_server_msgs::QueryEntities::Request& ros_req,
310  ed_gui_server_msgs::QueryEntities::Response& ros_res)
311 {
312  ed::ErrorContext errc("srvQueryEntities");
313  for(ed::WorldModel::const_iterator it = world_model_->begin(); it != world_model_->end(); ++it)
314  {
315  const ed::EntityConstPtr& e = *it;
316 
317  if (!e->has_pose())
318  continue;
319 
320  const geo::Pose3D& pose = e->pose();
321 
322  float pos_x = pose.t.x;
323  float pos_y = pose.t.y;
324 
325  if (ros_req.area_min.x < pos_x && pos_x < ros_req.area_max.x
326  && ros_req.area_min.y < pos_y && pos_y < ros_req.area_max.y)
327  {
328  ros_res.entities.push_back(ed_gui_server_msgs::EntityInfo());
329  ed_gui_server_msgs::EntityInfo& info = ros_res.entities.back();
330 
331  info.id = e->id().str();
332  info.visual_revision = e->visualRevision();
333  info.volumes_revision = e->volumesRevision();
334  geo::convert(pose, info.pose);
335  }
336  }
337 
338  return true;
339 }
340 
341 // ----------------------------------------------------------------------------------------------------
342 
343 enum ImageCompressionType
344 {
345  IMAGE_COMPRESSION_JPG,
346  IMAGE_COMPRESSION_PNG
347 };
348 
349 bool imageToBinary(const cv::Mat& image, std::vector<unsigned char>& data, ImageCompressionType compression_type)
350 {
351  ed::ErrorContext errc("imageToBinary");
352  if (compression_type == IMAGE_COMPRESSION_JPG)
353  {
354  // OpenCV compression settings
355  std::vector<int> rgb_params;
356  rgb_params.resize(3, 0);
357 
358  rgb_params[0] = cv::IMWRITE_JPEG_QUALITY;
359  rgb_params[1] = 95; // default is 95
360 
361  // Compress image
362  if (!cv::imencode(".jpg", image, data, rgb_params)) {
363  ROS_ERROR_STREAM("[ED Gui Server] RGB image compression failed");
364  return false;
365  }
366  }
367  else if (compression_type == IMAGE_COMPRESSION_PNG)
368  {
369  std::vector<int> params;
370  params.resize(3, 0);
371 
372  params[0] = cv::IMWRITE_PNG_COMPRESSION;
373  params[1] = 1;
374 
375  if (!cv::imencode(".png", image, data, params)) {
376  ROS_ERROR_STREAM("[ED Gui Server] PNG image compression failed");
377  return false;
378  }
379  }
380 
381  return true;
382 }
383 
384 // ----------------------------------------------------------------------------------------------------
385 
386 bool GUIServerPlugin::srvGetEntityInfo(const ed_gui_server_msgs::GetEntityInfo::Request& ros_req,
387  ed_gui_server_msgs::GetEntityInfo::Response& ros_res)
388 {
389  ed::ErrorContext errc("srvGetEntityInfo");
390  ed::EntityConstPtr e = world_model_->getEntity(ros_req.id);
391  if (!e)
392  return true;
393 
394  ros_res.type = e->type();
395 
396  // TODO: get affordances from entity
397  ros_res.affordances.push_back("navigate-to");
398  ros_res.affordances.push_back("pick-up");
399  ros_res.affordances.push_back("place");
400 
401  if (e->has_pose())
402  {
403  const geo::Pose3D& pose = e->pose();
404  std::stringstream ss_pose;
405  ss_pose << "(" << pose.t.x << ", " << pose.t.y << ", " << pose.t.z << ")";
406  ros_res.property_names.push_back("position");
407  ros_res.property_values.push_back(ss_pose.str());
408  }
409 
410 // std::stringstream ss_creationTime;
411 // ss_creationTime << e->creationTime();
412 // ros_res.property_names.push_back("creation time");
413 // ros_res.property_values.push_back(ss_creationTime.str());
414 
415  ed::MeasurementConstPtr m = e->lastMeasurement();
416  if (m)
417  {
418  const cv::Mat& rgb_image = m->image()->getRGBImage();
419  const ed::ImageMask& image_mask = m->imageMask();
420 
421  cv::Mat rgb_image_masked(rgb_image.rows, rgb_image.cols, CV_8UC3, cv::Scalar(0, 0, 0));
422 
423  cv::Point min(rgb_image.cols, rgb_image.rows);
424  cv::Point max(0, 0);
425  for(ed::ImageMask::const_iterator it = image_mask.begin(rgb_image.cols); it != image_mask.end(); ++it)
426  {
427  const cv::Point2i p(it());
428  rgb_image_masked.at<cv::Vec3b>(p) = rgb_image.at<cv::Vec3b>(p);
429 
430  min.x = std::min(min.x, p.x);
431  min.y = std::min(min.y, p.y);
432  max.x = std::max(max.x, p.x);
433  max.y = std::max(max.y, p.y);
434  }
435 
436  imageToBinary(rgb_image_masked, ros_res.measurement_image, IMAGE_COMPRESSION_JPG);
437 
438  // Add border to roi
439  min.x = std::max(0, min.x - ros_req.measurement_image_border);
440  min.y = std::max(0, min.y - ros_req.measurement_image_border);
441  max.x = std::min(rgb_image.cols - 1, max.x + ros_req.measurement_image_border);
442  max.y = std::min(rgb_image.rows - 1, max.y + ros_req.measurement_image_border);
443 
444  imageToBinary(rgb_image(cv::Rect(min, max)), ros_res.measurement_image_unmasked, IMAGE_COMPRESSION_JPG);
445  }
446 
447  return true;
448 }
449 
450 // ----------------------------------------------------------------------------------------------------
451 
452 bool GUIServerPlugin::srvQueryMeshes(const ed_gui_server_msgs::QueryMeshes::Request& ros_req,
453  ed_gui_server_msgs::QueryMeshes::Response& ros_res)
454 {
455  ed::ErrorContext errc("srvQueryMeshes");
456  // Check size of vectors to prevent segmentation faults
457  if (ros_req.entity_ids.size() != ros_req.visual_requests.size())
458  {
459  std::stringstream ss;
460  ss << "Number of requested ids(" << ros_req.entity_ids.size() << ") does not match the number of visual request values (" << ros_req.visual_requests.size() << ")\n";
461  ros_res.error_msg.append(ss.str());
462  ros_res.error_msg.append("\n");
463  ROS_ERROR_STREAM_NAMED("gui_server", ss.rdbuf());
464  }
465  if (ros_req.entity_ids.size() != ros_req.volumes_requests.size())
466  {
467  std::stringstream ss;
468  ss << "Number of requested ids(" << ros_req.entity_ids.size() << ") does not match the number of volumes request values (" << ros_req.volumes_requests.size() << ")";
469  ros_res.error_msg.append(ss.str());
470  ros_res.error_msg.append("\n");
471  ROS_ERROR_STREAM_NAMED("gui_server", ss.rdbuf());
472  }
473  if (!ros_res.error_msg.empty())
474  return true;
475 
476  for (unsigned int i = 0; i < ros_req.entity_ids.size(); ++i)
477  {
478  const std::string& id = ros_req.entity_ids[i];
479  bool visual_needs_update = ros_req.visual_requests[i];
480  bool volumes_needs_update = ros_req.volumes_requests[i];
481 
482  if (!visual_needs_update && !volumes_needs_update)
483  {
484  ROS_WARN_STREAM_NAMED("gui_server", "Got request for enity: '" << id << "', but both visual and volumes request are false");
485  continue;
486  }
487 
488  ed::EntityConstPtr e;
489  e = world_model_->getEntity(id);
490  bool robot_entity = false;
491  if (!e)
492  {
493 
494  if (robot_ && robot_->contains(id))
495  robot_entity = true;
496 
497  if (!robot_entity)
498  {
499  ros_res.error_msg += "Unknown entity: '" + id + "'.\n";
500  continue;
501  }
502  }
503 
504  ros_res.entity_geometries.push_back(ed_gui_server_msgs::EntityMeshAndVolumes());
505  ed_gui_server_msgs::EntityMeshAndVolumes& entity_geometry = ros_res.entity_geometries.back();
506 
507  if (visual_needs_update)
508  {
509  geo::ShapeConstPtr visual;
510  unsigned long visual_revision = 0;
511  if (robot_entity)
512  {
513  visual = robot_->getShape(id);
514  if (visual)
515  visual_revision = 1;
516  else
517  ROS_DEBUG_STREAM_NAMED("gui_server", "No visual from robot entity: '" << id << "'");
518  }
519  else
520  {
521  // If entity is not part of the robot
522  visual = e->visual();
523  visual_revision = e->visualRevision();
524  }
525  if (!visual && e->hasType("person"))
526  {
527  geo::Shape shape_tr;
528  geo::Transform compensate_z = geo::Transform::identity();
529  compensate_z.t.z = -e->pose().t.z;
530  shape_tr.setMesh(person_shape_.getMesh().getTransformed(compensate_z));
531  visual = std::make_shared<const geo::Shape>(shape_tr);
532  visual_revision = 1;
533  }
534 
535  if (visual)
536  {
537  entity_geometry.id = id;
538  entity_geometry.visual_revision = visual_revision;
539  shapeToMesh(visual, entity_geometry.mesh);
540  }
541  else
542  {
543  ROS_DEBUG_STREAM_NAMED("gui_server", "Could not get a visual for entity: '" << id << "'");
544  }
545  }
546 
547  if (volumes_needs_update)
548  {
549  if (robot_entity)
550  {
551  ROS_ERROR_STREAM_NAMED("gui_server", "Can't get volumes of robot entities. Requested for entity: '" << id << "'");
552  continue;
553  }
554 
555  entity_geometry.id = id;
556  entity_geometry.volumes_revision = e->volumesRevision();
557  for (const auto& kv: e->volumes())
558  {
559  if (kv.second)
560  {
561  entity_geometry.volumes.push_back(ed_gui_server_msgs::Volume());
562  ed_gui_server_msgs::Volume& entity_volume = entity_geometry.volumes.back();
563  entity_volume.name = kv.first;
564 
565  const geo::ShapeConstPtr& area_shape = kv.second;
566  shapeToMesh(area_shape, entity_volume.mesh);
567  }
568  }
569  }
570  }
571 
572  return true;
573 }
574 
575 // ----------------------------------------------------------------------------------------------------
576 
577 void GUIServerPlugin::storeMeasurement(const std::string& id, const std::string& type)
578 {
579  ed::ErrorContext errc("storeMeasurement", ("id: '" + id + "', type: '" + type + "'").c_str());
580  ed::EntityConstPtr e = world_model_->getEntity(id);
581  if (e)
582  {
583  char const* home = getenv("HOME");
584  if (home)
585  {
586  boost::filesystem::path dir(std::string(home) + "/.ed/measurements/" + type);
587  boost::filesystem::create_directories(dir);
588 
589  std::string filename = dir.string() + "/" + ed::Entity::generateID().str();
590  ed::write(filename, *e);
591 
592  ROS_INFO_STREAM("Writing entity info to '" << filename << "'.");
593  }
594  }
595  else
596  ROS_ERROR_STREAM("Entity with id " << id << " doesn't exist!");
597 }
598 
599 // ----------------------------------------------------------------------------------------------------
600 
601 bool GUIServerPlugin::srvInteract(const ed_gui_server_msgs::Interact::Request& ros_req,
602  ed_gui_server_msgs::Interact::Response& ros_res)
603 {
604  ed::ErrorContext errc("srvInteract", ros_req.command_yaml.c_str());
605  ROS_DEBUG_STREAM("[ED Gui Server] Received command: " << ros_req.command_yaml);
606 
607  tue::Configuration params;
608  tue::config::loadFromYAMLString(ros_req.command_yaml, params);
609 
610  std::string action;
611  if (params.value("action", action))
612  {
613  if (action == "store")
614  {
615  std::string id, type;
616  if (params.value("id", id) && params.value("type", type))
617  storeMeasurement(id, type);
618  else
619  ROS_ERROR_STREAM("Please specify an id and a type!");
620  }
621  }
622 
623  if (params.hasError())
624  {
625  ros_res.result_json = "{ error: \"" + params.error() + "\" }";
626  return false;
627  }
628  else
629  {
630  ros_res.result_json = "{}";
631  return true;
632  }
633 }
634 
635 // ----------------------------------------------------------------------------------------------------
636 
637 bool GUIServerPlugin::srvMap(const ed_gui_server_msgs::Map::Request& req,
638  ed_gui_server_msgs::Map::Response& res)
639 {
640  ed::ErrorContext errc("srvMap");
641  geo::Vec2 p_min(1e9, 1e9);
642  geo::Vec2 p_max(-1e9, -1e9);
643 
644  bool model_found = false;
645  for (const std::string& model : req.entities_in_view)
646  {
647  const ed::EntityConstPtr e = world_model_->getEntity(model);
648  if (!e || !e->has_pose())
649  {
650  ROS_DEBUG_STREAM_NAMED("srvMap", "Not taking into account entity: " << model);
651  continue;
652  }
653 
654  ROS_DEBUG_STREAM_NAMED("srvMap", "Taking into account entity: " << model);
655 
656  if (e->visual())
657  {
658  minMaxMesh(e->visual()->getBoundingBox().getMesh(), e->pose(), p_min, p_max);
659  model_found = true;
660  }
661  else if (e->hasType("room") && !e->volumes().empty())
662  {
663  for (const auto& v : e->volumes())
664  {
665  minMaxMesh(v.second->getBoundingBox().getMesh(), e->pose(), p_min, p_max);
666  model_found = true;
667  }
668  }
669  else
670  {
671  p_min.x = std::min(e->pose().t.x, p_min.x);
672  p_min.y = std::min(e->pose().t.y, p_min.y);
673 
674  p_max.x = std::max(e->pose().t.x, p_max.x);
675  p_max.y = std::max(e->pose().t.y, p_max.y);
676  }
677  }
678 
679  if (!model_found)
680  {
681  std::stringstream ss;
682  ss << "[";
683  for (const auto& e_id : req.entities_in_view)
684  {
685  ss << e_id << ", ";
686  }
687  ss << "]";
688  ROS_WARN_STREAM_NAMED("srvMap", "Could not find any of the following entities: " << ss.str() << ". All entities will now be taken into account.");
689 
690  for(ed::WorldModel::const_iterator it = world_model_->begin(); it != world_model_->end(); ++it)
691  {
692  const ed::EntityConstPtr& e = *it;
693  const std::string& id = e->id().str();
694 
695  if (e->visual() && e->has_pose() && !e->hasFlag("self") && (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane
696  {
697  minMaxMesh(e->visual()->getBoundingBox().getMesh(), e->pose(), p_min, p_max);
698  }
699  else if (e->hasType("room") && !e->volumes().empty())
700  {
701  for (const auto& v : e->volumes())
702  minMaxMesh(v.second->getBoundingBox().getMesh(), e->pose(), p_min, p_max);
703  }
704  }
705  }
706 
707  // Small padding around the entities in view
708  p_max.x *= (p_max.x>=0) ? 1.01 : 1/1.01;
709  p_max.y *= (p_max.y>=0) ? 1.01 : 1/1.01;
710  p_min.x *= (p_min.x>=0) ? 1/1.01 : 1.01;
711  p_min.y *= (p_min.y>=0) ? 1/1.01 : 1.01;
712 
713  geo::Vec2 range = p_max - p_min;
714  geo::Vec2 center = 0.5*(p_max + p_min);
715  double dist = 1000; // Value doesn't influence the generated img
716 
717  geo::Pose3D cam_pose;
718  cam_pose.t = center.projectTo3d();
719  cam_pose.t.z = dist;
720  cam_pose.R = geo::Matrix3::identity();
721  if (range.y > range.x) // Rotate image to landscape if needed
722  {
723  cam_pose.R.setRPY(0, 0, -M_PI_2);
724  std::swap(range.x, range.y);
725  }
726 
727  uint width = req.image_width ? req.image_width : static_cast<uint>(req.DEFAULT_WIDTH);
728  uint height = req.image_height ? req.image_height : static_cast<uint>(req.DEFAULT_HEIGHT);
729  double focal_length = std::min(width/range.x, height/range.y); // Pixels per meter
730 
731  geo::DepthCamera cam(width, height,
732  focal_length * dist, focal_length * dist,
733  width / 2 + 0.5, height / 2 + 0.5,
734  0, 0);
735 
736  cv::Mat depth_image = cv::Mat(height, width, CV_32FC1, 0.0);
737  cv::Scalar background_color;
738  if (req.background == req.WHITE)
739  background_color = cv::Scalar(255, 255, 255);
740  else if (req.background == req.BLACK)
741  background_color = cv::Scalar(20, 20, 20); // Not completely black
742  else
743  // Default black
744  background_color = cv::Scalar(20, 20, 20); // Not completely black
745  cv::Mat image = cv::Mat(height, width, CV_8UC3, background_color);
746 
747  const geo::Pose3D& cam_pose_inv = cam_pose.inverse();
748  ed::renderWorldModel(*world_model_, ed::ShowVolumes::NoVolumes, cam, cam_pose_inv, depth_image, image, true);
749 
750  if (req.print_labels)
751  {
752  const cv::Scalar text_color = cv::Scalar(255, 255, 255) - background_color;
753  for (ed::WorldModel::const_iterator it = world_model_->begin(); it != world_model_->end(); ++it)
754  {
755  const ed::EntityConstPtr& e = *it;
756  const std::string& id = e->id().str();
757 
758  if (e->visual() && e->has_pose() && !e->hasFlag("self") && (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane
759  {
760  geo::Vector3 center = e->pose().getOrigin();
761  center.z = 0;
762  cv::Point center_cv = cam.project3Dto2D(cam_pose_inv * center);
763  int tmp_baseline;
764  cv::Size textSize = cv::getTextSize(id, cv::FONT_HERSHEY_COMPLEX_SMALL, 1, 1, &tmp_baseline);
765  center_cv.x -= textSize.width/2;
766  center_cv.y += textSize.height/2;
767  cv::putText(image, id, center_cv, cv::FONT_HERSHEY_COMPLEX_SMALL, 1, text_color, 1);
768  }
769  }
770  }
771 
772  rgbd::convert(image, res.map);
773  res.pixels_per_meter_width = focal_length;
774  res.pixels_per_meter_height = focal_length;
775 
776  // Convert from geolib to ROS convention
777  geo::Mat3 rotate180 = geo::Mat3::identity();
778  rotate180.setRPY(M_PI, 0, 0);
779  cam_pose.R = cam_pose.R * rotate180;
780  cam_pose.t.z = 0;
781 
782  geo::convert(cam_pose.R.transpose(), res.pose.pose.orientation);
783 
784  geo::Vec3 cp_to_tl_image(-static_cast<double>(width)/2/focal_length, -static_cast<double>(height)/2/focal_length, 0);
785  geo::Vec3 tl_map = cam_pose * cp_to_tl_image;
786 
787  geo::convert(tl_map, res.pose.pose.position);
788  res.pose.header.frame_id = "map";
789  res.pose.header.stamp = ros::Time::now();
790 
791  return true;
792 }
793 
794 ED_REGISTER_PLUGIN(GUIServerPlugin)
geo::Vector3::y
const real & y() const
geo::Mat3T::identity
static Mat3T identity()
IMAGE_COMPRESSION_PNG
@ IMAGE_COMPRESSION_PNG
Definition: gui_server_plugin.cpp:346
GUIServerPlugin::srvInteract
bool srvInteract(const ed_gui_server_msgs::Interact::Request &ros_req, ed_gui_server_msgs::Interact::Response &ros_res)
Definition: gui_server_plugin.cpp:601
getPersonShape
void getPersonShape(geo::CompositeShapePtr &composite)
Definition: gui_server_plugin.cpp:38
geo::Shape::setMesh
virtual void setMesh(const Mesh &mesh)
datatypes.h
ed::WorldModel::getEntity
EntityConstPtr getEntity(const ed::UUID &id) const
sstream
std::vector::resize
T resize(T... args)
ed::ConvexHull::z_min
float z_min
std::stringstream::rdbuf
T rdbuf(T... args)
ed::ImageMask
ed::UpdateRequest
shapeToMesh
void shapeToMesh(const geo::ShapeConstPtr &shape, ed_gui_server_msgs::Mesh &mesh)
Definition: gui_server_plugin.cpp:74
std::string
std::shared_ptr
geo::Mat3T::transpose
Mat3T transpose() const
GUIServerPlugin::srvGetEntityInfo
bool srvGetEntityInfo(const ed_gui_server_msgs::GetEntityInfo::Request &ros_req, ed_gui_server_msgs::GetEntityInfo::Response &ros_res)
Definition: gui_server_plugin.cpp:386
GUIServerPlugin::person_shape_
geo::Shape person_shape_
Definition: gui_server_plugin.h:47
ed::ConvexHull
IMAGE_COMPRESSION_JPG
@ IMAGE_COMPRESSION_JPG
Definition: gui_server_plugin.cpp:345
GUIServerPlugin
Definition: gui_server_plugin.h:20
DepthCamera.h
t
Timer t
std::pair
geo::Vec2T::y
T y
ED_REGISTER_PLUGIN
#define ED_REGISTER_PLUGIN(Derived)
GUIServerPlugin::storeMeasurement
void storeMeasurement(const std::string &id, const std::string &type)
Definition: gui_server_plugin.cpp:577
vector
std::vector::size
T size(T... args)
GUIServerPlugin::entityToMsg
void entityToMsg(const ed::EntityConstPtr &e, ed_gui_server_msgs::EntityInfo &msg)
Definition: gui_server_plugin.cpp:119
rgb_image
cv::Mat rgb_image
cam
geo::DepthCamera cam
geo::Vec3T
Shape.h
std::stringstream
GUIServerPlugin::pub_entities_
ros::Publisher pub_entities_
Definition: gui_server_plugin.h:41
geo::Vec2T::x
T x
ed::renderWorldModel
bool renderWorldModel(const ed::WorldModel &world_model, const enum ShowVolumes show_volumes, const geo::DepthCamera &cam, const geo::Pose3D &cam_pose_inv, cv::Mat &depth_image, cv::Mat &image, bool flatten=false)
reader.h
geo::Transform3T
ed::InitData
CompositeShapeToMesh
void CompositeShapeToMesh(const geo::CompositeShapeConstPtr &composite, ed_gui_server_msgs::Mesh &mesh)
Definition: gui_server_plugin.cpp:104
ed::InitData::config
tue::Configuration & config
geo::Shape::getMesh
virtual const Mesh & getMesh() const
ed::ErrorContext
ed::WorldModel::begin
const_iterator begin() const
measurement.h
std::shared_ptr::reset
T reset(T... args)
ed::WorldModel::const_iterator
EntityIterator const_iterator
ed::EntityConstPtr
shared_ptr< const Entity > EntityConstPtr
GUIServerPlugin::~GUIServerPlugin
virtual ~GUIServerPlugin()
Definition: gui_server_plugin.cpp:214
ed::WorldModel::end
const_iterator end() const
tue::config::loadFromYAMLString
bool loadFromYAMLString(const std::string &string, ReaderWriter &config, const ResolveConfig &resolve_config=ResolveConfig::defaultConfig())
geo::Vec2T::projectTo3d
Vec3T< T > projectTo3d() const
std::string::push_back
T push_back(T... args)
image
cv::Mat image
geo::Vec3T::y
T y
tue::config::ReaderWriter
geo::Transform3T::inverse
Transform3T inverse() const
geo::TriangleI
gui_server_plugin.h
geo::Mesh::getPoints
const std::vector< geo::Vector3 > & getPoints() const
geo::Mesh::getTransformed
Mesh getTransformed(const geo::Transform t) const
info
std::ostream & info()
shape_loader.h
GUIServerPlugin::srvMap
bool srvMap(const ed_gui_server_msgs::Map::Request &req, ed_gui_server_msgs::Map::Response &rep)
Generate a map based on the entities that need to be in-view.
Definition: gui_server_plugin.cpp:637
tue::config::Reader
ed::Plugin::tf_buffer_
TFBufferConstPtr tf_buffer_
tue::config::ReaderWriter::error
const std::string & error() const
tue::config::ReaderWriter::endGroup
bool endGroup()
GUIServerPlugin::cb_queue_
ros::CallbackQueue cb_queue_
Definition: gui_server_plugin.h:39
GUIServerPlugin::srv_map_
ros::ServiceServer srv_map_
Definition: gui_server_plugin.h:72
write.h
geo::Transform3T::t
Vec3T< T > t
GUIServerPlugin::process
void process(const ed::WorldModel &world, ed::UpdateRequest &req)
Definition: gui_server_plugin.cpp:280
geo::Transform
tue::config::OPTIONAL
OPTIONAL
rendering.h
minMaxMesh
void minMaxMesh(const geo::Mesh &mesh, const geo::Pose3D &pose, geo::Vec2 &p_min, geo::Vec2 &p_max)
Update min/max bounds with a mesh.
Definition: gui_server_plugin.cpp:58
GUIServerPlugin::world_model_
const ed::WorldModel * world_model_
Definition: gui_server_plugin.h:37
CompositeShape.h
req
string req
geo::Vector3
cam_pose
geo::Pose3D cam_pose
yaml.h
ed::WorldModel
geo::Vector3::z
const real & z() const
memory
geo::convert
void convert(const geo::DepthCamera &cam_model, sensor_msgs::CameraInfo &msg)
ed::MeasurementConstPtr
shared_ptr< const Measurement > MeasurementConstPtr
std::swap
T swap(T... args)
std::min
T min(T... args)
geo::Mat3T
ed::Entity::generateID
static UUID generateID()
ed::models::createCylinder
void createCylinder(geo::Shape &shape, double radius, double height, int num_corners=12)
ed::write
bool write(const std::string &filename, const Entity &e)
geo::CompositeShape
configuration.h
b
void b()
tue::config::ReaderWriter::value
bool value(const std::string &name, T &value, RequiredOrOptional opt=REQUIRED)
ed::WorldModel::numEntities
size_t numEntities() const
world_model.h
ed::models::createSphere
void createSphere(geo::Shape &shape, double radius, uint recursion_level=2)
GUIServerPlugin::srv_get_entity_info_
ros::ServiceServer srv_get_entity_info_
Definition: gui_server_plugin.h:57
GUIServerPlugin::srv_query_meshes_
ros::ServiceServer srv_query_meshes_
Definition: gui_server_plugin.h:43
std::vector::begin
T begin(T... args)
geo::DepthCamera::project3Dto2D
cv::Point_< Tout > project3Dto2D(const geo::Vec3T< Tin > &p) const
GUIServerPlugin::srvQueryEntities
bool srvQueryEntities(const ed_gui_server_msgs::QueryEntities::Request &ros_req, ed_gui_server_msgs::QueryEntities::Response &ros_res)
Definition: gui_server_plugin.cpp:309
geo::Transform3T::R
Mat3T< T > R
tue::config::ReaderWriter::readGroup
bool readGroup(const std::string &name, const RequiredOrOptional opt=OPTIONAL)
geo::Vector3::x
const real & x() const
geo::Vec3T::z
T z
geo::DepthCamera
std::stringstream::str
T str(T... args)
conversions.h
std::vector::end
T end(T... args)
ed::ImageMask::begin
const_iterator begin(int width=0) const
std::max
T max(T... args)
GUIServerPlugin::GUIServerPlugin
GUIServerPlugin()
Definition: gui_server_plugin.cpp:204
entity.h
ImageCompressionType
ImageCompressionType
GUIServerPlugin::robot_
ed::shared_ptr< gui::Robot > robot_
Definition: gui_server_plugin.h:45
ed::ConvexHull::z_max
float z_max
GUIServerPlugin::srv_query_entities_
ros::ServiceServer srv_query_entities_
Definition: gui_server_plugin.h:52
ed::Plugin::initialize
virtual void initialize()
ed::ImageMask::end
const_iterator end() const
geo::Mesh
geo::Mat3T::setRPY
void setRPY(T roll, T pitch, T yaw)
rgbd::convert
bool convert(const cv::Mat &image, const geo::DepthCamera &cam_model, sensor_msgs::Image &image_msg, sensor_msgs::CameraInfo &cam_model_msg)
depth_image
cv::Mat depth_image
geo::Vec3T::x
T x
geo::Vec2T
error_context.h
imageToBinary
bool imageToBinary(const cv::Mat &image, std::vector< unsigned char > &data, ImageCompressionType compression_type)
Definition: gui_server_plugin.cpp:349
GUIServerPlugin::srv_interact_
ros::ServiceServer srv_interact_
Definition: gui_server_plugin.h:62
ed::UUID::str
const std::string & str() const
msg_conversions.h
geo::Shape
GUIServerPlugin::srvQueryMeshes
bool srvQueryMeshes(const ed_gui_server_msgs::QueryMeshes::Request &ros_req, ed_gui_server_msgs::QueryMeshes::Response &ros_res)
Definition: gui_server_plugin.cpp:452
config
tue::config::ReaderWriter config
tue::config::ReaderWriter::hasError
bool hasError() const
ed::ConvexHull::points
std::vector< geo::Vec2f > points
Generated on Fri Apr 11 2025 04:38:36 for ed_gui_server by   doxygen 1.8.17