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> SemanticWorld::generatePlacePoses(
232  const object_recognition_msgs::Table& chosen_table, const shapes::ShapeConstPtr& object_shape,
233  const geometry_msgs::Quaternion& object_orientation, double resolution, double delta_height,
234  unsigned int num_heights) const
235 {
236  std::vector<geometry_msgs::PoseStamped> place_poses;
237  if (object_shape->type != shapes::MESH && object_shape->type != shapes::SPHERE && object_shape->type != shapes::BOX &&
238  object_shape->type != shapes::CONE)
239  {
240  return place_poses;
241  }
242 
243  double x_min(std::numeric_limits<double>::max()), x_max(-std::numeric_limits<double>::max());
244  double y_min(std::numeric_limits<double>::max()), y_max(-std::numeric_limits<double>::max());
245  double z_min(std::numeric_limits<double>::max()), z_max(-std::numeric_limits<double>::max());
246 
247  Eigen::Quaterniond rotation(object_orientation.x, object_orientation.y, object_orientation.z, object_orientation.w);
248  Eigen::Affine3d object_pose(rotation);
249  double min_distance_from_edge;
250  double height_above_table;
251 
252  if (object_shape->type == shapes::MESH)
253  {
254  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(object_shape.get());
255 
256  for (std::size_t i = 0; i < mesh->vertex_count; ++i)
257  {
258  Eigen::Vector3d position(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
259  position = object_pose * position;
260 
261  if (x_min > position.x())
262  x_min = position.x();
263  if (x_max < position.x())
264  x_max = position.x();
265  if (y_min > position.y())
266  y_min = position.y();
267  if (y_max < position.y())
268  y_max = position.y();
269  if (z_min > position.z())
270  z_min = position.z();
271  if (z_max < position.z())
272  z_max = position.z();
273  }
274  min_distance_from_edge = 0.5 * std::max<double>(fabs(x_max - x_min), fabs(y_max - y_min));
275  height_above_table = -z_min;
276  }
277  else if (object_shape->type == shapes::BOX) // assuming box is being kept down upright
278  {
279  const shapes::Box* box = static_cast<const shapes::Box*>(object_shape.get());
280  min_distance_from_edge = std::max<double>(fabs(box->size[0]), fabs(box->size[1])) / 2.0;
281  height_above_table = fabs(box->size[2]) / 2.0;
282  }
283  else if (object_shape->type == shapes::SPHERE)
284  {
285  const shapes::Sphere* sphere = static_cast<const shapes::Sphere*>(object_shape.get());
286  min_distance_from_edge = sphere->radius;
287  height_above_table = -sphere->radius;
288  }
289  else if (object_shape->type == shapes::CYLINDER) // assuming cylinder is being kept down upright
290  {
291  const shapes::Cylinder* cylinder = static_cast<const shapes::Cylinder*>(object_shape.get());
292  min_distance_from_edge = cylinder->radius;
293  height_above_table = cylinder->length / 2.0;
294  }
295  else if (object_shape->type == shapes::CONE) // assuming cone is being kept down upright
296  {
297  const shapes::Cone* cone = static_cast<const shapes::Cone*>(object_shape.get());
298  min_distance_from_edge = cone->radius;
299  height_above_table = cone->length / 2.0;
300  }
301 
302  return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights,
303  min_distance_from_edge);
304 }
305 
306 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table& table,
307  double resolution, double height_above_table,
308  double delta_height, unsigned int num_heights,
309  double min_distance_from_edge) const
310 {
311  std::vector<geometry_msgs::PoseStamped> place_poses;
312  // Assumption that the table's normal is along the Z axis
313  if (table.convex_hull.empty())
314  return place_poses;
315  const int scale_factor = 100;
316  std::vector<cv::Point2f> table_contour;
317  float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
318  for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
319  {
320  if (table.convex_hull[j].x < x_min)
321  x_min = table.convex_hull[j].x;
322  else if (table.convex_hull[j].x > x_max)
323  x_max = table.convex_hull[j].x;
324  if (table.convex_hull[j].y < y_min)
325  y_min = table.convex_hull[j].y;
326  else if (table.convex_hull[j].y > y_max)
327  y_max = table.convex_hull[j].y;
328  }
329  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
330  table_contour.push_back(
331  cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
332 
333  double x_range = fabs(x_max - x_min);
334  double y_range = fabs(y_max - y_min);
335  int max_range = (int)x_range + 1;
336  if (max_range < (int)y_range + 1)
337  max_range = (int)y_range + 1;
338 
339  int image_scale = std::max<int>(max_range, 4);
340  cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
341 
342  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
343  {
344  cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
345  }
346 
347  unsigned int num_x = fabs(x_max - x_min) / resolution + 1;
348  unsigned int num_y = fabs(y_max - y_min) / resolution + 1;
349 
350  ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y);
351 
352  std::vector<std::vector<cv::Point> > contours;
353  std::vector<cv::Vec4i> hierarchy;
354  cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
355 
356  for (std::size_t j = 0; j < num_x; ++j)
357  {
358  int point_x = j * resolution * scale_factor;
359  for (std::size_t k = 0; k < num_y; ++k)
360  {
361  for (std::size_t mm = 0; mm < num_heights; ++mm)
362  {
363  int point_y = k * resolution * scale_factor;
364  cv::Point2f point2f(point_x, point_y);
365  double result = cv::pointPolygonTest(contours[0], point2f, true);
366  if ((int)result >= (int)(min_distance_from_edge * scale_factor))
367  {
368  Eigen::Vector3d point((double)(point_x) / scale_factor + x_min, (double)(point_y) / scale_factor + y_min,
369  height_above_table + mm * delta_height);
370  Eigen::Affine3d pose;
371  tf::poseMsgToEigen(table.pose, pose);
372  point = pose * point;
373  geometry_msgs::PoseStamped place_pose;
374  place_pose.pose.orientation.w = 1.0;
375  place_pose.pose.position.x = point.x();
376  place_pose.pose.position.y = point.y();
377  place_pose.pose.position.z = point.z();
378  place_pose.header = table.header;
379  place_poses.push_back(place_pose);
380  }
381  }
382  }
383  }
384  return place_poses;
385 }
386 
387 bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const object_recognition_msgs::Table& table,
388  double min_distance_from_edge, double min_vertical_offset) const
389 {
390  // Assumption that the table's normal is along the Z axis
391  if (table.convex_hull.empty())
392  return false;
393  float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
394  for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
395  {
396  if (table.convex_hull[j].x < x_min)
397  x_min = table.convex_hull[j].x;
398  else if (table.convex_hull[j].x > x_max)
399  x_max = table.convex_hull[j].x;
400  if (table.convex_hull[j].y < y_min)
401  y_min = table.convex_hull[j].y;
402  else if (table.convex_hull[j].y > y_max)
403  y_max = table.convex_hull[j].y;
404  }
405  const int scale_factor = 100;
406  std::vector<cv::Point2f> table_contour;
407  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
408  table_contour.push_back(
409  cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
410 
411  double x_range = fabs(x_max - x_min);
412  double y_range = fabs(y_max - y_min);
413  int max_range = (int)x_range + 1;
414  if (max_range < (int)y_range + 1)
415  max_range = (int)y_range + 1;
416 
417  int image_scale = std::max<int>(max_range, 4);
418  cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
419 
420  for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
421  {
422  cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
423  }
424 
425  std::vector<std::vector<cv::Point> > contours;
426  std::vector<cv::Vec4i> hierarchy;
427  cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
428 
429  Eigen::Vector3d point(pose.position.x, pose.position.y, pose.position.z);
430  Eigen::Affine3d pose_table;
431  tf::poseMsgToEigen(table.pose, pose_table);
432 
433  // Point in table frame
434  point = pose_table.inverse() * point;
435  // Assuming Z axis points upwards for the table
436  if (point.z() < -fabs(min_vertical_offset))
437  {
438  ROS_ERROR("Object is not above table");
439  return false;
440  }
441 
442  int point_x = (point.x() - x_min) * scale_factor;
443  int point_y = (point.y() - y_min) * scale_factor;
444  cv::Point2f point2f(point_x, point_y);
445  double result = cv::pointPolygonTest(contours[0], point2f, true);
446  ROS_DEBUG("table distance: %f", result);
447 
448  if ((int)result >= (int)(min_distance_from_edge * scale_factor))
449  return true;
450 
451  return false;
452 }
453 
454 std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, double min_distance_from_edge,
455  double min_vertical_offset) const
456 {
457  std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
458  for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
459  {
460  ROS_DEBUG("Testing table: %s", it->first.c_str());
461  if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset))
462  return it->first;
463  }
464  return std::string();
465 }
466 
467 void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr& msg)
468 {
469  table_array_ = *msg;
470  ROS_INFO("Table callback with %d tables", (int)table_array_.tables.size());
472  // Callback on an update
473  if (table_callback_)
474  {
475  ROS_INFO("Calling table callback");
476  table_callback_();
477  }
478 }
479 
480 void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& table_array) const
481 {
482  for (std::size_t i = 0; i < table_array.tables.size(); ++i)
483  {
484  std::string original_frame = table_array.tables[i].header.frame_id;
485  if (table_array.tables[i].convex_hull.empty())
486  continue;
487  ROS_INFO_STREAM("Original pose: " << table_array.tables[i].pose.position.x << ","
488  << table_array.tables[i].pose.position.y << ","
489  << table_array.tables[i].pose.position.z);
490  std::string error_text;
491  const Eigen::Affine3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame);
492  Eigen::Affine3d original_pose;
493  tf::poseMsgToEigen(table_array.tables[i].pose, original_pose);
494  original_pose = original_transform * original_pose;
495  tf::poseEigenToMsg(original_pose, table_array.tables[i].pose);
496  table_array.tables[i].header.frame_id = planning_scene_->getTransforms().getTargetFrame();
497  ROS_INFO_STREAM("Successfully transformed table array from " << original_frame << "to "
498  << table_array.tables[i].header.frame_id);
499  ROS_INFO_STREAM("Transformed pose: " << table_array.tables[i].pose.position.x << ","
500  << table_array.tables[i].pose.position.y << ","
501  << table_array.tables[i].pose.position.z);
502  }
503 }
504 
506 {
507  if (polygon.vertex_count < 3 || polygon.triangle_count < 1)
508  return 0;
509  // first get the normal of the first triangle of the input polygon
510  Eigen::Vector3d vec1, vec2, vec3, normal;
511 
512  int vIdx1 = polygon.triangles[0];
513  int vIdx2 = polygon.triangles[1];
514  int vIdx3 = polygon.triangles[2];
515  vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
516  vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
517  vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
518  vec2 -= vec1;
519  vec3 -= vec1;
520  normal = vec3.cross(vec2);
521 
522  if (normal[2] < 0.0)
523  normal *= -1.0;
524 
525  normal.normalize();
526 
527  shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count); // + polygon.vertex_count * 2);
528  solid->type = shapes::MESH;
529 
530  // copy the first set of vertices
531  memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
532  // copy the first set of triangles
533  memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
534 
535  for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
536  {
537  int vIdx1 = polygon.triangles[tIdx * 3];
538  int vIdx2 = polygon.triangles[tIdx * 3 + 1];
539  int vIdx3 = polygon.triangles[tIdx * 3 + 2];
540 
541  vec1 =
542  Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
543  vec2 =
544  Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
545  vec3 =
546  Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
547 
548  vec2 -= vec1;
549  vec3 -= vec1;
550 
551  Eigen::Vector3d triangle_normal = vec2.cross(vec1);
552 
553  if (triangle_normal.dot(normal) < 0.0)
554  std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
555  }
556  return solid;
557 }
558 
560 {
561  if (polygon.vertex_count < 3 || polygon.triangle_count < 1 || thickness <= 0)
562  return 0;
563  // first get the normal of the first triangle of the input polygon
564  Eigen::Vector3d vec1, vec2, vec3, normal;
565 
566  int vIdx1 = polygon.triangles[0];
567  int vIdx2 = polygon.triangles[1];
568  int vIdx3 = polygon.triangles[2];
569  vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
570  vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
571  vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
572  vec2 -= vec1;
573  vec3 -= vec1;
574  normal = vec3.cross(vec2);
575 
576  if (normal[2] < 0.0)
577  normal *= -1.0;
578 
579  normal.normalize();
580 
581  // shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count);// + polygon.vertex_count *
582  // 2);
583 
584  shapes::Mesh* solid =
585  new shapes::Mesh(polygon.vertex_count * 2, polygon.triangle_count * 2); // + polygon.vertex_count * 2);
586  solid->type = shapes::MESH;
587 
588  // copy the first set of vertices
589  memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
590  // copy the first set of triangles
591  memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
592 
593  for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
594  {
595  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 0] = solid->triangles[tIdx * 3 + 0] + polygon.vertex_count;
596  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1] = solid->triangles[tIdx * 3 + 1] + polygon.vertex_count;
597  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2] = solid->triangles[tIdx * 3 + 2] + polygon.vertex_count;
598 
599  int vIdx1 = polygon.triangles[tIdx * 3];
600  int vIdx2 = polygon.triangles[tIdx * 3 + 1];
601  int vIdx3 = polygon.triangles[tIdx * 3 + 2];
602 
603  vec1 =
604  Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
605  vec2 =
606  Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
607  vec3 =
608  Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
609 
610  vec2 -= vec1;
611  vec3 -= vec1;
612 
613  Eigen::Vector3d triangle_normal = vec2.cross(vec1);
614 
615  if (triangle_normal.dot(normal) < 0.0)
616  std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
617  else
618  std::swap(solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1],
619  solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2]);
620  }
621 
622  for (unsigned vIdx = 0; vIdx < polygon.vertex_count; ++vIdx)
623  {
624  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 0] = solid->vertices[vIdx * 3 + 0] - thickness * normal[0];
625  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 1] = solid->vertices[vIdx * 3 + 1] - thickness * normal[1];
626  solid->vertices[(vIdx + polygon.vertex_count) * 3 + 2] = solid->vertices[vIdx * 3 + 2] - thickness * normal[2];
627  }
628 
629  return solid;
630 }
631 }
632 }
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 Wed Jul 18 2018 02:48:55