/**
* Occupied node handler
*/
void srs_env_model::CCollisionGridPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp)
{
if (it.getDepth() == m_crawlDepth)
{
octomap::OcTreeKey nKey = it.getKey(); // TODO: remove intermedate obj (1.4)
int i = (nKey[0] - m_paddedMinKey[0])/m_multires2DScale;;
int j = (nKey[1] - m_paddedMinKey[1])/m_multires2DScale;;
m_data->data[m_data->info.width * j + i] = 100;
} else {
int intSize = 1 << (m_crawlDepth - it.getDepth());
octomap::OcTreeKey minKey = it.getIndexKey();
for (int dx = 0; dx < intSize; dx++) {
int i = (minKey[0] + dx - m_paddedMinKey[0])/m_multires2DScale;;
for (int dy = 0; dy < intSize; dy++) {
int j = (minKey[1] + dy - m_paddedMinKey[1])/m_multires2DScale;;
m_data->data[m_data->info.width * j + i] = 100;
}
}
}
}
/**
* Free node handler
*/
void srs_env_model::CMap2DPlugin::handleFreeNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp )
{
if (it.getDepth() == mp.treeDepth) {
octomap::OcTreeKey nKey = it.getKey(); //TODO: remove intermedate obj (1.4)
int i = nKey[0] - m_paddedMinKey[0];
int j = nKey[1] - m_paddedMinKey[1];
if (m_data->data[m_data->info.width * j + i] == -1) {
m_data->data[m_data->info.width * j + i] = 0;
}
} else {
int intSize = 1 << (mp.treeDepth - it.getDepth());
octomap::OcTreeKey minKey = it.getIndexKey();
for (int dx = 0; dx < intSize; dx++) {
int i = minKey[0] + dx - m_paddedMinKey[0];
for (int dy = 0; dy < intSize; dy++) {
int j = minKey[1] + dy - m_paddedMinKey[1];
if (m_data->data[m_data->info.width * j + i] == -1) {
m_data->data[m_data->info.width * j + i] = 0;
}
}
}
}
}
/**
* 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... ");
}
}
/**
* 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 );
}
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);
}
/// 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 SMapWithParameters & 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;
tBox 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);
}