C++ (Cpp) robotBaseFrameId Examples

Example #1

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

//! Set used octomap frame id and timestamp
void srs_env_model::CCMapPlugin::onFrameStart( const SMapParameters & par )
{
	if( m_bLocked )
		return;

	// store parameters
	tOctomapCrawler::onFrameStart( par );

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

}

Example #2

File: cmap_plugin.cpp Project: vstancl/srs_public

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