C++ (Cpp) iterator::getSize Examples

Example #1

File: cmap_plugin.cpp Project: beds-tao/srs_public

/// 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);
}

Example #2

File: collision_object_plugin.cpp Project: vstancl/srs_public

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);
}