semantic_world.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
37 #include <ros/ros.h>
38 #include <visualization_msgs/MarkerArray.h>
39 #include <geometry_msgs/Quaternion.h>
40 
41 // MoveIt!
44 #include <moveit_msgs/PlanningScene.h>
45 
46 // OpenCV
47 #include <opencv2/imgproc/imgproc.hpp>
48 
49 // Eigen
51 #include <Eigen/Geometry>
52 
53 namespace moveit
54 {
55 namespace semantic_world
56 {
57 SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene)
58  : planning_scene_(planning_scene)
59 {
61  visualization_publisher_ = node_handle_.advertise<visualization_msgs::MarkerArray>("visualize_place", 20, true);
62  collision_object_publisher_ = node_handle_.advertise<moveit_msgs::CollisionObject>("/collision_object", 20);
63  planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
64 }
65 
66 visualization_msgs::MarkerArray
67 SemanticWorld::getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped>& poses) const
68 {
69  ROS_DEBUG("Visualizing: %d place poses", (int)poses.size());
70  visualization_msgs::MarkerArray marker;
71  for (std::size_t i = 0; i < poses.size(); ++i)
72  {
73  visualization_msgs::Marker m;
74  m.action = m.ADD;
75  m.type = m.SPHERE;
76  m.ns = "place_locations";
77  m.id = i;
78  m.pose = poses[i].pose;
79  m.header = poses[i].header;
80 
81  m.scale.x = 0.02;
82  m.scale.y = 0.02;
83  m.scale.z = 0.02;
84 
85  m.color.r = 1.0;
86  m.color.g = 0.0;
87  m.color.b = 0.0;
88  m.color.a = 1.0;
89  marker.markers.push_back(m);
90  }
91  return marker;
92 }
93 
95 {
96  moveit_msgs::PlanningScene planning_scene;
97  planning_scene.is_diff = true;
98 
99  // Remove the existing tables
100  std::map<std::string, object_recognition_msgs::Table>::iterator it;
101  for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
102  {
103  moveit_msgs::CollisionObject co;
104  co.id = it->first;
105  co.operation = moveit_msgs::CollisionObject::REMOVE;
106  planning_scene.world.collision_objects.push_back(co);
107  // collision_object_publisher_.publish(co);
108  }
109 
110  planning_scene_diff_publisher_.publish(planning_scene);
111  planning_scene.world.collision_objects.clear();
113  // Add the new tables
114  for (std::size_t i = 0; i < table_array_.tables.size(); ++i)
115  {
116  moveit_msgs::CollisionObject co;
117  std::stringstream ss;
118  ss << "table_" << i;
119  co.id = ss.str();
121  co.operation = moveit_msgs::CollisionObject::ADD;
122 
123  const std::vector<geometry_msgs::Point>& convex_hull = table_array_.tables[i].convex_hull;
124 
125  EigenSTL::vector_Vector3d vertices(convex_hull.size());
126  std::vector<unsigned int> triangles((vertices.size() - 2) * 3);
127  for (unsigned int j = 0; j < convex_hull.size(); ++j)
128  vertices[j] = Eigen::Vector3d(convex_hull[j].x, convex_hull[j].y, convex_hull[j].z);
129  for (unsigned int j = 1; j < triangles.size() - 1; ++j)
130  {
131  unsigned int i3 = j * 3;
132  triangles[i3++] = 0;
133  triangles[i3++] = j;
134  triangles[i3] = j + 1;
135  }
136 
137  shapes::Shape* table_shape = shapes::createMeshFromVertices(vertices, triangles);
138  if (!table_shape)
139  continue;
140 
141  shapes::Mesh* table_mesh = static_cast<shapes::Mesh*>(table_shape);
142  shapes::Mesh* table_mesh_solid = orientPlanarPolygon(*table_mesh);
143  if (!table_mesh_solid)
144  {
145  delete table_shape;
146  continue;
147  }
148 
149  shapes::ShapeMsg table_shape_msg;
150  if (!shapes::constructMsgFromShape(table_mesh_solid, table_shape_msg))
151  {
152  delete table_shape;
153  delete table_mesh_solid;
154  continue;
155  }
156 
157  const shape_msgs::Mesh& table_shape_msg_mesh = boost::get<shape_msgs::Mesh>(table_shape_msg);
158 
159  co.meshes.push_back(table_shape_msg_mesh);
160  co.mesh_poses.push_back(table_array_.tables[i].pose);
161  co.header = table_array_.tables[i].header;
162  planning_scene.world.collision_objects.push_back(co);
163  // collision_object_publisher_.publish(co);
164  delete table_shape;
165  delete table_mesh_solid;
166  }
167  planning_scene_diff_publisher_.publish(planning_scene);
168  return true;
169 }
170 
171 object_recognition_msgs::TableArray SemanticWorld::getTablesInROI(double minx, double miny, double minz, double maxx,
172  double maxy, double maxz) const
173 {
174  object_recognition_msgs::TableArray tables_in_roi;
175  std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
176  for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
177  {
178  if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
179  it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
180  it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
181  {
182  tables_in_roi.tables.push_back(it->second);
183  }
184  }
185  return tables_in_roi;
186 }
187 
188 std::vector<std::string> SemanticWorld::getTableNamesInROI(double minx, double miny, double minz, double maxx,
189  double maxy, double maxz) const
190 {
191  std::vector<std::string> result;
192  std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
193  for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
194  {
195  if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
196  it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
197  it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
198  {
199  result.push_back(it->first);
200  }
201  }
202  return result;
203 }
204 
206 {
207  table_array_.tables.clear();
209 }
210 
211 std::vector<geometry_msgs::PoseStamped>
212 SemanticWorld::generatePlacePoses(const std::string& table_name, const shapes::ShapeConstPtr& object_shape,
213  const geometry_msgs::Quaternion& object_orientation, double resolution,
214  double delta_height, unsigned int num_heights) const
215 {
216  object_recognition_msgs::Table chosen_table;
217  std::map<std::string, object_recognition_msgs::Table>::const_iterator it =
218  current_tables_in_collision_world_.find(table_name);
219 
220  if (it != current_tables_in_collision_world_.end())
221  {
222  chosen_table = it->second;
223  return generatePlacePoses(chosen_table, object_shape, object_orientation, resolution, delta_height, num_heights);
224  }
225 
226  std::vector<geometry_msgs::PoseStamped> place_poses;
227  ROS_ERROR("Did not find table %s to place on", table_name.c_str());
228  return place_poses;
229 }
230 
231 std::vector<geometry_msgs::PoseStamped>
232 SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table& chosen_table,
233  const shapes::ShapeConstPtr& object_shape,
234  const geometry_msgs::Quaternion& object_orientation, double resolution,
235  double delta_height, unsigned int num_heights) const
236 {
237  std::vector<geometry_msgs::PoseStamped> place_poses;
238  if (object_shape->type != shapes::MESH && object_shape->type != shapes::SPHERE && object_shape->type != shapes::BOX &&
239  object_shape->type != shapes::CONE)
240  {
241  return place_poses;
242  }
243 
244  double x_min(std::numeric_limits<double>::max()), x_max(-std::numeric_limits<double>::max());
245  double y_min(std::numeric_limits<double>::max()), y_max(-std::numeric_limits<double>::max());
246  double z_min(std::numeric_limits<double>::max()), z_max(-std::numeric_limits<double>::max());
247 
248  Eigen::Quaterniond rotation(object_orientation.x, object_orientation.y, object_orientation.z, object_orientation.w);
249  Eigen::Affine3d object_pose(rotation);
250  double min_distance_from_edge;
251  double height_above_table;
252 
253  if (object_shape->type == shapes::MESH)
254  {
255  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(object_shape.get());
256 
257  for (std::size_t i = 0; i < mesh->vertex_count; ++i)
258  {
259  Eigen::Vector3d position(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
260  position = object_pose * position;
261 
262  if (x_min > position.x())
263  x_min = position.x();
264  if (x_max < position.x())
265  x_max = position.x();
266  if (y_min > position.y())
267  y_min = position.y();
268  if (y_max < position.y())
269  y_max = position.y();
270  if (z_min > position.z())
271  z_min = position.z();
272  if (z_max < position.z())
273  z_max = position.z();
274  }
275  min_distance_from_edge = 0.5 * std::max<double>(fabs(x_max - x_min), fabs(y_max - y_min));
276  height_above_table = -z_min;
277  }
278  else if (object_shape->type == shapes::BOX) // assuming box is being kept down upright
279  {
280  const shapes::Box* box = static_cast<const shapes::Box*>(object_shape.get());
281  min_distance_from_edge = std::max<double>(fabs(box->size[0]), fabs(box->size[1])) / 2.0;
282  height_above_table = fabs(box->size[2]) / 2.0;
283  }
284  else if (object_shape->type == shapes::SPHERE)
285  {
286  const shapes::Sphere* sphere = static_cast<const shapes::Sphere*>(object_shape.get());
287  min_distance_from_edge = sphere->radius;
288  height_above_table = -sphere->radius;
289  }
290  else if (object_shape->type == shapes::CYLINDER) // assuming cylinder is being kept down upright
291  {
292  const shapes::Cylinder* cylinder = static_cast<const shapes::Cylinder*>(object_shape.get());
293  min_distance_from_edge = cylinder->radius;
294  height_above_table = cylinder->length / 2.0;
295  }
296  else if (object_shape->type == shapes::CONE) // assuming cone is being kept down upright
297  {
298  const shapes::Cone* cone = static_cast<const shapes::Cone*>(object_shape.get());
299  min_distance_from_edge = cone->radius;
300  height_above_table = cone->length / 2.0;
301  }
302 
303  return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights,
304  min_distance_from_edge);
305 }
306 
307 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table& table,
308  double resolution, double height_above_table,
309  double delta_height, unsigned int num_heights,
310  double min_distance_from_edge) const
311 {
312  std::vector<geometry_msgs::PoseStamped> place_poses;
313  // Assumption that the table's normal is along the Z axis
314  if (table.convex_hull.empty())
315  return place_poses;
316  const int scale_factor = 100;
317  std::vector<cv::Point2f> table_contour;
318  float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
319  for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
320  {
321  if (table.convex_hull[j].x < x_min)
322  x_min = table.convex_hull[j].x;
323  else if (table.convex_hull[j].x > x_max)
324  x_max = table.convex_hull[j].x;
325  if (table.convex_hull[j].y < y_min)
326  y_min = table.convex_hull[j].y;
327  else if (table.convex_hull[j].y > y_max)
328  y_max = table.convex_hull[j].y;
329  }
330  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
331  table_contour.push_back(
332  cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
333 
334  double x_range = fabs(x_max - x_min);
335  double y_range = fabs(y_max - y_min);
336  int max_range = (int)x_range + 1;
337  if (max_range < (int)y_range + 1)
338  max_range = (int)y_range + 1;
339 
340  int image_scale = std::max<int>(max_range, 4);
341  cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
342 
343  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
344  {
345  cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
346  }
347 
348  unsigned int num_x = fabs(x_max - x_min) / resolution + 1;
349  unsigned int num_y = fabs(y_max - y_min) / resolution + 1;
350 
351  ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y);
352 
353  std::vector<std::vector<cv::Point> > contours;
354  std::vector<cv::Vec4i> hierarchy;
355  cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
356 
357  for (std::size_t j = 0; j < num_x; ++j)
358  {
359  int point_x = j * resolution * scale_factor;
360  for (std::size_t k = 0; k < num_y; ++k)
361  {
362  for (std::size_t mm = 0; mm < num_heights; ++mm)
363  {
364  int point_y = k * resolution * scale_factor;
365  cv::Point2f point2f(point_x, point_y);
366  double result = cv::pointPolygonTest(contours[0], point2f, true);
367  if ((int)result >= (int)(min_distance_from_edge * scale_factor))
368  {
369  Eigen::Vector3d point((double)(point_x) / scale_factor + x_min, (double)(point_y) / scale_factor + y_min,
370  height_above_table + mm * delta_height);
371  Eigen::Affine3d pose;
372  tf::poseMsgToEigen(table.pose, pose);
373  point = pose * point;
374  geometry_msgs::PoseStamped place_pose;
375  place_pose.pose.orientation.w = 1.0;
376  place_pose.pose.position.x = point.x();
377  place_pose.pose.position.y = point.y();
378  place_pose.pose.position.z = point.z();
379  place_pose.header = table.header;
380  place_poses.push_back(place_pose);
381  }
382  }
383  }
384  }
385  return place_poses;
386 }
387 
388 bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const object_recognition_msgs::Table& table,
389  double min_distance_from_edge, double min_vertical_offset) const
390 {
391  // Assumption that the table's normal is along the Z axis
392  if (table.convex_hull.empty())
393  return false;
394  float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
395  for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
396  {
397  if (table.convex_hull[j].x < x_min)
398  x_min = table.convex_hull[j].x;
399  else if (table.convex_hull[j].x > x_max)
400  x_max = table.convex_hull[j].x;
401  if (table.convex_hull[j].y < y_min)
402  y_min = table.convex_hull[j].y;
403  else if (table.convex_hull[j].y > y_max)
404  y_max = table.convex_hull[j].y;
405  }
406  const int scale_factor = 100;
407  std::vector<cv::Point2f> table_contour;
408  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
409  table_contour.push_back(
410  cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
411 
412  double x_range = fabs(x_max - x_min);
413  double y_range = fabs(y_max - y_min);
414  int max_range = (int)x_range + 1;
415  if (max_range < (int)y_range + 1)
416  max_range = (int)y_range + 1;
417 
418  int image_scale = std::max<int>(max_range, 4);
419  cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
420 
421  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
422  {
423  cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
424  }
425 
426  std::vector<std::vector<cv::Point> > contours;
427  std::vector<cv::Vec4i> hierarchy;
428  cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
429 
430  Eigen::Vector3d point(pose.position.x, pose.position.y, pose.position.z);
431  Eigen::Affine3d pose_table;
432  tf::poseMsgToEigen(table.pose, pose_table);
433 
434  // Point in table frame
435  point = pose_table.inverse(Eigen::Isometry) * point;
436  // Assuming Z axis points upwards for the table
437  if (point.z() < -fabs(min_vertical_offset))
438  {
439  ROS_ERROR("Object is not above table");
440  return false;
441  }
442 
443  int point_x = (point.x() - x_min) * scale_factor;
444  int point_y = (point.y() - y_min) * scale_factor;
445  cv::Point2f point2f(point_x, point_y);
446  double result = cv::pointPolygonTest(contours[0], point2f, true);
447  ROS_DEBUG("table distance: %f", result);
448 
449  if ((int)result >= (int)(min_distance_from_edge * scale_factor))
450  return true;
451 
452  return false;
453 }
454 
455 std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, double min_distance_from_edge,
456  double min_vertical_offset) const
457 {
458  std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
459  for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
460  {
461  ROS_DEBUG("Testing table: %s", it->first.c_str());
462  if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset))
463  return it->first;
464  }
465  return std::string();
466 }
467 
468 void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr& msg)
469 {
470  table_array_ = *msg;
471  ROS_INFO("Table callback with %d tables", (int)table_array_.tables.size());
473  // Callback on an update
474  if (table_callback_)
475  {
476  ROS_INFO("Calling table callback");
477  table_callback_();
478  }
479 }
480 
481 void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& table_array) const
482 {
483  for (std::size_t i = 0; i < table_array.tables.size(); ++i)
484  {
485  std::string original_frame = table_array.tables[i].header.frame_id;
486  if (table_array.tables[i].convex_hull.empty())
487  continue;
488  ROS_INFO_STREAM("Original pose: " << table_array.tables[i].pose.position.x << ","
489  << table_array.tables[i].pose.position.y << ","
490  << table_array.tables[i].pose.position.z);
491  std::string error_text;
492  const Eigen::Affine3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame);
493  Eigen::Affine3d original_pose;
494  tf::poseMsgToEigen(table_array.tables[i].pose, original_pose);
495  original_pose = original_transform * original_pose;
496  tf::poseEigenToMsg(original_pose, table_array.tables[i].pose);
497  table_array.tables[i].header.frame_id = planning_scene_->getTransforms().getTargetFrame();
498  ROS_INFO_STREAM("Successfully transformed table array from " << original_frame << "to "
499  << table_array.tables[i].header.frame_id);
500  ROS_INFO_STREAM("Transformed pose: " << table_array.tables[i].pose.position.x << ","
501  << table_array.tables[i].pose.position.y << ","
502  << table_array.tables[i].pose.position.z);
503  }
504 }
505 
507 {
508  if (polygon.vertex_count < 3 || polygon.triangle_count < 1)
509  return 0;
510  // first get the normal of the first triangle of the input polygon
511  Eigen::Vector3d vec1, vec2, vec3, normal;
512 
513  int vIdx1 = polygon.triangles[0];
514  int vIdx2 = polygon.triangles[1];
515  int vIdx3 = polygon.triangles[2];
516  vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
517  vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
518  vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
519  vec2 -= vec1;
520  vec3 -= vec1;
521  normal = vec3.cross(vec2);
522 
523  if (normal[2] < 0.0)
524  normal *= -1.0;
525 
526  normal.normalize();
527 
528  shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count); // + polygon.vertex_count * 2);
529  solid->type = shapes::MESH;
530 
531  // copy the first set of vertices
532  memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
533  // copy the first set of triangles
534  memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
535 
536  for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
537  {
538  int vIdx1 = polygon.triangles[tIdx * 3];
539  int vIdx2 = polygon.triangles[tIdx * 3 + 1];
540  int vIdx3 = polygon.triangles[tIdx * 3 + 2];
541 
542  vec1 =
543  Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
544  vec2 =
545  Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
546  vec3 =
547  Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
548 
549  vec2 -= vec1;
550  vec3 -= vec1;
551 
552  Eigen::Vector3d triangle_normal = vec2.cross(vec1);
553 
554  if (triangle_normal.dot(normal) < 0.0)
555  std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
556  }
557  return solid;
558 }
559 
561 {
562  if (polygon.vertex_count < 3 || polygon.triangle_count < 1 || thickness <= 0)
563  return 0;
564  // first get the normal of the first triangle of the input polygon
565  Eigen::Vector3d vec1, vec2, vec3, normal;
566 
567  int vIdx1 = polygon.triangles[0];
568  int vIdx2 = polygon.triangles[1];
569  int vIdx3 = polygon.triangles[2];
570  vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
571  vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
572  vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
573  vec2 -= vec1;
574  vec3 -= vec1;
575  normal = vec3.cross(vec2);
576 
577  if (normal[2] < 0.0)
578  normal *= -1.0;
579 
580  normal.normalize();
581 
582  // shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count);// + polygon.vertex_count *
583  // 2);
584 
585  shapes::Mesh* solid =
586  new shapes::Mesh(polygon.vertex_count * 2, polygon.triangle_count * 2); // + polygon.vertex_count * 2);
587  solid->type = shapes::MESH;
588 
589  // copy the first set of vertices
590  memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
591  // copy the first set of triangles
592  memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
593 
594  for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
595  {
596  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 0] = solid->triangles[tIdx * 3 + 0] + polygon.vertex_count;
597  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1] = solid->triangles[tIdx * 3 + 1] + polygon.vertex_count;
598  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2] = solid->triangles[tIdx * 3 + 2] + polygon.vertex_count;
599 
600  int vIdx1 = polygon.triangles[tIdx * 3];
601  int vIdx2 = polygon.triangles[tIdx * 3 + 1];
602  int vIdx3 = polygon.triangles[tIdx * 3 + 2];
603 
604  vec1 =
605  Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
606  vec2 =
607  Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
608  vec3 =
609  Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
610 
611  vec2 -= vec1;
612  vec3 -= vec1;
613 
614  Eigen::Vector3d triangle_normal = vec2.cross(vec1);
615 
616  if (triangle_normal.dot(normal) < 0.0)
617  std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
618  else
619  std::swap(solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1],
620  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2]);
621  }
622 
623  for (unsigned vIdx = 0; vIdx < polygon.vertex_count; ++vIdx)
624  {
625  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 0] = solid->vertices[vIdx * 3 + 0] - thickness * normal[0];
626  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 1] = solid->vertices[vIdx * 3 + 1] - thickness * normal[1];
627  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 2] = solid->vertices[vIdx * 3 + 2] - thickness * normal[2];
628  }
629 
630  return solid;
631 }
632 }
633 }
void transformTableArray(object_recognition_msgs::TableArray &table_array) const
planning_scene::PlanningSceneConstPtr planning_scene_
void publish(const boost::shared_ptr< M > &message) const
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
std::map< std::string, object_recognition_msgs::Table > current_tables_in_collision_world_
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
SemanticWorld(const planning_scene::PlanningSceneConstPtr &planning_scene)
A (simple) semantic world representation for pick and place and other tasks. Currently this is used o...
double size[3]
ShapeType type
visualization_msgs::MarkerArray getPlaceLocationsMarker(const std::vector< geometry_msgs::PoseStamped > &poses) const
object_recognition_msgs::TableArray table_array_
void tableCallback(const object_recognition_msgs::TableArrayPtr &msg)
unsigned int * triangles
double y
std::vector< std::string > getTableNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz) const
Get all the tables within a region of interest.
unsigned int vertex_count
Mesh * createMeshFromVertices(const EigenSTL::vector_Vector3d &vertices, const std::vector< unsigned int > &triangles)
unsigned int triangle_count
#define ROS_INFO(...)
shapes::Mesh * createSolidMeshFromPlanarPolygon(const shapes::Mesh &polygon, double thickness) const
bool isInsideTableContour(const geometry_msgs::Pose &pose, const object_recognition_msgs::Table &table, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const
double z
double * vertices
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
#define ROS_INFO_STREAM(args)
std::vector< geometry_msgs::PoseStamped > generatePlacePoses(const std::string &table_name, const shapes::ShapeConstPtr &object_shape, const geometry_msgs::Quaternion &object_orientation, double resolution, double delta_height=0.01, unsigned int num_heights=2) const
Generate possible place poses on the table for a given object. This chooses appropriate values for mi...
object_recognition_msgs::TableArray getTablesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz) const
Get all the tables within a region of interest.
double x
#define ROS_ERROR(...)
shapes::Mesh * orientPlanarPolygon(const shapes::Mesh &polygon) const
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
std::string findObjectTable(const geometry_msgs::Pose &pose, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23