#include <cmap_plugin.h>

Inheritance diagram for srs_env_model::CCMapPlugin:

Public Member Functions
	CCMapPlugin (const std::string &name)
	Constructor.
void	enable (bool enabled)
	Enable or disable publishing.
virtual void	init (ros::NodeHandle &node_handle)
	Initialize plugin - called in server constructor.
bool	lockChanges (bool bLock)
	Enable/disable collision map changes.
virtual void	pause (bool bPause, ros::NodeHandle &node_handle)
	Connect/disconnect plugin to/from all topics.
Protected Types
typedef arm_navigation_msgs::OrientedBoundingBox	tBox
	Box type.
typedef geometry_msgs::Point32	tBoxPoint
	Point type.
typedef arm_navigation_msgs::CollisionMap::_boxes_type	tBoxVec
	Boxes vector type.
typedef COctomapCrawlerBase < tButServerOcTree::NodeType >	tOctomapCrawler
	Crawler type.
Protected Member Functions
void	addBox (const tBoxPoint &center, const tBoxPoint &size, tBoxVec &boxes)
	Adds box to the collision map.
bool	addBoxCallback (srs_env_model::RemoveCube::Request &req, srs_env_model::RemoveCube::Response &res)
	Remove all boxes from cubical volume - service callback function.
bool	getCollisionMapSrvCallback (srs_env_model::GetCollisionMap::Request &req, srs_env_model::GetCollisionMap::Response &res)
	Get collision map service call.
void	handleOccupiedNode (tButServerOcTree::iterator &it, const SMapWithParameters &mp)
	hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
bool	isNearRobot (const btVector3 &point, double extent)
	Test collision point if it is in the collision distance from the robot.
bool	isNewCmapSrvCallback (srs_env_model::IsNewCollisionMap::Request &req, srs_env_model::IsNewCollisionMap::Response &res)
	Get true if given timestamp is older then current map time.
bool	lockCmapSrvCallback (srs_env_model::LockCollisionMap::Request &req, srs_env_model::LockCollisionMap::Response &res)
	Lock collision map - disable its updates from new point cloud data.
virtual void	newMapDataCB (SMapWithParameters &par)
	Set used octomap frame id and timestamp.
virtual void	publishInternal (const ros::Time &timestamp)
	Called when new scan was inserted and now all can be published.
bool	removeBoxCallback (srs_env_model::RemoveCube::Request &req, srs_env_model::RemoveCube::Response &res)
	Remove all boxes from cubical volume - service callback function.
long	removeInsideBox (const tBoxPoint &center, const tBoxPoint &size, tBoxVec &boxes)
	Remove all collision boxes within given box. Box is aligned with axes and uses the same frame id.
void	retransformTF (const ros::Time &currentTime)
	retransform map to the new time frame
bool	sameCMaps (arm_navigation_msgs::CollisionMap map1, arm_navigation_msgs::CollisionMap map2)
	Compare two collision maps and test if they are the same.
virtual bool	shouldPublish ()
	Is something to publish and some subscriber to publish to?
Protected Attributes
bool	m_bConvertPoint
	Does need input point to be converted?
bool	m_bLocked
	Are cmap changes enabled now.
std::string	m_cmapFrameId
	Collision map frame id.
ros::Publisher	m_cmapPublisher
	Publisher.
std::string	m_cmapPublisherName
	Collision map publisher name.
double	m_collisionMapLimitRadius
	Collision map distance limit.
long int	m_collisionMapVersion
	Collision map version counter.
tDataPtr	m_dataBuffer
	Collision map message buffer - used to resolve if collision map has changed.
arm_navigation_msgs::CollisionMap	m_dataEmpty
	Empty collision map - used when callers map id is the same as the current map id.
bool	m_latchedTopics
ros::Time	m_mapTime
	Current cmap timestamp.
bool	m_publishCollisionMap
	Is publishing enabled?
tf::Stamped< btVector3 >	m_robotBasePosition
	Robot position in the octomap coordinate system.
ros::ServiceServer	m_serviceAddCube
	Add cube to cmap service.
ros::ServiceServer	m_serviceGetCollisionMap
	Get current collision map service.
ros::ServiceServer	m_serviceIsNewCMap
	Is new cmap service.
ros::ServiceServer	m_serviceLockCMap
	Lock collision map service.
ros::ServiceServer	m_serviceRemoveCube
	Remove cube from map service.
