#include <distance_manager.hpp>

Public Member Functions
void	addObjectOfInterest (const std::string &id, PtrIMarkerShape_t s)
void	addObstacle (const std::string &id, PtrIMarkerShape_t s)
void	calculate ()
void	clear ()
	DistanceManager (ros::NodeHandle &nh)
void	drawObjectsOfInterest ()
void	drawObstacles ()
const std::string	getRootFrame () const
Eigen::Affine3d	getSynchedCbToBlTransform ()
int	init ()
void	jointstateCb (const sensor_msgs::JointState::ConstPtr &msg)
bool	registerLinkOfInterest (cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
void	registerObstacle (const moveit_msgs::CollisionObject::ConstPtr &msg)
void	transform ()
void	transformSelfCollisionLinks (const std::string link_name)
	~DistanceManager ()
Private Member Functions
void	buildObstacleMesh (const moveit_msgs::CollisionObject::ConstPtr &msg, const tf::StampedTransform &transform)
void	buildObstaclePrimitive (const moveit_msgs::CollisionObject::ConstPtr &msg, const tf::StampedTransform &transform)
Private Attributes
boost::scoped_ptr < AdvancedChainFkSolverVel_recursive >	adv_chn_fk_solver_vel_
KDL::Chain	chain_
std::string	chain_base_link_
std::string	chain_tip_link_
std::vector< std::string >	joints_
KDL::JntArray	last_q_
KDL::JntArray	last_q_dot_
LinkToCollision	link_to_collision_
ros::Publisher	marker_pub_
std::mutex	mtx_
ros::NodeHandle &	nh_
boost::scoped_ptr< ShapesManager >	object_of_interest_mgr_
ros::Publisher	obstacle_distances_pub_
boost::scoped_ptr< ShapesManager >	obstacle_mgr_
std::mutex	obstacle_mgr_mtx_
std::string	root_frame_id_
std::vector< std::string >	segments_
std::vector< std::thread >	self_collision_transform_threads_
bool	stop_sca_threads_
Eigen::Affine3d	tf_cb_frame_bl_
tf::TransformListener	tf_listener_
Static Private Attributes
static uint32_t	seq_nr_ = 0

Detailed Description

Definition at line 50 of file distance_manager.hpp.

Constructor & Destructor Documentation

DistanceManager::DistanceManager ( ros::NodeHandle & nh )

Parameters:

nh	Reference to the ROS node handle.

Definition at line 62 of file distance_manager.cpp.

DistanceManager::~DistanceManager ( )

Definition at line 65 of file distance_manager.cpp.

Member Function Documentation

void DistanceManager::addObjectOfInterest	(	const std::string &	id,
		PtrIMarkerShape_t	s
	)

Add a new object of interest that shall be investigated for collisions.

Parameters:

s	Pointer to an already created MarkerShape that represent the object of interest (i.e. shape in reference frame of segment).

Definition at line 177 of file distance_manager.cpp.

void DistanceManager::addObstacle	(	const std::string &	id,
		PtrIMarkerShape_t	s
	)

Add a new obstacle to the obstacles that shall be managed.

Parameters:

s	Pointer to an already created MarkerShape that represent an obstacle.

Definition at line 171 of file distance_manager.cpp.

void DistanceManager::buildObstacleMesh	(	const moveit_msgs::CollisionObject::ConstPtr &	msg,
		const tf::StampedTransform &	transform
	)		`[private]`

Build an obstacle from a message containing a mesh.

Parameters:

msg	Msg struct that contains mesh info.
transform	The transformation from a frame in msg header to root_frame_id.

Definition at line 452 of file distance_manager.cpp.

void DistanceManager::buildObstaclePrimitive	(	const moveit_msgs::CollisionObject::ConstPtr &	msg,
		const tf::StampedTransform &	transform
	)		`[private]`

Build an obstacle from a message containing a primitive shape.

Parameters:

msg	Msg struct that contains mesh info.
transform	The transformation from a frame in msg header to root_frame_id.

Definition at line 525 of file distance_manager.cpp.

void DistanceManager::calculate ( )

Calculate the distances between the objects of interest (reference frames at KDL::segments) and obstacles. Publishes them on the obstacle_distance topic according to robot_namespace (arm_right, arm_left, ...)

Definition at line 195 of file distance_manager.cpp.

void DistanceManager::clear ( )

Clears all managed obstacles and objects of interest.

Definition at line 157 of file distance_manager.cpp.

void DistanceManager::drawObjectsOfInterest ( )

Simply draw all object of interest markers in RVIZ.

Definition at line 189 of file distance_manager.cpp.

void DistanceManager::drawObstacles ( )

Simply draw all obstacle markers in RVIZ.

Definition at line 183 of file distance_manager.cpp.

const std::string DistanceManager::getRootFrame ( ) const [inline]

Definition at line 105 of file distance_manager.hpp.

Eigen::Affine3d DistanceManager::getSynchedCbToBlTransform ( )

Get method with mutex access on transform data.

Returns:: Inverse transformation between chain base and base link.

Definition at line 644 of file distance_manager.cpp.

int DistanceManager::init ( )

Initialization of ROS robot structure, parameters, publishers and subscribers.

Returns:: Error status. If 0 then success.

Definition at line 70 of file distance_manager.cpp.

void DistanceManager::jointstateCb ( const sensor_msgs::JointState::ConstPtr & msg )

Updates the joint states.

Parameters:

msg	Joint state message.

Definition at line 372 of file distance_manager.cpp.

bool DistanceManager::registerLinkOfInterest	(	cob_srvs::SetString::Request &	request,
		cob_srvs::SetString::Response &	response
	)

Registers a new link of interest for distance computation.

Parameters:

request	The service request for registration of a new link of interest (e.g. link name)
response	Success message.

Returns:: Registration service call successfull or not.

Definition at line 604 of file distance_manager.cpp.

void DistanceManager::registerObstacle ( const moveit_msgs::CollisionObject::ConstPtr & msg )

Registers an obstacle via message.

Parameters:

msg	MoveIt CollisionObject message type.

Definition at line 404 of file distance_manager.cpp.

void DistanceManager::transform ( )

tf Transformation thread between chain_base_link (arm_right_base_link or arm_left_base_link) and the root frame (e.g. base_link)). Runs endless.

Definition at line 310 of file distance_manager.cpp.

void DistanceManager::transformSelfCollisionLinks ( const std::string link_name )

Thread that runs endless to listen to transforms for the self collision parts of the robot. This will directly update the self collision obstacle pose.

Parameters:

link_name The link name of the self collision checking part. Similar to link name in URDF.

Definition at line 335 of file distance_manager.cpp.