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
58 : planning_scene_(planning_scene)
66 visualization_msgs::MarkerArray
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)
73 visualization_msgs::Marker m;
76 m.ns =
"place_locations";
78 m.pose = poses[i].pose;
79 m.header = poses[i].header;
89 marker.markers.push_back(m);
97 planning_scene.is_diff =
true;
100 std::map<std::string, object_recognition_msgs::Table>::iterator it;
103 moveit_msgs::CollisionObject co;
105 co.operation = moveit_msgs::CollisionObject::REMOVE;
106 planning_scene.world.collision_objects.push_back(co);
111 planning_scene.world.collision_objects.clear();
114 for (std::size_t i = 0; i <
table_array_.tables.size(); ++i)
116 moveit_msgs::CollisionObject co;
117 std::stringstream ss;
121 co.operation = moveit_msgs::CollisionObject::ADD;
123 const std::vector<geometry_msgs::Point>& convex_hull =
table_array_.tables[i].convex_hull;
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)
131 unsigned int i3 = j * 3;
134 triangles[i3] = j + 1;
143 if (!table_mesh_solid)
153 delete table_mesh_solid;
157 const shape_msgs::Mesh& table_shape_msg_mesh = boost::get<shape_msgs::Mesh>(table_shape_msg);
159 co.meshes.push_back(table_shape_msg_mesh);
162 planning_scene.world.collision_objects.push_back(co);
165 delete table_mesh_solid;
172 double maxy,
double maxz)
const 174 object_recognition_msgs::TableArray tables_in_roi;
175 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
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)
182 tables_in_roi.tables.push_back(it->second);
185 return tables_in_roi;
189 double maxy,
double maxz)
const 191 std::vector<std::string> result;
192 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
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)
199 result.push_back(it->first);
211 std::vector<geometry_msgs::PoseStamped>
213 const geometry_msgs::Quaternion& object_orientation,
double resolution,
214 double delta_height,
unsigned int num_heights)
const 216 object_recognition_msgs::Table chosen_table;
217 std::map<std::string, object_recognition_msgs::Table>::const_iterator it =
222 chosen_table = it->second;
223 return generatePlacePoses(chosen_table, object_shape, object_orientation, resolution, delta_height, num_heights);
226 std::vector<geometry_msgs::PoseStamped> place_poses;
227 ROS_ERROR(
"Did not find table %s to place on", table_name.c_str());
231 std::vector<geometry_msgs::PoseStamped>
234 const geometry_msgs::Quaternion& object_orientation,
double resolution,
235 double delta_height,
unsigned int num_heights)
const 237 std::vector<geometry_msgs::PoseStamped> place_poses;
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());
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;
260 position = object_pose * position;
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();
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;
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;
287 min_distance_from_edge = sphere->
radius;
288 height_above_table = -sphere->
radius;
293 min_distance_from_edge = cylinder->
radius;
294 height_above_table = cylinder->
length / 2.0;
299 min_distance_from_edge = cone->
radius;
300 height_above_table = cone->
length / 2.0;
303 return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights,
304 min_distance_from_edge);
308 double resolution,
double height_above_table,
309 double delta_height,
unsigned int num_heights,
310 double min_distance_from_edge)
const 312 std::vector<geometry_msgs::PoseStamped> place_poses;
314 if (table.convex_hull.empty())
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)
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;
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));
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;
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);
343 for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
345 cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
348 unsigned int num_x = fabs(x_max - x_min) / resolution + 1;
349 unsigned int num_y = fabs(y_max - y_min) / resolution + 1;
351 ROS_DEBUG(
"Num points for possible place operations: %d %d", num_x, num_y);
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);
357 for (std::size_t j = 0; j < num_x; ++j)
359 int point_x = j * resolution * scale_factor;
360 for (std::size_t k = 0; k < num_y; ++k)
362 for (std::size_t mm = 0; mm < num_heights; ++mm)
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))
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;
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);
389 double min_distance_from_edge,
double min_vertical_offset)
const 392 if (table.convex_hull.empty())
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)
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;
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));
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::Affine3d pose_table;
435 point = pose_table.inverse(Eigen::Isometry) * point;
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 if ((
int)result >= (
int)(min_distance_from_edge * scale_factor))
456 double min_vertical_offset)
const 458 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
461 ROS_DEBUG(
"Testing table: %s", it->first.c_str());
465 return std::string();
483 for (std::size_t i = 0; i < table_array.tables.size(); ++i)
485 std::string original_frame = table_array.tables[i].header.frame_id;
486 if (table_array.tables[i].convex_hull.empty())
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;
495 original_pose = original_transform * original_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);
511 Eigen::Vector3d vec1, vec2, vec3, normal;
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]);
521 normal = vec3.cross(vec2);
539 int vIdx2 = polygon.
triangles[tIdx * 3 + 1];
540 int vIdx3 = polygon.
triangles[tIdx * 3 + 2];
552 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
554 if (triangle_normal.dot(normal) < 0.0)
565 Eigen::Vector3d vec1, vec2, vec3, normal;
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]);
575 normal = vec3.cross(vec2);
601 int vIdx2 = polygon.
triangles[tIdx * 3 + 1];
602 int vIdx3 = polygon.
triangles[tIdx * 3 + 2];
614 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
616 if (triangle_normal.dot(normal) < 0.0)
623 for (
unsigned vIdx = 0; vIdx < polygon.
vertex_count; ++vIdx)
void transformTableArray(object_recognition_msgs::TableArray &table_array) const
TableCallbackFn table_callback_
bool addTablesToCollisionWorld()
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())
ros::Publisher planning_scene_diff_publisher_
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...
visualization_msgs::MarkerArray getPlaceLocationsMarker(const std::vector< geometry_msgs::PoseStamped > &poses) const
ros::Publisher collision_object_publisher_
object_recognition_msgs::TableArray table_array_
void tableCallback(const object_recognition_msgs::TableArrayPtr &msg)
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
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber table_subscriber_
ros::Publisher visualization_publisher_
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...
ros::NodeHandle node_handle_
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.
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
std::shared_ptr< const Shape > ShapeConstPtr