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