void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min, const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud, TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles) { const uint triangles_size = triangles->size(); const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); std::vector<uint> valid_points_remap(cloud_size,0); std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n"; uint offset; // check the points for (uint i = 0; i < cloud_size; i++) { const pcl::PointXYZ & pt = (*cloud)[i]; if (pt.x > max.x || pt.y > max.y || pt.z > max.z || pt.x < min.x || pt.y < min.y || pt.z < min.z) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); offset = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); // save new position for triangles remap valid_points_remap[i] = offset; offset++; } out_cloud->resize(offset); // discard invalid triangles out_triangles->clear(); out_triangles->reserve(triangles_size); offset = 0; for (uint i = 0; i < triangles_size; i++) { const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i]; bool is_valid = true; // validate all the vertices for (uint h = 0; h < 3; h++) if (!valid_points[tri.vertex_id[h]]) { is_valid = false; break; } if (is_valid) { kinfu_msgs::KinfuMeshTriangle out_tri; // remap the triangle for (uint h = 0; h < 3; h++) out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]]; out_triangles->push_back(out_tri); offset++; } } out_triangles->resize(offset); std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n"; }
void callback(const PointCloud::ConstPtr& cloud) { sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); NODELET_DEBUG("Got cloud"); //Copy Header output->header = cloud->header; output->header.frame_id = output_frame_id_; output->angle_min = -M_PI / 6; output->angle_max = M_PI / 6; output->angle_increment = M_PI / 180.0 / 2.0; output->time_increment = 0.0; output->scan_time = 1.0 / 30.0; output->range_min = 0.1; output->range_max = 10.0; uint32_t ranges_size = std::ceil( (output->angle_max - output->angle_min) / output->angle_increment); output->ranges.assign(ranges_size, output->range_max + 1.0); //pcl::PointCloud<pcl::PointXYZ> cloud; //pcl::fromROSMsg(*input, cloud); visualization_msgs::Marker line_list; float min_z = 100; float max_z = -100; float min_y = 100; float max_y = -100; // NODELET_INFO("New scan..."); for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it) { const float &x = it->x; const float &y = it->y; const float &z = it->z; if (z < min_z) min_z = z; if (z > max_z) max_z = z; if (y < min_y) min_y = y; if (y > max_y) max_y = y; /* for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++) for (uint32_t v = 0; v < cloud.width -1; v++) { //NODELET_ERROR("%d %d, %d %d", u, v, cloud.height, cloud.width); pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major? const float &x = point.x; const float &y = point.y; const float &z = point.z; */ if (std::isnan(x) || std::isnan(y) || std::isnan(z)) { NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } if (z > max_height_ || z < min_height_) { NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); continue; } double angle = atan2(y, x); if (angle < output->angle_min || angle > output->angle_max) { NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max); continue; } int index = (angle - output->angle_min) / output->angle_increment; // NODELET_INFO("index xyz( %f %f %f) angle %f index %d", x, y, z, angle, index); double range_sq = y * y + x * x; if (output->ranges[index] * output->ranges[index] > range_sq) output->ranges[index] = sqrt(range_sq); } NODELET_INFO("Y: %f %f, Z: %f %f", min_y, max_y, min_z, max_z); line_list.header = cloud->header; line_list.header.frame_id = output_frame_id_; line_list.ns = "points_and_lines"; line_list.action = visualization_msgs::Marker::ADD; line_list.pose.orientation.w = 1.0; line_list.id = 0; line_list.type = visualization_msgs::Marker::LINE_LIST; line_list.scale.x = 0.1; // line_list.color.r = 1.0; line_list.color.a = 1.0; // Create the vertices for the points and lines for (uint32_t i = 0; i < ranges_size; ++i) { float rng = output->ranges[i]; float a = output->angle_min + i * output->angle_increment; float x = rng * cos(a); float y = rng * sin(a); geometry_msgs::Point p; std_msgs::ColorRGBA col; p.x = x; p.y = y; p.z = min_height_; col.g = rng / (output->range_max); col.r = 1.0 - col.g; line_list.colors.push_back(col); line_list.colors.push_back(col); // The line list needs two points for each line line_list.points.push_back(p); p.z = max_height_; line_list.points.push_back(p); } marker_pub.publish(line_list); pub_.publish(output); }
void cloudCallback(const PointCloud::ConstPtr& source_msg) { if (source_msg->empty()) { ROS_ERROR("Can't get source cloud message.\n"); fType_ = t_null; return; } PointCloudWN::Ptr source_n (new PointCloudWN()); PointCloudWN::Ptr cloud_t (new PointCloudWN ()); Utilities::estimateNorCurv(source_msg, source_n); Utilities::rotateCloudXY(source_n, cloud_t, roll_, pitch_, transform_inv_); // std::cout << "trans_inv: "<< transform_inv_ << std::endl; FindPlane FP; FP.getParameters(GH); FP.findPlaneInCloud(cloud_t); PointCloudMono::Ptr plane_max (new PointCloudMono()); PointCloudMono::Ptr ground_max (new PointCloudMono()); pcl::ModelCoefficients::Ptr mcp (new pcl::ModelCoefficients); pcl::ModelCoefficients::Ptr mcg (new pcl::ModelCoefficients); processHullVector(FP.plane_hull, FP.plane_coeff, plane_max, mcp); processHullVector(FP.ground_hull, FP.ground_coeff, ground_max, mcg); if (!plane_max->empty() && (cloudPubPlaneMax_.getNumSubscribers() || posePubPlaneMax_.getNumSubscribers())) { sensor_msgs::PointCloud2 plane_max_msg; pcl::toROSMsg(*plane_max, plane_max_msg); plane_max_msg.header.frame_id = source_msg->header.frame_id; plane_max_msg.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp); geometry_msgs::PoseStamped pose_plane_max_msg; pose_plane_max_msg.header = plane_max_msg.header; pose_plane_max_msg.pose.orientation.x = mcp->values[0]; pose_plane_max_msg.pose.orientation.y = mcp->values[1]; pose_plane_max_msg.pose.orientation.z = mcp->values[2]; pose_plane_max_msg.pose.orientation.w = mcp->values[3]; cloudPubPlaneMax_.publish(plane_max_msg); posePubPlaneMax_.publish(pose_plane_max_msg); } if (!ground_max->empty() && (cloudPubGround_.getNumSubscribers() || posePubGround_.getNumSubscribers())) { sensor_msgs::PointCloud2 ground_max_msg; pcl::toROSMsg(*ground_max, ground_max_msg); ground_max_msg.header.frame_id = source_msg->header.frame_id; ground_max_msg.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp); geometry_msgs::PoseStamped pose_ground_msg; pose_ground_msg.header = ground_max_msg.header; pose_ground_msg.pose.orientation.x = mcg->values[0]; pose_ground_msg.pose.orientation.y = mcg->values[1]; pose_ground_msg.pose.orientation.z = mcg->values[2]; pose_ground_msg.pose.orientation.w = mcg->values[3]; cloudPubGround_.publish(ground_max_msg); posePubGround_.publish(pose_ground_msg); } if (!plane_max->empty() && !ground_max->empty()) { fType_ = t_both; } else if (plane_max->empty() && !ground_max->empty()) { fType_ = t_ground; } else if (!plane_max->empty() && ground_max->empty()) { fType_ = t_table; } else { fType_ = t_null; } }
void cloudCallback(const tf::TransformListener& listener, const PointCloud::ConstPtr& source_msg) { if (modeType_ != m_recognize && modeType_ != m_track) { return; } if (source_msg->empty()) { ROS_ERROR("Can't get source cloud message.\n"); return; } if (inliers->indices.empty()) { ROS_WARN_THROTTLE(5, "Object to grasp has not been found.\n"); return; } PointCloud::Ptr cloud_in (new PointCloud()); msgToCloud(source_msg, cloud_in); PointCloud::Ptr cloud_out (new PointCloud ()); getCloudByInliers(cloud_in, cloud_out, inliers, false, false); MakePlan MP; pcl::PointXYZRGB avrPt; hasGraspPlan_ = MP.process(cloud_out, pa_, pb_, pc_, pd_, avrPt); if (hasGraspPlan_) { visualization_msgs::Marker marker; // Set the frame ID and timestamp. See the TF tutorials for information on these. marker.header.frame_id = "/camera_yaw_frame"; marker.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp); // Set the namespace and id for this marker. This serves to create a unique ID // Any marker sent with the same namespace and id will overwrite the old one marker.ns = "grasp"; marker.id = 0; marker.type = shape; marker.action = visualization_msgs::Marker::ADD; // transform the vector listener.waitForTransform("/camera_yaw_frame", source_msg->header.frame_id, ros::Time::now(), ros::Duration(1.0)); tf::Stamped<tf::Point> p_in_a; tf::Stamped<tf::Point> p_out_a; p_in_a.setX(avrPt.x); p_in_a.setY(avrPt.y); p_in_a.setZ(avrPt.z); p_in_a.frame_id_ = source_msg->header.frame_id; listener.transformPoint("/camera_yaw_frame", p_in_a, p_out_a); tf::Stamped<tf::Point> p_in_b; tf::Stamped<tf::Point> p_out_b; float la, lb, lc; normalize(la, lb, lc, 0.15); p_in_b.setX(avrPt.x + la); p_in_b.setY(avrPt.y + lb); p_in_b.setZ(avrPt.z + lc); p_in_b.frame_id_ = source_msg->header.frame_id; listener.transformPoint("/camera_yaw_frame", p_in_b, p_out_b); // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker.points.resize(2); marker.points[0].x = p_out_a.x() + 0.03; marker.points[0].y = p_out_a.y(); marker.points[0].z = p_out_a.z(); marker.points[1].x = p_out_b.x() + 0.03; marker.points[1].y = p_out_b.y(); marker.points[1].z = p_out_b.z(); // The point at index 0 is assumed to be the start point, and the point at index 1 is assumed to be the end. // scale.x is the shaft diameter, and scale.y is the head diameter. If scale.z is not zero, it specifies the head length. // Set the scale of the marker -- 1x1x1 here means 1m on a side marker.scale.x = 0.01; marker.scale.y = 0.015; marker.scale.z = 0.04; // Set the color -- be sure to set alpha to something non-zero! marker.color.r = 0.0f; marker.color.g = 1.0f; marker.color.b = 0.0f; marker.color.a = 1.0; markerPub_.publish(marker); } else { ROS_WARN_THROTTLE(5, "Failed to generate grasp plan.\n"); } }
void callback(const PointCloud::ConstPtr& cloud) { sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); NODELET_DEBUG("Got cloud"); //Copy Header output->header = cloud->header; output->header.frame_id = output_frame_id_; output->angle_min = -M_PI/2; output->angle_max = M_PI/2; output->angle_increment = M_PI/180.0/2.0; output->time_increment = 0.0; output->scan_time = 1.0/30.0; output->range_min = 0.45; output->range_max = 10.0; uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment); output->ranges.assign(ranges_size, output->range_max + 1.0); //pcl::PointCloud<pcl::PointXYZ> cloud; //pcl::fromROSMsg(*input, cloud); for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it) { const float &x = it->x; const float &y = it->y; const float &z = it->z; /* for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++) for (uint32_t v = 0; v < cloud.width -1; v++) { //NODELET_ERROR("%d %d, %d %d", u, v, cloud.height, cloud.width); pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major? const float &x = point.x; const float &y = point.y; const float &z = point.z; */ if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ) { NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } if (-y > max_height_ || -y < min_height_) { NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_); continue; } double angle = -atan2(x, z); if (angle < output->angle_min || angle > output->angle_max) { NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max); continue; } int index = (angle - output->angle_min) / output->angle_increment; //printf ("index xyz( %f %f %f) angle %f index %d\n", x, y, z, angle, index); double range_sq = z*z+x*x; if (output->ranges[index] * output->ranges[index] > range_sq) output->ranges[index] = sqrt(range_sq); } pub_.publish(output); }