示例#1
0
/// hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
void srs_env_model::CCMapPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapParameters & mp)
{
	if( m_bLocked )
			return;

	// Should we publish something?
	if (! m_publishCollisionMap)
		return;

	// Is this point near enough to the robot?
	if( ! isNearRobot( btVector3( it.getX(), it.getY(), it.getZ() ), it.getSize() ) )
		return;

	Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() );

	if( m_bConvertPoint )
	{
	    // Transform point from the world to the CMap TF
	    point = m_worldToCMapRot * point + m_worldToCMapTrans;
	}

	// Add point to the collision map
	arm_navigation_msgs::OrientedBoundingBox box;
	double size = it.getSize();
	box.extents.x = box.extents.y = box.extents.z = size;
	box.axis.x = box.axis.y = 0.0;
	box.axis.z = 1.0;
	box.angle = 0.0;
	box.center.x = point[0];
	box.center.y = point[1];
	box.center.z = point[2];


	m_dataBuffer->boxes.push_back(box);
}
void srs_env_model::CMarkerArrayPlugin::handleNode(const srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp)
{
    unsigned idx = it.getDepth();
    assert(idx < m_data->markers.size());

    geometry_msgs::Point cubeCenter;



    if( m_bTransform )
    {
        // Transform input point
        Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() );
        point = m_ocToMarkerArrayRot * point + m_ocToMarkerArrayTrans;
        cubeCenter.x = it.getX();
        cubeCenter.y = it.getY();
        cubeCenter.z = it.getZ();

    }else{
        cubeCenter.x = it.getX();
        cubeCenter.y = it.getY();
        cubeCenter.z = it.getZ();
    }

    m_data->markers[idx].points.push_back(cubeCenter);

    if (m_bHeightMap){
        double h = (1.0 - std::min(std::max((cubeCenter.z-m_minZ)/ (m_maxZ - m_minZ), 0.0), 1.0)) *m_colorFactor;
        m_data->markers[idx].colors.push_back(heightMapColor(h));
    }
}
/// hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
void srs_env_model::CPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
//	std::cerr << "PCP: handle occupied" << std::endl;
	tPclPoint point;

	// Set position
	point.x = it.getX();
	point.y = it.getY();
	point.z = it.getZ();


	// Set color
	point.r = it->r();
	point.g = it->g();
	point.b = it->b();

//	std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl;
	/*
	// Set color
	point.r = 255 - counter % 255;
	point.g = counter % 255;
	point.b = 128;
*/
	m_data->push_back( point );

	++counter;
}
/**
 * hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
 */
void srs_env_model::CCompressedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
//	PERROR("OnHandleOccupied");

	++m_countAll;

	if( ! m_bCamModelInitialized )
		return;

	if( ! m_bPublishComplete )
	{
		// Test input point
		tf::Point pos(it.getX(), it.getY(), it.getZ());
		tf::Point posRel = m_to_sensor(pos);
		cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( posRel.x(), posRel.y(), posRel.z()));

		// ignore point if not in sensor cone
		if (!inSensorCone(uv))
			return;
	}

	// Ok, add it...
	CPointCloudPlugin::handleOccupiedNode( it, mp );
	++m_countVisible;
}
/**
 * hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
 */
void srs_env_model::COcPatchMaker::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
//	PERROR("OnHandleOccupied");

	if( ! m_bCamModelInitialized )
		return;

	// Test input point
	tf::Point pos(it.getX(), it.getY(), it.getZ());
//	if( m_bTransformCamera )
	pos = m_to_sensorTf(pos);

	pos = pos * m_fracMatrix;

	if( pos.z() < 0 )
		return;

	cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( pos.x(), pos.y(), pos.z()));

	// ignore point if not in sensor cone
	if (!inSensorCone(uv))
		return;

	// Ok, add it...
	//	std::cerr << "PCP: handle occupied" << std::endl;
	tPclPoint point;

	// Set position
	point.x = it.getX();
	point.y = it.getY();
	point.z = it.getZ();

	// Set color
	point.r = it->r();
	point.g = it->g();
	point.b = it->b();

//	std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl;

	m_cloud.push_back( point );
}
/**
 * hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
 */
void srs_env_model::CLimitedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
    Eigen::Vector3f p( it.getX(), it.getY(), it.getZ() );

     float f( p.dot( m_normal ) + m_d);
    if( f > max )
        max = f;
    if( f < min)
        min = f;

    // Test if point is in front of camera
    if( (p.dot( m_normal ) + m_d) > 0 )
    {
        // Ok, add it...
        CPointCloudPlugin::handleOccupiedNode( it, mp );
        ++m_countVisible;
    }
    else
    {
        ++m_countHidden;
  //      PERROR( "Point behind camera... ");
    }
}
void srs_env_model::CCollisionObjectPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp)
{
	// Transform input point
	Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() );

	// Convert point if needed...
	if( m_bConvert )
	    point = m_ocToCoRot * point + m_ocToCoTrans;

	// Add shape
	arm_navigation_msgs::Shape shape;
	shape.type = arm_navigation_msgs::Shape::BOX;
	shape.dimensions.resize(3);
	shape.dimensions[0] = shape.dimensions[1] = shape.dimensions[2] = it.getSize();
	m_data->shapes.push_back(shape);

	// Add pose
	geometry_msgs::Pose pose;
	pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
	pose.position.x = point.x();
	pose.position.y = point.y();
	pose.position.z = point.z();
	m_data->poses.push_back(pose);
}