22 #include "tf_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <ros/this_node.h>
27 using namespace fawkes;
44 __tf_msg_queue_mutex =
new Mutex();
45 __seq_num_mutex =
new Mutex();
51 delete __tf_msg_queue_mutex;
52 delete __seq_num_mutex;
65 __sub_tf =
rosnode->subscribe(
"/tf", 100, &RosTfThread::tf_message_cb,
this);
66 __pub_tf =
rosnode->advertise< ::tf::tfMessage >(
"/tf", 100);
70 std::list<TransformInterface *>::iterator i;
71 std::list<TransformInterface *>::iterator own_if = __tfifs.end();
72 for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
74 if (strcmp((*i)->id(),
"TF ROS") == 0) {
84 if (own_if != __tfifs.end()) __tfifs.erase(own_if);
102 std::list<TransformInterface *>::iterator i;
103 for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
113 __tf_msg_queue_mutex->
lock();
114 unsigned int queue = __active_queue;
115 __active_queue = 1 - __active_queue;
116 __tf_msg_queue_mutex->
unlock();
118 while (! __tf_msg_queues[queue].empty()) {
119 const ::tf::tfMessage::ConstPtr &msg = __tf_msg_queues[queue].front();
120 const size_t tsize = msg->transforms.size();
121 for (
size_t i = 0; i < tsize; ++i) {
122 const geometry_msgs::TransformStamped &ts = msg->transforms[i];
123 const geometry_msgs::Vector3 &t = ts.transform.translation;
124 const geometry_msgs::Quaternion &r = ts.transform.rotation;
126 fawkes::Time time(ts.header.stamp.sec, ts.header.stamp.nsec / 1000);
128 fawkes::tf::Transform tr(fawkes::tf::Quaternion(r.x, r.y, r.z, r.w),
129 fawkes::tf::Vector3(t.x, t.y, t.z));
131 st(tr, time, ts.header.frame_id, ts.child_frame_id);
135 __tf_msg_queues[queue].pop();
150 double *rotation = tfif->
rotation();
153 geometry_msgs::Vector3 t;
154 t.x = translation[0]; t.y = translation[1]; t.z = translation[2];
155 geometry_msgs::Quaternion r;
156 r.x = rotation[0]; r.y = rotation[1]; r.z = rotation[2]; r.w = rotation[3];
157 geometry_msgs::Transform tr;
161 geometry_msgs::TransformStamped ts;
162 __seq_num_mutex->lock();
163 ts.header.seq = ++__seq_num;
164 __seq_num_mutex->unlock();
166 ts.header.frame_id = tfif->
frame();
170 ::tf::tfMessage tmsg;
171 tmsg.transforms.push_back(ts);
173 __pub_tf.publish(tmsg);
180 if (strncmp(type,
"TransformInterface", __INTERFACE_TYPE_SIZE) != 0)
return;
188 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
193 bbil_add_data_interface(tfif);
194 bbil_add_reader_interface(tfif);
195 bbil_add_writer_interface(tfif);
197 __tfifs.push_back(tfif);
199 blackboard->
close(tfif);
200 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
207 unsigned int instance_serial)
throw()
209 conditional_close(interface);
214 unsigned int instance_serial)
throw()
216 conditional_close(interface);
220 RosTfThread::conditional_close(
Interface *interface)
throw()
226 std::list<TransformInterface *>::iterator i;
227 for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
228 if (*interface == **i) {
229 if (! interface->has_writer() && (interface->num_readers() == 1)) {
231 logger->
log_info(name(),
"Last on %s, closing", interface->uid());
232 bbil_remove_data_interface(*i);
233 bbil_remove_reader_interface(*i);
234 bbil_remove_writer_interface(*i);
236 blackboard->
close(*i);
249 RosTfThread::tf_message_cb(const ::tf::tfMessage::ConstPtr &msg)
253 std::map<std::string, std::string> *msg_header_map =
254 msg->__connection_header.get();
255 std::map<std::string, std::string>::iterator it =
256 msg_header_map->find(
"callerid");
258 if (it == msg_header_map->end()) {
260 }
else if (it->second != ros::this_node::getName()) {
261 __tf_msg_queues[__active_queue].push(msg);
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
void unlock()
Unlock the mutex.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
A class for handling time.
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.
Logger * logger
This is the Logger member used to access the logger.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
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 const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
long get_sec() const
Get seconds.
void read()
Read from BlackBoard into local copy.
virtual void finalize()
Finalize the thread.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
RosTfThread()
Constructor.
virtual void loop()
Code to execute in the thread.
long get_nsec() const
Get nanoseconds.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Mutex mutual exclusion lock.
virtual void init()
Initialize the thread.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual ~RosTfThread()
Destructor.
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.