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");
        }
}
Exemple #5
0
    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);
    }