C++ (Cpp) sameCMaps Examples

Example #1

File: cmap_plugin.cpp Project: beds-tao/srs_public

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

Example #2

File: cmap_plugin.cpp Project: vstancl/srs_public

//! Called when new scan was inserted and now all can be published
void srs_env_model::CCMapPlugin::publishInternal(const ros::Time & timestamp)
{
	// Lock data
	boost::mutex::scoped_lock lock(m_lockData);

	// 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.get(), m_dataBuffer.get() ) )
	{
		// CMaps differs, increase version index and swap them
		++m_collisionMapVersion;
		m_mapTime = timestamp;
		swap( m_data, m_dataBuffer );

		lock.unlock();

		// Call invalidation
		invalidate();
	}

	if( m_bLocked )
	{
		if( m_mapTime != timestamp )
		{
			retransformTF(timestamp);
		}
	}

	// 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 = timestamp;
		m_cmapPublisher.publish(*m_data);
	}
}