Helper class to broadcast transform information on TF and joint states. More...

#include <broadcaster.h>

Classes
struct	StaticTransform
	Information for a static transform. More...

Public Member Functions
	RobotBroadcaster (const RobotConstPtr &robot, const std::string &base_frame="world", const std::string &name="robowflex")
	Constructor. Sets up TF2 broadcaster. More...

	~RobotBroadcaster ()
	Destructor. More...

void	start ()
	Begin publishing TF and joint information. More...

void	stop ()
	Stop publishing TF and joint information. More...

void	addStaticTransform (const std::string &name, const std::string &base, const std::string &target, const RobotPose &tf=RobotPose::Identity())
	Add a new static transform to the broadcaster. More...

void	removeStaticTransform (const std::string &name)
	Remove a named static transform. More...

Private Member Functions
void	update ()
	Send out the TF and joint information. More...

Private Attributes
RobotConstPtr	robot_
	Robot being published. More...

const std::string	base_
	Base frame to use. More...

bool	active_ {false}
	Is thread active? More...

bool	done_ {false}
	Is thread done? More...

unsigned int	rate_ {10}
	Times per second to send out. More...

std::unique_ptr< std::thread >	thread_
	Worker thread. More...

ros::NodeHandle	nh_
	Handle for publishing. More...

tf2_ros::TransformBroadcaster	tf2br_
	TF2 broadcaster. More...

ros::Publisher	state_pub_
	State publisher. More...

std::map< std::string, StaticTransform >	static_
	Static transforms. More...

Detailed Description

Helper class to broadcast transform information on TF and joint states.

Definition at line 23 of file broadcaster.h.

Constructor & Destructor Documentation

◆ RobotBroadcaster()

IO::RobotBroadcaster::RobotBroadcaster	(	const RobotConstPtr &	robot,
		const std::string &	base_frame = `"world"`,
		const std::string &	name = `"robowflex"`
	)

Constructor. Sets up TF2 broadcaster.

Parameters

[in]	robot	Robot to broadcast.
[in]	base_frame	Base frame to use for robot transforms.
[in]	name	Namespace to use.

Definition at line 20 of file broadcaster.cpp.

   : robot_(robot), base_(base_frame), nh_("/" + name)
 {
     state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
 }

◆ ~RobotBroadcaster()

IO::RobotBroadcaster::~RobotBroadcaster ( )

Destructor.

Definition at line 27 of file broadcaster.cpp.

 {
     active_ = false;
     while (not done_)
     {
         ros::WallDuration pause(1. / rate_);
         pause.sleep();
     }
 }

Member Function Documentation

◆ addStaticTransform()

void IO::RobotBroadcaster::addStaticTransform	(	const std::string &	name,
		const std::string &	base,
		const std::string &	target,
		const RobotPose &	tf = `RobotPose::Identity()`
	)

Add a new static transform to the broadcaster.

Parameters

[in]	name	Name of transform (used for removal)
[in]	base	Base frame of transform.
[in]	target	Target frame of transform.
[in]	tf	The transform from base to target.

Definition at line 60 of file broadcaster.cpp.

 {
     if (static_.find(name) == static_.end())
     {
         StaticTransform stf;
         stf.base = base;
         stf.target = target;
         stf.tf = tf;
  
         static_.emplace(name, stf);
     }
     else
         RBX_ERROR("Static transform %s already in map!", name);
 }

◆ removeStaticTransform()

void IO::RobotBroadcaster::removeStaticTransform ( const std::string & name )

Remove a named static transform.

Parameters

[in] name Name of transform to remove.

Definition at line 76 of file broadcaster.cpp.

 {
     auto it = static_.find(name);
     if (it != static_.end())
         static_.erase(it);
     else
         RBX_ERROR("Static transform %s does not exist in map!", name);
 }

◆ start()

void IO::RobotBroadcaster::start ( void )

Begin publishing TF and joint information.