tf::TransformListener	m_tfListener
	Transform listener.
Eigen::Matrix3f	m_worldToCMapRot
	World to cmap translation and rotation.
Eigen::Matrix4f	m_worldToCMapTM
	World to collision map transform matrix.
Eigen::Vector3f	m_worldToCMapTrans

Detailed Description

Definition at line 44 of file cmap_plugin.h.

Member Typedef Documentation

typedef arm_navigation_msgs::OrientedBoundingBox srs_env_model::CCMapPlugin::tBox [protected]

Box type.

Definition at line 54 of file cmap_plugin.h.

typedef geometry_msgs::Point32 srs_env_model::CCMapPlugin::tBoxPoint [protected]

Point type.

Definition at line 57 of file cmap_plugin.h.

typedef arm_navigation_msgs::CollisionMap::_boxes_type srs_env_model::CCMapPlugin::tBoxVec [protected]

Boxes vector type.

Definition at line 51 of file cmap_plugin.h.

typedef COctomapCrawlerBase<tButServerOcTree::NodeType> srs_env_model::CCMapPlugin::tOctomapCrawler [protected]

Crawler type.

Definition at line 48 of file cmap_plugin.h.

Constructor & Destructor Documentation

srs_env_model::CCMapPlugin::CCMapPlugin ( const std::string & name )

Constructor.

Definition at line 35 of file cmap_plugin.cpp.

Member Function Documentation

void srs_env_model::CCMapPlugin::addBox	(	const tBoxPoint &	center,
		const tBoxPoint &	size,
		tBoxVec &	boxes
	)		`[protected]`

Adds box to the collision map.

Parameters:

center	Box center
size	Box size

Definition at line 481 of file cmap_plugin.cpp.

bool srs_env_model::CCMapPlugin::addBoxCallback	(	srs_env_model::RemoveCube::Request &	req,
		srs_env_model::RemoveCube::Response &	res
	)		`[protected]`

Remove all boxes from cubical volume - service callback function.

Parameters:

req	Request
res	Response

Definition at line 506 of file cmap_plugin.cpp.

void srs_env_model::CCMapPlugin::enable ( bool enabled ) [inline]

Enable or disable publishing.

Definition at line 64 of file cmap_plugin.h.

bool srs_env_model::CCMapPlugin::getCollisionMapSrvCallback	(	srs_env_model::GetCollisionMap::Request &	req,
		srs_env_model::GetCollisionMap::Response &	res
	)		`[protected]`

Get collision map service call.

Parameters:

req	request - caller's map version
res	response - current map and current version

Definition at line 308 of file cmap_plugin.cpp.

void srs_env_model::CCMapPlugin::handleOccupiedNode	(	tButServerOcTree::iterator &	it,
		const SMapWithParameters &	mp
	)		`[protected]`

hook that is called when traversing occupied nodes of the updated Octree (does nothing here)

Definition at line 200 of file cmap_plugin.cpp.

void srs_env_model::CCMapPlugin::init ( ros::NodeHandle & node_handle ) [virtual]

Initialize plugin - called in server constructor.

Reimplemented from srs_env_model::CServerPluginBase.

Definition at line 51 of file cmap_plugin.cpp.

bool srs_env_model::CCMapPlugin::isNearRobot	(	const btVector3 &	point,
		double	extent
	)		`[protected]`

Test collision point if it is in the collision distance from the robot.

Test collision object if it is in the collision distance from the robot.

Parameters:

point	Position of the object center - in the world coordinates
extent	Bounding sphere of the collision object

Returns:

Definition at line 295 of file cmap_plugin.cpp.

bool srs_env_model::CCMapPlugin::isNewCmapSrvCallback	(	srs_env_model::IsNewCollisionMap::Request &	req,
		srs_env_model::IsNewCollisionMap::Response &	res
	)		`[protected]`

Get true if given timestamp is older then current map time.

Parameters:

req	request - caller's map timestamp
res	response - true, if new map and current timestamp

Definition at line 341 of file cmap_plugin.cpp.

bool srs_env_model::CCMapPlugin::lockChanges ( bool bLock )

Enable/disable collision map changes.

bool srs_env_model::CCMapPlugin::lockCmapSrvCallback	(	srs_env_model::LockCollisionMap::Request &	req,
		srs_env_model::LockCollisionMap::Response &	res
	)		`[protected]`