58 #include "robot_state_publisher_thread.h" 59 #include <kdl/frames_io.hpp> 60 #include <kdl_parser/kdl_parser.h> 65 #define CFG_PREFIX "/robot_state_publisher/" 77 :
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_)) {
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());
132 for (map<string, SegmentPair>::const_iterator it = unknown_segments.begin();
133 it != unknown_segments.end(); it++) {
148 for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); it++) {
155 publish_fixed_transforms();
160 void RobotStatePublisherThread::add_children(
const KDL::SegmentMap::const_iterator segment)
162 const std::string& root = segment->second.segment.getName();
164 const std::vector<KDL::SegmentMap::const_iterator>& children = segment->second.children;
165 for (
unsigned int i=0; i<children.size(); i++){
166 const KDL::Segment& child = children[i]->second.segment;
167 SegmentPair s(children[i]->second.segment, root, child.getName());
168 if (child.getJoint().getType() == KDL::Joint::None){
169 segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
170 logger->
log_debug(
name(),
"Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
173 segments_.insert(make_pair(child.getJoint().getName(), s));
174 logger->
log_debug(
name(),
"Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
176 add_children(children[i]);
181 void RobotStatePublisherThread::publish_fixed_transforms()
183 std::vector<tf::StampedTransform> tf_transforms;
186 tf_transform.
stamp = now + cfg_postdate_to_future_;
189 for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
190 transform_kdl_to_tf(seg->second.segment.pose(0), tf_transform);
191 tf_transform.
frame_id = seg->second.root;
193 tf_transforms.push_back(tf_transform);
195 for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin();
196 it != tf_transforms.end(); it++) {
201 void RobotStatePublisherThread::transform_kdl_to_tf(
const KDL::Frame &k, fawkes::tf::Transform &t)
203 t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
204 t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
205 k.M.data[3], k.M.data[4], k.M.data[5],
206 k.M.data[6], k.M.data[7], k.M.data[8]));
213 bool RobotStatePublisherThread::joint_is_in_model(
const char *
id) {
214 return (segments_.find(
id) != segments_.end());
221 if (strncmp(type,
"JointInterface", __INTERFACE_TYPE_SIZE) != 0)
return;
222 if (!joint_is_in_model(
id))
return;
225 interface =
blackboard->open_for_reading<JointInterface>(id);
232 ifs_.push_back(interface);
251 unsigned int instance_serial)
254 conditional_close(interface);
260 unsigned int instance_serial)
263 conditional_close(interface);
267 RobotStatePublisherThread::conditional_close(
Interface *interface)
throw()
271 if (! jiface)
return;
273 std::list<JointInterface *>::iterator it;
274 for (it = ifs_.begin(); it != ifs_.end(); ++it) {
275 if (*interface == **it) {
296 std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jiface->
id());
297 if (seg == segments_.end())
return;
300 transform.
frame_id = seg->second.root;
302 transform_kdl_to_tf(seg->second.segment.pose(jiface->
position()), transform);
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
float position() const
Get position value.
Fawkes library namespace.
This class represents the segment between a parent and a child joint.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
const char * id() const
Get identifier of interface.
A class for handling time.
virtual const char * what() const
Get primary string.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Base class for all Fawkes BlackBoard interfaces.
void bbil_remove_writer_interface(Interface *interface)
Remove an interface to the writer addition/removal watch list.
Logger * logger
This is the Logger member used to access the logger.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
virtual void loop()
Code to execute in the thread.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
bool has_writer() const
Check if there is a writer for the interface.
const char * name() const
Get name of thread.
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.
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.
void bbil_remove_data_interface(Interface *interface)
Remove an interface to the data modification watch list.
RobotStatePublisherThread()
Constructor.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual void init()
Initialize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
JointInterface Fawkes BlackBoard Interface.
unsigned int num_readers() const
Get the number of readers.
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.
void bbil_remove_reader_interface(Interface *interface)
Remove an interface to the reader addition/removal watch list.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.