Definition at line 37 of file broadcaster.cpp.

 {
     active_ = true;
     thread_.reset(new std::thread([&]() {
         done_ = false;
  
         while (active_)
         {
             update();
  
             ros::WallDuration pause(1. / rate_);
             pause.sleep();
         }
  
         done_ = true;
     }));
 }

◆ stop()

void IO::RobotBroadcaster::stop ( void )

Stop publishing TF and joint information.

Definition at line 55 of file broadcaster.cpp.

 {
     active_ = false;
 }

◆ update()

void IO::RobotBroadcaster::update ( void )

private

Send out the TF and joint information.

Definition at line 85 of file broadcaster.cpp.

 {
     const auto &state = robot_->getScratchStateConst();
     const auto &model = robot_->getModelConst();
  
     for (const auto &link : model->getLinkModels())
     {
         const auto &parent = link->getParentLinkModel();
  
         RobotPose tf =
             link->getJointOriginTransform() * state->getJointTransform(link->getParentJointModel());
  
         std::string source = (parent) ? parent->getName() : base_;
         const std::string &target = link->getName();
  
         auto msg = TF::transformEigenToMsg(source, target, tf);
  
         tf2br_.sendTransform(msg);
     }
  
     for (const auto &pair : static_)
     {
         const auto &stf = pair.second;
         auto static_msg = TF::transformEigenToMsg(stf.base, stf.target, stf.tf);
         tf2br_.sendTransform(static_msg);
     }
  
     unsigned int n = state->getVariableCount();
  
     sensor_msgs::JointState msg;
     msg.header.stamp = ros::Time::now();
     msg.name = state->getVariableNames();
  
     arrayToVector(state->getVariablePositions(), n, msg.position);
     arrayToVector(state->getVariableVelocities(), n, msg.velocity);
     arrayToVector(state->getVariableEffort(), n, msg.effort);
  
     state_pub_.publish(msg);
 }

Member Data Documentation

◆ active_

bool robowflex::IO::RobotBroadcaster::active_ {false}

private

Is thread active?

Definition at line 67 of file broadcaster.h.

◆ base_

const std::string robowflex::IO::RobotBroadcaster::base_

private

Base frame to use.

Definition at line 66 of file broadcaster.h.

◆ done_

bool robowflex::IO::RobotBroadcaster::done_ {false}

private

Is thread done?

Definition at line 68 of file broadcaster.h.

◆ nh_

ros::NodeHandle robowflex::IO::RobotBroadcaster::nh_

private

Handle for publishing.

Definition at line 71 of file broadcaster.h.

◆ rate_

unsigned int robowflex::IO::RobotBroadcaster::rate_ {10}

private

Times per second to send out.

Definition at line 69 of file broadcaster.h.

◆ robot_

RobotConstPtr robowflex::IO::RobotBroadcaster::robot_

private

Robot being published.

Definition at line 65 of file broadcaster.h.

◆ state_pub_

ros::Publisher robowflex::IO::RobotBroadcaster::state_pub_

private

State publisher.

Definition at line 73 of file broadcaster.h.

◆ static_

std::map<std::string, StaticTransform> robowflex::IO::RobotBroadcaster::static_

private

Static transforms.

Definition at line 84 of file broadcaster.h.

◆ tf2br_

tf2_ros::TransformBroadcaster robowflex::IO::RobotBroadcaster::tf2br_

private

TF2 broadcaster.

Definition at line 72 of file broadcaster.h.

◆ thread_

std::unique_ptr<std::thread> robowflex::IO::RobotBroadcaster::thread_

private

Worker thread.

Definition at line 70 of file broadcaster.h.

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

Classes

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RobotBroadcaster()

◆ ~RobotBroadcaster()

Member Function Documentation

◆ addStaticTransform()

◆ removeStaticTransform()

◆ start()

◆ stop()

◆ update()

Member Data Documentation

◆ active_

◆ base_

◆ done_

◆ nh_

◆ rate_

◆ robot_

◆ state_pub_

◆ static_

◆ tf2br_

◆ thread_