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