38 #include <visualization_msgs/MarkerArray.h>
39 #include <geometry_msgs/Quaternion.h>
44 #include <moveit_msgs/PlanningScene.h>
47 #include <opencv2/imgproc/imgproc.hpp>
51 #include <Eigen/Geometry>
55 namespace semantic_world
57 static const std::string
LOGNAME =
"semantic_world";
62 table_subscriber_ = node_handle_.subscribe(
"table_array", 1, &SemanticWorld::tableCallback,
this);
63 visualization_publisher_ = node_handle_.advertise<visualization_msgs::MarkerArray>(
"visualize_place", 20,
true);
64 collision_object_publisher_ = node_handle_.advertise<moveit_msgs::CollisionObject>(
"/collision_object", 20);
65 planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>(
"planning_scene", 1);
68 visualization_msgs::MarkerArray
69 SemanticWorld::getPlaceLocationsMarker(
const std::vector<geometry_msgs::PoseStamped>& poses)
const
72 visualization_msgs::MarkerArray marker;
73 for (std::size_t i = 0; i < poses.size(); ++i)
75 visualization_msgs::Marker m;
78 m.ns =
"place_locations";
80 m.pose = poses[i].pose;
81 m.header = poses[i].header;
91 marker.markers.push_back(m);
96 bool SemanticWorld::addTablesToCollisionWorld()
102 std::map<std::string, object_recognition_msgs::Table>::iterator it;
103 for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
105 moveit_msgs::CollisionObject co;
107 co.operation = moveit_msgs::CollisionObject::REMOVE;
114 current_tables_in_collision_world_.clear();
116 for (std::size_t i = 0; i < table_array_.tables.size(); ++i)
118 moveit_msgs::CollisionObject co;
119 std::stringstream ss;
122 current_tables_in_collision_world_[co.id] = table_array_.tables[i];
123 co.operation = moveit_msgs::CollisionObject::ADD;
125 const std::vector<geometry_msgs::Point>& convex_hull = table_array_.tables[i].convex_hull;
128 std::vector<unsigned int> triangles((vertices.size() - 2) * 3);
129 for (
unsigned int j = 0; j < convex_hull.size(); ++j)
130 vertices[j] = Eigen::Vector3d(convex_hull[j].x, convex_hull[j].y, convex_hull[j].z);
131 for (
unsigned int j = 1; j < triangles.size() - 1; ++j)
133 unsigned int i3 = j * 3;
136 triangles[i3] = j + 1;
144 shapes::Mesh* table_mesh_solid = orientPlanarPolygon(*table_mesh);
145 if (!table_mesh_solid)
155 delete table_mesh_solid;
159 const shape_msgs::Mesh& table_shape_msg_mesh = boost::get<shape_msgs::Mesh>(table_shape_msg);
161 co.meshes.push_back(table_shape_msg_mesh);
162 co.mesh_poses.push_back(table_array_.tables[i].pose);
163 co.header = table_array_.tables[i].header;
167 delete table_mesh_solid;
173 object_recognition_msgs::TableArray SemanticWorld::getTablesInROI(
double minx,
double miny,
double minz,
double maxx,
174 double maxy,
double maxz)
const
176 object_recognition_msgs::TableArray tables_in_roi;
177 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
178 for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
180 if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
181 it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
182 it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
184 tables_in_roi.tables.push_back(it->second);
187 return tables_in_roi;
190 std::vector<std::string> SemanticWorld::getTableNamesInROI(
double minx,
double miny,
double minz,
double maxx,
191 double maxy,
double maxz)
const
193 std::vector<std::string> result;
194 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
195 for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
197 if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
198 it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
199 it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
201 result.push_back(it->first);
207 void SemanticWorld::clear()
209 table_array_.tables.clear();
210 current_tables_in_collision_world_.clear();
213 std::vector<geometry_msgs::PoseStamped>
214 SemanticWorld::generatePlacePoses(
const std::string& table_name,
const shapes::ShapeConstPtr& object_shape,
215 const geometry_msgs::Quaternion& object_orientation,
double resolution,
216 double delta_height,
unsigned int num_heights)
const
218 object_recognition_msgs::Table chosen_table;
219 std::map<std::string, object_recognition_msgs::Table>::const_iterator it =
220 current_tables_in_collision_world_.find(table_name);
222 if (it != current_tables_in_collision_world_.end())
224 chosen_table = it->second;
225 return generatePlacePoses(chosen_table, object_shape, object_orientation, resolution, delta_height, num_heights);
228 std::vector<geometry_msgs::PoseStamped> place_poses;
233 std::vector<geometry_msgs::PoseStamped>
234 SemanticWorld::generatePlacePoses(
const object_recognition_msgs::Table& chosen_table,
236 const geometry_msgs::Quaternion& object_orientation,
double resolution,
237 double delta_height,
unsigned int num_heights)
const
239 std::vector<geometry_msgs::PoseStamped> place_poses;
246 double x_min(std::numeric_limits<double>::max()), x_max(-std::numeric_limits<double>::max());
247 double y_min(std::numeric_limits<double>::max()), y_max(-std::numeric_limits<double>::max());
248 double z_min(std::numeric_limits<double>::max()), z_max(-std::numeric_limits<double>::max());
250 Eigen::Quaterniond rotation(object_orientation.x, object_orientation.y, object_orientation.z, object_orientation.w);
251 Eigen::Isometry3d object_pose(rotation);
252 double min_distance_from_edge = 0;
253 double height_above_table = 0;
262 position = object_pose * position;
264 if (x_min > position.x())
265 x_min = position.x();
266 if (x_max < position.x())
267 x_max = position.x();
268 if (y_min > position.y())
269 y_min = position.y();
270 if (y_max < position.y())
271 y_max = position.y();
272 if (z_min > position.z())
273 z_min = position.z();
274 if (z_max < position.z())
275 z_max = position.z();
277 min_distance_from_edge = 0.5 * std::max<double>(fabs(x_max - x_min), fabs(y_max - y_min));
278 height_above_table = -z_min;
283 min_distance_from_edge = std::max<double>(fabs(box->
size[0]), fabs(box->
size[1])) / 2.0;
284 height_above_table = fabs(box->
size[2]) / 2.0;
289 min_distance_from_edge = sphere->
radius;
290 height_above_table = -sphere->
radius;
295 min_distance_from_edge = cylinder->
radius;
296 height_above_table = cylinder->
length / 2.0;
301 min_distance_from_edge = cone->
radius;
302 height_above_table = cone->
length / 2.0;
305 return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights,
306 min_distance_from_edge);
309 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(
const object_recognition_msgs::Table& table,
310 double resolution,
double height_above_table,
311 double delta_height,
unsigned int num_heights,
312 double min_distance_from_edge)
const
314 std::vector<geometry_msgs::PoseStamped> place_poses;
316 if (table.convex_hull.empty())
318 const int scale_factor = 100;
319 std::vector<cv::Point2f> table_contour;
320 float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
321 for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
323 if (table.convex_hull[j].x < x_min)
324 x_min = table.convex_hull[j].x;
325 else if (table.convex_hull[j].x > x_max)
326 x_max = table.convex_hull[j].x;
327 if (table.convex_hull[j].y < y_min)
328 y_min = table.convex_hull[j].y;
329 else if (table.convex_hull[j].y > y_max)
330 y_max = table.convex_hull[j].y;
332 for (
const geometry_msgs::Point& vertex : table.convex_hull)
333 table_contour.push_back(cv::Point((vertex.x - x_min) * scale_factor, (vertex.y - y_min) * scale_factor));
335 double x_range = fabs(x_max - x_min);
336 double y_range = fabs(y_max - y_min);
337 int max_range = (int)x_range + 1;
338 if (max_range < (
int)y_range + 1)
339 max_range = (
int)y_range + 1;
341 int image_scale = std::max<int>(max_range, 4);
342 cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
344 for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
346 cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
349 unsigned int num_x = fabs(x_max - x_min) / resolution + 1;
350 unsigned int num_y = fabs(y_max - y_min) / resolution + 1;
354 std::vector<std::vector<cv::Point> > contours;
355 std::vector<cv::Vec4i> hierarchy;
356 cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
358 for (std::size_t j = 0; j < num_x; ++j)
360 int point_x = j * resolution * scale_factor;
361 for (std::size_t k = 0; k < num_y; ++k)
363 for (std::size_t mm = 0; mm < num_heights; ++mm)
365 int point_y = k * resolution * scale_factor;
366 cv::Point2f point2f(point_x, point_y);
367 double result = cv::pointPolygonTest(contours[0], point2f,
true);
368 if ((
int)result >= (
int)(min_distance_from_edge * scale_factor))
370 Eigen::Vector3d
point((
double)(point_x) / scale_factor + x_min, (
double)(point_y) / scale_factor + y_min,
371 height_above_table + mm * delta_height);
372 Eigen::Isometry3d pose;
375 geometry_msgs::PoseStamped place_pose;
376 place_pose.pose.orientation.w = 1.0;
377 place_pose.pose.position.x =
point.x();
378 place_pose.pose.position.y =
point.y();
379 place_pose.pose.position.z =
point.z();
380 place_pose.header = table.header;
381 place_poses.push_back(place_pose);
389 bool SemanticWorld::isInsideTableContour(
const geometry_msgs::Pose& pose,
const object_recognition_msgs::Table& table,
390 double min_distance_from_edge,
double min_vertical_offset)
const
393 if (table.convex_hull.empty())
395 float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
396 for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
398 if (table.convex_hull[j].x < x_min)
399 x_min = table.convex_hull[j].x;
400 else if (table.convex_hull[j].x > x_max)
401 x_max = table.convex_hull[j].x;
402 if (table.convex_hull[j].y < y_min)
403 y_min = table.convex_hull[j].y;
404 else if (table.convex_hull[j].y > y_max)
405 y_max = table.convex_hull[j].y;
407 const int scale_factor = 100;
408 std::vector<cv::Point2f> table_contour;
409 for (
const geometry_msgs::Point& vertex : table.convex_hull)
410 table_contour.push_back(cv::Point((vertex.x - x_min) * scale_factor, (vertex.y - y_min) * scale_factor));
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;
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);
421 for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
423 cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
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);
430 Eigen::Vector3d
point(pose.position.x, pose.position.y, pose.position.z);
431 Eigen::Isometry3d pose_table;
437 if (
point.z() < -fabs(min_vertical_offset))
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);
449 return (
int)result >= (int)(min_distance_from_edge * scale_factor);
452 std::string SemanticWorld::findObjectTable(
const geometry_msgs::Pose& pose,
double min_distance_from_edge,
453 double min_vertical_offset)
const
455 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
456 for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
459 if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset))
462 return std::string();
465 void SemanticWorld::tableCallback(
const object_recognition_msgs::TableArrayPtr& msg)
469 transformTableArray(table_array_);
478 void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& table_array)
const
480 for (object_recognition_msgs::Table& table : table_array.tables)
482 std::string original_frame = table.header.frame_id;
483 if (table.convex_hull.empty())
486 << table.pose.position.z);
487 std::string error_text;
488 const Eigen::Isometry3d& original_transform = planning_scene_->getFrameTransform(original_frame);
489 Eigen::Isometry3d original_pose;
491 original_pose = original_transform * original_pose;
493 table.header.frame_id = planning_scene_->getTransforms().getTargetFrame();
495 << table.header.frame_id);
497 << table.pose.position.z);
506 Eigen::Vector3d vec1, vec2, vec3, normal;
519 normal = vec3.cross(vec2);
536 int v_idx1 = polygon.
triangles[t_idx * 3];
537 int v_idx2 = polygon.
triangles[t_idx * 3 + 1];
538 int v_idx3 = polygon.
triangles[t_idx * 3 + 2];
540 vec1 = Eigen::Vector3d(polygon.
vertices[v_idx1 * 3], polygon.
vertices[v_idx1 * 3 + 1],
544 vec3 = Eigen::Vector3d(polygon.
vertices[v_idx3 * 3], polygon.
vertices[v_idx3 * 3 + 1],
550 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
552 if (triangle_normal.dot(normal) < 0.0)
563 Eigen::Vector3d vec1, vec2, vec3, normal;
576 normal = vec3.cross(vec2);
601 int v_idx1 = polygon.
triangles[t_idx * 3];
602 int v_idx2 = polygon.
triangles[t_idx * 3 + 1];
603 int v_idx3 = polygon.
triangles[t_idx * 3 + 2];
605 vec1 = Eigen::Vector3d(polygon.
vertices[v_idx1 * 3], polygon.
vertices[v_idx1 * 3 + 1],
607 vec2 = Eigen::Vector3d(polygon.
vertices[v_idx2 * 3], polygon.
vertices[v_idx2 * 3 + 1],
609 vec3 = Eigen::Vector3d(polygon.
vertices[v_idx3 * 3], polygon.
vertices[v_idx3 * 3 + 1],
615 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
617 if (triangle_normal.dot(normal) < 0.0)
624 for (
unsigned v_idx = 0; v_idx < polygon.
vertex_count; ++v_idx)