void srs_env_model::CCollisionObjectPlugin::newMapDataCB( SMapWithParameters & par )
{
m_data->header.frame_id = m_coFrameId;
m_data->header.stamp = par.currentTime;
m_data->id = "map";
m_ocFrameId = par.frameId;
tf::StampedTransform ocToCoTf;
m_bConvert = m_ocFrameId != m_coFrameId;
/// We need no transformation - frames are the same...
if( ! m_bConvert )
return;
// Get transform
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_coFrameId, m_ocFrameId,
par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform(m_coFrameId, m_ocFrameId,
par.currentTime, ocToCoTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR("Transform error.");
return;
}
Eigen::Matrix4f ocToPcTM;
// Get transformation matrix
pcl_ros::transformAsMatrix(ocToCoTf, ocToPcTM);
// Disassemble translation and rotation
m_ocToCoRot = ocToPcTM.block<3, 3> (0, 0);
m_ocToCoTrans = ocToPcTM.block<3, 1> (0, 3);
// Initialize leaf iterators
tButServerOcTree & tree( par.map->octree );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
m_DataTimeStamp = par.currentTime;
invalidate();
}
//! Set used octomap frame id and timestamp
void srs_env_model::CPointCloudPlugin::newMapDataCB( SMapWithParameters & par )
{
if( ! m_publishPointCloud )
return;
// PERROR("New map: Try lock");
boost::mutex::scoped_lock lock(m_lockData);
// PERROR("New map: Lock");
m_data->clear();
// Just for sure
if(m_frame_id != par.frameId)
{
ROS_ERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB.");
return;
}
m_frame_id = par.frameId;
m_DataTimeStamp = m_time_stamp = par.currentTime;
counter = 0;
// Pointcloud is used as output for octomap...
m_bAsInput = false;
// Initialize leaf iterators
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
// 2013/01/31 Majkl
m_data->header.frame_id = par.frameId;
m_data->header.stamp = par.currentTime;
lock.unlock();
// PERROR( "New map: Unlocked");
invalidate();
}
//! Set used octomap frame id and timestamp
void srs_env_model::CCMapPlugin::newMapDataCB( SMapWithParameters & par )
{
if( m_bLocked )
{
return;
}
// Lock data
boost::mutex::scoped_lock lock(m_lockData);
// Reset collision map buffer
m_dataBuffer->boxes.clear();
std::string robotBaseFrameId("/base_footprint");
// Get octomap to collision map transform matrix
tf::StampedTransform omapToCmapTf, // Octomap to collision map
baseToOmapTf, // Robot baselink to octomap
cmapToWorldTF; // Collision map to world
try
{
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_cmapFrameId, par.frameId, par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform(m_cmapFrameId, par.frameId, par.currentTime, omapToCmapTf);
m_tfListener.waitForTransform(par.frameId, robotBaseFrameId, par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform(par.frameId, robotBaseFrameId, par.currentTime, baseToOmapTf);
}
catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR( "Transform error.");
return;
}
// World TF to the collision map TF
pcl_ros::transformAsMatrix(omapToCmapTf, m_worldToCMapTM );
// Disassemble world to cmap translation and rotation
m_worldToCMapRot = m_worldToCMapTM.block<3, 3> (0, 0);
m_worldToCMapTrans = m_worldToCMapTM.block<3, 1> (0, 3);
m_bConvertPoint = m_cmapFrameId != par.frameId;
// Compute robot position in the collision map coordinate system
geometry_msgs::TransformStamped msg;
tf::transformStampedTFToMsg(baseToOmapTf, msg);
m_robotBasePosition.setX( msg.transform.translation.x );
m_robotBasePosition.setY( msg.transform.translation.y );
m_robotBasePosition.setZ( msg.transform.translation.z );
tButServerOcTree & tree( par.map->octree );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
m_DataTimeStamp = par.currentTime;
lock.unlock();
invalidate();
}
/**
* Get output pointcloud
*/
bool srs_env_model::COcPatchMaker::computeCloud( const SMapWithParameters & par, const ros::Time & time )
{
ROS_DEBUG( "CCompressedPointCloudPlugin: onFrameStart" );
// Copy buffered camera normal and d parameter
boost::recursive_mutex::scoped_lock lock( m_camPosMutex );
// Clear data
m_cloud.clear();
m_ocFrameId = par.frameId;
m_DataTimeStamp = m_time_stamp = time;
bool bTransformOutput = m_ocFrameId != m_pcFrameId;
// Output transform matrix
Eigen::Matrix4f pcOutTM;
// If different frame id
if( bTransformOutput )
{
tf::StampedTransform ocToPcTf;
// Get transform
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_pcFrameId, m_ocFrameId,
m_time_stamp, ros::Duration(5));
m_tfListener.lookupTransform(m_pcFrameId, m_ocFrameId,
m_time_stamp, ocToPcTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
ROS_ERROR_STREAM( "Transform error.");
return false;
}
// Get transformation matrix
pcl_ros::transformAsMatrix(ocToPcTf, pcOutTM); // Sensor TF to defined base TF
}
if( m_cameraFrameId.size() == 0 )
{
ROS_ERROR_STREAM("Wrong camera frame id...");
m_bTransformCamera = false;
return false;
}
m_bTransformCamera = m_cameraFrameId != m_ocFrameId;
// Store camera information
m_camera_size = m_camera_size_buffer;
m_camera_model.fromCameraInfo( m_camera_info_buffer);
// Transform camera model to the cloud time and frame id
{
tf::StampedTransform camTf;
// Modify camera position to the current time
if( m_bTransformCamera )
{
// We need camera to octomap transfor
try {
// Transformation - to, from, time, waiting time
// std::cerr << "T1, try to get transform from " << m_cameraFrameId << " to " << m_ocFrameId << ", time: " << par.currentTime << std::endl;
m_tfListener.waitForTransform(m_ocFrameId, m_cameraFrameId,
m_time_stamp, ros::Duration(5.0));
m_tfListener.lookupTransform(m_ocFrameId, m_cameraFrameId,
m_time_stamp, camTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error1: " << ex.what() << ", quitting callback");
return false;
}
}
else
{
// Just move camera position "in time"
try {
// Time transformation - target frame, target time, source frame, source time, fixed frame, time, waiting time
m_tfListener.waitForTransform(m_cameraFrameId, m_time_stamp,
m_cameraFrameId, m_camera_info_buffer.header.stamp,
"/map", ros::Duration(1.0));
m_tfListener.lookupTransform( m_cameraFrameId, m_time_stamp,
m_cameraFrameId, m_camera_info_buffer.header.stamp,
"/map", camTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error2: " << ex.what() << ", quitting callback");
return false;
}
}
// We have transform so convert it to the eigen matrix
m_to_sensorTf = camTf.inverse();
}
// Initialize leaf iterators
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Compute fraction matrix
m_fracMatrix = btMatrix3x3::getIdentity().scaled( tf::Point( 1.0 / m_fracX, 1.0 / m_fracY, 1.0 ) );
// Crawl through nodes
for ( it = tree.begin_leafs(par.treeDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
if( bTransformOutput )
{
// transform point cloud from octomap frame to the preset frame
pcl::transformPointCloud< tPclPoint >(m_cloud, m_cloud, pcOutTM);
}
if( m_bPublishCloud )
publishInternal( m_DataTimeStamp );
return true;
}
void srs_env_model::CCollisionGridPlugin::newMapDataCB(SMapWithParameters & par)
{
// init projected 2D map:
m_data->header.frame_id = par.frameId;
m_data->header.stamp = par.currentTime;
m_crawlDepth = par.treeDepth;
bool sizeChanged(false);
// TODO: move most of this stuff into c'tor and init map only once (adjust if size changes)
double minX, minY, minZ, maxX, maxY, maxZ;
par.map->getTree().getMetricMin(minX, minY, minZ);
par.map->getTree().getMetricMax(maxX, maxY, maxZ);
octomap::point3d minPt(minX, minY, minZ);
octomap::point3d maxPt(maxX, maxY, maxZ);
octomap::OcTreeKey minKey, maxKey, curKey;
if (!par.map->getTree().genKey(minPt, minKey)){
ROS_ERROR("Could not create min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
return;
}
if (!par.map->getTree().genKey(maxPt, maxKey)){
ROS_ERROR("Could not create max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
return;
}
par.map->getTree().genKeyAtDepth(minKey, par.treeDepth, minKey);
par.map->getTree().genKeyAtDepth(maxKey, par.treeDepth, maxKey);
ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]);
// add padding if requested (= new min/maxPts in x&y):
double halfPaddedX = 0.5*m_minSizeX;
double halfPaddedY = 0.5*m_minSizeY;
minX = std::min(minX, -halfPaddedX);
maxX = std::max(maxX, halfPaddedX);
minY = std::min(minY, -halfPaddedY);
maxY = std::max(maxY, halfPaddedY);
minPt = octomap::point3d(minX, minY, minZ);
maxPt = octomap::point3d(maxX, maxY, maxZ);
octomap::OcTreeKey paddedMaxKey;
if (!par.map->getTree().genKey(minPt, m_paddedMinKey)){
ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
return;
}
if (!par.map->getTree().genKey(maxPt, paddedMaxKey)){
ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
return;
}
par.map->getTree().genKeyAtDepth(m_paddedMinKey, par.treeDepth, m_paddedMinKey);
par.map->getTree().genKeyAtDepth(paddedMaxKey, par.treeDepth, paddedMaxKey);
ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]);
assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]);
m_multires2DScale = 1 << (par.treeDepth - m_crawlDepth);
unsigned int newWidth = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1;
unsigned int newHeight = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1;
if (newWidth != m_data->info.width || newHeight != m_data->info.height)
sizeChanged = true;
m_data->info.width = newWidth;
m_data->info.height = newHeight;
int mapOriginX = minKey[0] - m_paddedMinKey[0];
int mapOriginY = minKey[1] - m_paddedMinKey[1];
assert(mapOriginX >= 0 && mapOriginY >= 0);
// might not exactly be min / max of octree:
octomap::point3d origin;
par.map->getTree().genCoords(m_paddedMinKey, m_crawlDepth, origin);
double gridRes = par.map->getTree().getNodeSize(par.treeDepth);
m_data->info.resolution = gridRes;
m_data->info.origin.position.x = origin.x() - gridRes*0.5;
m_data->info.origin.position.y = origin.y() - gridRes*0.5;
// std::cerr << "Origin: " << origin << ", grid resolution: " << gridRes << ", computed origin: "<< mapOriginX << ".." << mapOriginY << std::endl;
if (par.treeDepth != m_crawlDepth){
m_data->info.origin.position.x -= par.resolution/2.0;
m_data->info.origin.position.y -= par.resolution/2.0;
}
if (sizeChanged){
ROS_INFO("2D grid map size changed to %d x %d", m_data->info.width, m_data->info.height);
m_data->data.clear();
// init to unknown:
m_data->data.resize(m_data->info.width * m_data->info.height, -1);
}
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
else
{
handleFreeNode( it, par );
}
} // Iterate through octree
m_DataTimeStamp = par.currentTime;
invalidate();
}
void srs_env_model::CMap2DPlugin::newMapDataCB(SMapWithParameters & par)
{
m_data->header.frame_id = m_map2DFrameId;
m_data->header.stamp = par.currentTime;
m_data->info.resolution = par.resolution;
m_ocFrameId = par.frameId;
ros::Time timestamp( par.currentTime );
tf::StampedTransform ocToMap2DTf;
m_bConvert = m_ocFrameId != m_map2DFrameId;
// Get transform
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_map2DFrameId, m_ocFrameId,
timestamp, ros::Duration(5));
m_tfListener.lookupTransform(m_map2DFrameId, m_ocFrameId,
timestamp, ocToMap2DTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR( "Transform error.");
return;
}
Eigen::Matrix4f ocToMap2DTM;
// Get transformation matrix
pcl_ros::transformAsMatrix(ocToMap2DTf, ocToMap2DTM);
const tButServerOcMap &map(*par.map);
// Disassemble translation and rotation
m_ocToMap2DRot = ocToMap2DTM.block<3, 3> (0, 0);
m_ocToMap2DTrans = ocToMap2DTM.block<3, 1> (0, 3);
double minX, minY, minZ, maxX, maxY, maxZ;
map.getTree().getMetricMin(minX, minY, minZ);
map.getTree().getMetricMax(maxX, maxY, maxZ);
octomap::point3d minPt(minX, minY, minZ);
octomap::point3d maxPt(maxX, maxY, maxZ);
octomap::OcTreeKey minKey, maxKey, curKey;
// Try to create key
if (!map.getTree().genKey(minPt, minKey)) {
ROS_ERROR("Could not create min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
return;
}
if (!map.getTree().genKey(maxPt, maxKey)) {
ROS_ERROR("Could not create max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
return;
}
ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]);
// add padding if requested (= new min/maxPts in x&y):
double halfPaddedX = 0.5 * m_minSizeX;
double halfPaddedY = 0.5 * m_minSizeY;
minX = std::min(minX, -halfPaddedX);
maxX = std::max(maxX, halfPaddedX);
minY = std::min(minY, -halfPaddedY);
maxY = std::max(maxY, halfPaddedY);
minPt = octomap::point3d(minX, minY, minZ);
maxPt = octomap::point3d(maxX, maxY, maxZ);
octomap::OcTreeKey paddedMaxKey;
if (!map.getTree().genKey(minPt, m_paddedMinKey)) {
ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
return;
}
if (!map.getTree().genKey(maxPt, paddedMaxKey)) {
ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
return;
}
ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1],
m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]);
assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]);
m_data->info.width = paddedMaxKey[0] - m_paddedMinKey[0] + 1;
m_data->info.height = paddedMaxKey[1] - m_paddedMinKey[1] + 1;
int mapOriginX = minKey[0] - m_paddedMinKey[0];
int mapOriginY = minKey[1] - m_paddedMinKey[1];
assert(mapOriginX >= 0 && mapOriginY >= 0);
// might not exactly be min / max of octree:
octomap::point3d origin;
map.getTree().genCoords(m_paddedMinKey, par.treeDepth, origin);
m_data->info.origin.position.x = origin.x() - par.resolution* 0.5;
m_data->info.origin.position.y = origin.y() - par.resolution * 0.5;
// Allocate space to hold the data (init to unknown)
m_data->data.resize(m_data->info.width * m_data->info.height, -1);
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
else
{
handleFreeNode( it, par );
}
} // Iterate through octree
invalidate();
}
//! Set used octomap frame id and timestamp
void srs_env_model::CPointCloudPlugin::newMapDataCB( SMapWithParameters & par )
{
if( ! m_publishPointCloud )
return;
m_data->clear();
m_ocFrameId = par.frameId;
m_DataTimeStamp = m_time_stamp = par.currentTime;
counter = 0;
// Pointcloud is used as output for octomap...
m_bAsInput = false;
// If different frame id
if( m_ocFrameId != m_pcFrameId )
{
tf::StampedTransform ocToPcTf;
// Get transform
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_pcFrameId, m_ocFrameId,
par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform(m_pcFrameId, m_ocFrameId,
par.currentTime, ocToPcTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR( "Transform error.");
return;
}
// Get transformation matrix
pcl_ros::transformAsMatrix(ocToPcTf, m_pcOutTM); // Sensor TF to defined base TF
}
// Initialize leaf iterators
tButServerOcTree & tree( par.map->octree );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
// If different frame id
if( (!m_bAsInput) && (m_ocFrameId != m_pcFrameId) )
{
// transform point cloud from sensor frame to the preset frame
pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, m_pcOutTM);
}
invalidate();
}
void srs_env_model::CCompressedPointCloudPlugin::newMapDataCB( SMapWithParameters & par )
{
ROS_DEBUG( "CCompressedPointCloudPlugin: onFrameStart" );
// Copy buffered camera normal and d parameter
boost::recursive_mutex::scoped_lock lock( m_camPosMutex );
// Increase frames count
++m_frame_counter;
// Is this frame complete?
m_bPublishComplete = m_frame_counter % m_uncomplete_frames == 0;
// Create packed octomap update info?
m_bCreatePackedInfoMsg = m_ocUpdatePublisher.getNumSubscribers() > 0;
// Reset counters
m_countVisible = m_countAll = 0;
min = 10000000;
max = -10000000;
// Call parent frame start
if( ! m_publishPointCloud )
return;
// Just for sure
if(m_frameId != par.frameId)
{
PERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB.");
return;
}
// Clear data
m_data->clear();
m_DataTimeStamp = m_time_stamp = par.currentTime;
counter = 0;
// Pointcloud is used as output for octomap...
m_bAsInput = false;
if( m_cameraFrameId.size() == 0 )
{
ROS_DEBUG("CCompressedPointCloudPlugin::newMapDataCB: Wrong camera frame id...");
// m_bTransformCamera = false;
return;
}
// m_bTransformCamera = m_cameraFrameId != m_pcFrameId;
bool m_bTransformCamera(m_cameraFrameId != m_frameId);
m_to_sensor = tf::StampedTransform::getIdentity();
// If different frame id
if( m_bTransformCamera )
{
// Some transforms
tf::StampedTransform camToOcTf, ocToCamTf;
// Get transforms
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_cameraFrameId, m_frameId,
par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform( m_cameraFrameId, m_frameId,
par.currentTime, ocToCamTf );
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM( m_name << ": Transform error - " << ex.what() << ", quitting callback");
PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_frameId );
return;
}
m_to_sensor = ocToCamTf;
// PERROR( "Camera position: " << m_camToOcTrans );
}
// Store camera information
m_camera_size = m_camera_size_buffer;
m_camera_model.fromCameraInfo( m_camera_info_buffer);
m_octomap_updates_msg->camera_info = m_camera_info_buffer;
m_octomap_updates_msg->pointcloud2.header.stamp = par.currentTime;
// Majkl 2013/01/24: missing frame id in the message header
m_octomap_updates_msg->pointcloud2.header.frame_id = par.frameId;
// Initialize leaf iterators
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
// 2013/01/31 Majkl
m_data->header.frame_id = par.frameId;
m_data->header.stamp = par.currentTime;
m_DataTimeStamp = par.currentTime;
lock.unlock();
invalidate();
}
void srs_env_model::CLimitedPointCloudPlugin::newMapDataCB( SMapWithParameters & par )
{
// Reset counters
m_countVisible = m_countHidden = 0;
min = 10000000;
max = -10000000;
if( m_cameraFrameId.size() == 0 )
{
m_bTransformCamera = false;
return;
}
if( ! m_publishPointCloud )
return;
// Just for sure
if(m_frame_id != par.frameId)
{
PERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB.");
return;
}
// Clear data
m_data->clear();
m_DataTimeStamp = m_time_stamp = par.currentTime;
counter = 0;
// Pointcloud is used as output for octomap...
m_bAsInput = false;
// m_bTransformCamera = m_cameraFrameId != m_ocFrameId;
bool m_bTransformCamera = m_cameraFrameId != m_frame_id;
// If different frame id
if( m_bTransformCamera )
{
// Some transforms
tf::StampedTransform camToOcTf;
// Get transforms
try {
// Transformation - from, to, time, waiting time
m_tfListener.waitForTransform(m_frame_id, m_cameraFrameId,
par.currentTime, ros::Duration(5));
m_tfListener.lookupTransform(m_frame_id, m_cameraFrameId,
par.currentTime, camToOcTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM( m_name << ": Transform error - " << ex.what() << ", quitting callback");
PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_frame_id );
return;
}
// PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_ocFrameId );
// Get transformation matrix
Eigen::Matrix4f cameraToOcTM;
pcl_ros::transformAsMatrix(camToOcTf, cameraToOcTM); // Sensor TF to defined base TF
// Get translation and rotation
m_camToOcRot = cameraToOcTM.block<3, 3> (0, 0);
m_camToOcTrans = cameraToOcTM.block<3, 1> (0, 3);
// PERROR( "Camera position: " << m_camToOcTrans );
}
// Copy buffered camera normal and d parameter
boost::recursive_mutex::scoped_lock lock( m_camPosMutex );
// PERROR( "Copy position...");
m_d = m_dBuf;
m_normal = m_normalBuf;
tButServerOcTree & tree( par.map->getTree() );
srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() );
// Crawl through nodes
for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it)
{
// Node is occupied?
if (tree.isNodeOccupied(*it))
{
handleOccupiedNode(it, par);
}// Node is occupied?
} // Iterate through octree
// 2013/01/31 Majkl
m_data->header.frame_id = par.frameId;
m_data->header.stamp = par.currentTime;
m_DataTimeStamp = par.currentTime;
lock.unlock();
invalidate();
}
