56 #include "robot_state_publisher_thread.h"
58 #include <kdl_parser/kdl_parser.h>
61 #include <kdl/frames_io.hpp>
64 #define CFG_PREFIX "/robot_state_publisher/"
76 :
Thread(
"RobotStatePublisherThread",
Thread::OPMODE_WAITFORWAKEUP),
88 cfg_postdate_to_future_ =
config->
get_float(CFG_PREFIX
"postdate_to_future");
90 cfg_postdate_to_future_ = 0.f;
95 if (cfg_urdf_path_.substr(0, 1) !=
"/") {
97 cfg_urdf_path_.insert(0, RESDIR
"/urdf/");
99 ifstream urdf_file(cfg_urdf_path_);
100 if (!urdf_file.is_open()) {
101 throw Exception(
"Failed to open URDF File %s", cfg_urdf_path_.c_str());
103 while (getline(urdf_file, line)) {
108 if (!kdl_parser::tree_from_string(urdf, tree_)) {
110 throw Exception(
"Failed to parse URDF description");
113 add_children(tree_.getRootSegment());
115 std::map<std::string, SegmentPair> unknown_segments = segments_;
119 for (std::list<JointInterface *>::iterator it = ifs.begin(); it != ifs.end(); ++it) {
120 if (joint_is_in_model((*it)->id())) {
122 unknown_segments.erase((*it)->id());
131 for (map<string, SegmentPair>::const_iterator it = unknown_segments.begin();
132 it != unknown_segments.end();
149 for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); ++it) {
157 publish_fixed_transforms();
162 RobotStatePublisherThread::add_children(
const KDL::SegmentMap::const_iterator segment)
164 const std::string &root = segment->second.segment.getName();
166 const std::vector<KDL::SegmentMap::const_iterator> &children = segment->second.children;
167 for (
unsigned int i = 0; i < children.size(); ++i) {
168 const KDL::Segment &child = children[i]->second.segment;
169 SegmentPair s(children[i]->second.segment, root, child.getName());
170 if (child.getJoint().getType() == KDL::Joint::None) {
171 segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
173 "Adding fixed segment from %s to %s",
175 child.getName().c_str());
177 segments_.insert(make_pair(child.getJoint().getName(), s));
179 "Adding moving segment from %s to %s",
181 child.getName().c_str());
183 add_children(children[i]);
189 RobotStatePublisherThread::publish_fixed_transforms()
191 std::vector<tf::StampedTransform> tf_transforms;
194 tf_transform.
stamp = now + cfg_postdate_to_future_;
197 for (map<string, SegmentPair>::const_iterator seg = segments_fixed_.begin();
198 seg != segments_fixed_.end();
200 transform_kdl_to_tf(seg->second.segment.pose(0), tf_transform);
201 tf_transform.
frame_id = seg->second.root;
203 tf_transforms.push_back(tf_transform);
205 for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin();
206 it != tf_transforms.end();
213 RobotStatePublisherThread::transform_kdl_to_tf(
const KDL::Frame &k, fawkes::tf::Transform &t)
215 t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
216 t.setBasis(tf::Matrix3x3(k.M.data[0],
231 RobotStatePublisherThread::joint_is_in_model(
const char *
id)
233 return (segments_.find(
id) != segments_.end());
240 if (strncmp(type,
"JointInterface", INTERFACE_TYPE_SIZE_) != 0)
242 if (!joint_is_in_model(
id))
248 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
251 logger->
log_debug(name(),
"Found joint information for %s", interface->
id());
253 ifs_.push_back(interface);
254 bbil_add_data_interface(interface);
255 bbil_add_reader_interface(interface);
256 bbil_add_writer_interface(interface);
260 bbil_remove_data_interface(interface);
261 bbil_remove_reader_interface(interface);
262 bbil_remove_writer_interface(interface);
264 blackboard->
close(interface);
265 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
272 Uuid instance_serial) noexcept
274 conditional_close(interface);
279 Uuid instance_serial) noexcept
281 conditional_close(interface);
285 RobotStatePublisherThread::conditional_close(
Interface *interface) noexcept
292 std::list<JointInterface *>::iterator it;
293 for (it = ifs_.begin(); it != ifs_.end(); ++it) {
294 if (*interface == **it) {
295 if (!interface->has_writer() && (interface->num_readers() == 1)) {
297 bbil_remove_data_interface(*it);
298 bbil_remove_reader_interface(*it);
299 bbil_remove_writer_interface(*it);
301 blackboard->
close(*it);
316 std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jiface->
id());
317 if (seg == segments_.end())
321 transform.
frame_id = seg->second.root;
323 transform_kdl_to_tf(seg->second.segment.pose(jiface->
position()), transform);
324 tf_publisher->send_transform(transform);
RobotStatePublisherThread()
Constructor.
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual void init()
Initialize the thread.
This class represents the segment between a parent and a child joint.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
Base class for all Fawkes BlackBoard interfaces.
const char * id() const
Get identifier of interface.
void read()
Read from BlackBoard into local copy.
JointInterface Fawkes BlackBoard Interface.
float position() const
Get position value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
A convenience class for universally unique identifiers (UUIDs).
Fawkes library namespace.