void srs_env_model::CCollisionGridPlugin::publishInternal(const ros::Time & timestamp)
{
boost::mutex::scoped_lock lock(m_lockData);
if( shouldPublish() )
m_gridPublisher.publish(*m_data);
}
//! Called when new scan was inserted and now all can be published
void srs_env_model::CPointCloudPlugin::publishInternal(const ros::Time & timestamp)
{
// PERROR("Try lock");
boost::mutex::scoped_lock lock(m_lockData);
// PERROR("Lock");
// No subscriber or disabled
if( ! shouldPublish() )
return;
// No data...
if( m_data->size() == 0 )
return;
// Convert data
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg< tPclPoint >(*m_data, cloud);
// Set message parameters and publish
cloud.header.frame_id = m_pcFrameId;
cloud.header.stamp = timestamp;
// PERROR( "Publishing cloud. Size: " << m_data->size() << ", topic: " << m_pcPublisher.getTopic() );
m_pcPublisher.publish(cloud);
// PERROR("Unlock");
}
//! Called when new scan was inserted and now all can be published
void srs_env_model::CCMapPlugin::onPublish(const ros::Time & timestamp)
{
if( ! shouldPublish() )
return;
// Should map be published?
bool publishCollisionMap = m_publishCollisionMap && (m_latchedTopics || m_cmapPublisher.getNumSubscribers() > 0);
// Test collision maps and swap them, if needed
if( ! sameCMaps( m_data, m_dataBuffer ) )
{
// CMaps differs, increase version index and swap them
++m_collisionMapVersion;
m_mapTime = timestamp;
swap( m_data, m_dataBuffer );
// Call invalidation
invalidate();
}
// Publish collision map
if (publishCollisionMap) {
// std::cerr << "Publishing cmap. Frame id: " << m_cmapFrameId << ", Size: " << m_data->boxes.size() << std::endl;
m_data->header.frame_id = m_cmapFrameId;
m_data->header.stamp = m_time_stamp;
m_cmapPublisher.publish(*m_data);
}
}
//! Called when new scan was inserted and now all can be published
void srs_env_model::CCompressedPointCloudPlugin::publishInternal(const ros::Time & timestamp)
{
// ROS_DEBUG( "CCompressedPointCloudPlugin: onPublish" );
// PERROR( "Visible: " << m_countVisible << ", all: " << m_countAll << ", compression: " << double(m_countVisible)/double(m_countAll) );
// PERROR( "Num of points: " << m_data->size() );
// srs_env_model::CPointCloudPlugin::publishInternal( timestamp );
if( shouldPublish() )
{
// Fill header information
m_octomap_updates_msg->header = m_data->header;
// Convert data
pcl::toROSMsg< tPclPoint >(*m_data, m_octomap_updates_msg->pointcloud2);
// Set message parameters and publish
m_octomap_updates_msg->pointcloud2.header.frame_id = m_pcFrameId;
// m_octomap_updates_msg->pointcloud2.header.stamp = timestamp;
if( m_bPublishComplete )
{
// std::cerr << "Publishing complete cloud." << std::endl;
m_octomap_updates_msg->isPartial = 0;
}
else
{
// std::cerr << "Publishing only differential cloud." << std::endl;
m_octomap_updates_msg->isPartial = 1;
}
// Publish data
m_ocUpdatePublisher.publish( m_octomap_updates_msg );
}
}
/**
* Publishing function
*/
void srs_env_model::COctoMapPlugin::publishInternal(const ros::Time & timestamp)
{
if( !shouldPublish() )
return;
// Lock data
// PERROR( "publish: Try lock");
boost::mutex::scoped_lock lock(m_lockData);
// PERROR( "publish: Locked");
octomap_ros::OctomapBinary map;
map.header.frame_id = m_mapParameters.frameId;
map.header.stamp = timestamp;
octomap::octomapMapToMsgData(m_data->getTree(), map.data);
m_ocPublisher.publish(map);
// PERROR( "publish: Unlocked");
}
void srs_env_model::CMap2DPlugin::publishInternal(const ros::Time & timestamp)
{
if( shouldPublish() )
m_map2DPublisher.publish(*m_data);
}
