#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.

Member Data Documentation

boost::scoped_ptr<AdvancedChainFkSolverVel_recursive> DistanceManager::adv_chn_fk_solver_vel_

private

Definition at line 65 of file distance_manager.hpp.

KDL::Chain DistanceManager::chain_

private

Definition at line 66 of file distance_manager.hpp.

std::string DistanceManager::chain_base_link_

private

Definition at line 54 of file distance_manager.hpp.

std::string DistanceManager::chain_tip_link_

private

Definition at line 55 of file distance_manager.hpp.

std::vector<std::string> DistanceManager::joints_

private

Definition at line 74 of file distance_manager.hpp.

KDL::JntArray DistanceManager::last_q_

private

Definition at line 76 of file distance_manager.hpp.

KDL::JntArray DistanceManager::last_q_dot_

private

Definition at line 77 of file distance_manager.hpp.

LinkToCollision DistanceManager::link_to_collision_

private

Definition at line 79 of file distance_manager.hpp.

ros::Publisher DistanceManager::marker_pub_

private

Definition at line 69 of file distance_manager.hpp.

std::mutex DistanceManager::mtx_

private

Definition at line 61 of file distance_manager.hpp.

ros::NodeHandle& DistanceManager::nh_

private

Definition at line 68 of file distance_manager.hpp.

boost::scoped_ptr<ShapesManager> DistanceManager::object_of_interest_mgr_

private

Definition at line 58 of file distance_manager.hpp.

ros::Publisher DistanceManager::obstacle_distances_pub_

private

Definition at line 70 of file distance_manager.hpp.

boost::scoped_ptr<ShapesManager> DistanceManager::obstacle_mgr_

private

Definition at line 57 of file distance_manager.hpp.

std::mutex DistanceManager::obstacle_mgr_mtx_

private

Definition at line 62 of file distance_manager.hpp.

std::string DistanceManager::root_frame_id_

private

Definition at line 53 of file distance_manager.hpp.

std::vector<std::string> DistanceManager::segments_

private

Definition at line 75 of file distance_manager.hpp.

std::vector<std::thread> DistanceManager::self_collision_transform_threads_

private

Definition at line 60 of file distance_manager.hpp.

uint32_t DistanceManager::seq_nr_ = 0

staticprivate

Definition at line 81 of file distance_manager.hpp.

bool DistanceManager::stop_sca_threads_

private

Definition at line 63 of file distance_manager.hpp.

Eigen::Affine3d DistanceManager::tf_cb_frame_bl_

private

Definition at line 72 of file distance_manager.hpp.

tf::TransformListener DistanceManager::tf_listener_

private

Definition at line 71 of file distance_manager.hpp.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation