Fawkes API  Fawkes Development Version
tf_thread.cpp
1 
2 /***************************************************************************
3  * tf_thread.cpp - Thread to exchange transforms
4  *
5  * Created: Wed Oct 26 01:02:59 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "tf_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <ros/this_node.h>
26 
27 using namespace fawkes;
28 
29 /** @class RosTfThread "tf_thread.h"
30  * Thread to exchange transforms between Fawkes and ROS.
31  * This threads connects to Fawkes and ROS to read and write transforms.
32  * Transforms received on one end are republished to the other side. To
33  * Fawkes new frames are published during the sensor hook.
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor. */
39  : Thread("RosTfThread", Thread::OPMODE_WAITFORWAKEUP),
40  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
41  TransformAspect(TransformAspect::ONLY_PUBLISHER, "ROS"),
42  BlackBoardInterfaceListener("RosTfThread")
43 {
44  __tf_msg_queue_mutex = new Mutex();
45  __seq_num_mutex = new Mutex();
46 }
47 
48 /** Destructor. */
50 {
51  delete __tf_msg_queue_mutex;
52  delete __seq_num_mutex;
53 }
54 
55 
56 
57 void
59 {
60  __active_queue = 0;
61  __seq_num = 0;
62 
63  // Must do that before registering listener because we might already
64  // get events right away
65  __sub_tf = rosnode->subscribe("/tf", 100, &RosTfThread::tf_message_cb, this);
66  __pub_tf = rosnode->advertise< ::tf::tfMessage >("/tf", 100);
67 
69 
70  std::list<TransformInterface *>::iterator i;
71  std::list<TransformInterface *>::iterator own_if = __tfifs.end();
72  for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
73  //logger->log_info(name(), "Opened %s", (*i)->uid());
74  if (strcmp((*i)->id(), "TF ROS") == 0) {
75  // that's our own Fawkes publisher, do NOT republish what we receive...
76  own_if = i;
77  blackboard->close(*i);
78  } else {
82  }
83  }
84  if (own_if != __tfifs.end()) __tfifs.erase(own_if);
86 
87  bbio_add_observed_create("TransformInterface", "TF *");
89 
90 }
91 
92 
93 void
95 {
98 
99  __sub_tf.shutdown();
100  __pub_tf.shutdown();
101 
102  std::list<TransformInterface *>::iterator i;
103  for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
104  blackboard->close(*i);
105  }
106  __tfifs.clear();
107 }
108 
109 
110 void
112 {
113  __tf_msg_queue_mutex->lock();
114  unsigned int queue = __active_queue;
115  __active_queue = 1 - __active_queue;
116  __tf_msg_queue_mutex->unlock();
117 
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;
125 
126  fawkes::Time time(ts.header.stamp.sec, ts.header.stamp.nsec / 1000);
127 
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);
132 
134  }
135  __tf_msg_queues[queue].pop();
136  }
137 }
138 
139 
140 void
142 {
143  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
144  if (! tfif) return;
145 
146  tfif->read();
147 
148 
149  double *translation = tfif->translation();
150  double *rotation = tfif->rotation();
151  const Time *time = tfif->timestamp();
152 
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;
158  tr.translation = t;
159  tr.rotation = r;
160 
161  geometry_msgs::TransformStamped ts;
162  __seq_num_mutex->lock();
163  ts.header.seq = ++__seq_num;
164  __seq_num_mutex->unlock();
165  ts.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
166  ts.header.frame_id = tfif->frame();
167  ts.child_frame_id = tfif->child_frame();
168  ts.transform = tr;
169 
170  ::tf::tfMessage tmsg;
171  tmsg.transforms.push_back(ts);
172 
173  __pub_tf.publish(tmsg);
174 }
175 
176 
177 void
178 RosTfThread::bb_interface_created(const char *type, const char *id) throw()
179 {
180  if (strncmp(type, "TransformInterface", __INTERFACE_TYPE_SIZE) != 0) return;
181 
182  TransformInterface *tfif;
183  try {
184  //logger->log_info(name(), "Opening %s:%s", type, id);
185  tfif = blackboard->open_for_reading<TransformInterface>(id);
186  } catch (Exception &e) {
187  // ignored
188  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
189  return;
190  }
191 
192  try {
193  bbil_add_data_interface(tfif);
194  bbil_add_reader_interface(tfif);
195  bbil_add_writer_interface(tfif);
196  blackboard->update_listener(this);
197  __tfifs.push_back(tfif);
198  } catch (Exception &e) {
199  blackboard->close(tfif);
200  logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
201  return;
202  }
203 }
204 
205 void
207  unsigned int instance_serial) throw()
208 {
209  conditional_close(interface);
210 }
211 
212 void
214  unsigned int instance_serial) throw()
215 {
216  conditional_close(interface);
217 }
218 
219 void
220 RosTfThread::conditional_close(Interface *interface) throw()
221 {
222  // Verify it's a TransformInterface
223  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
224  if (! tfif) return;
225 
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)) {
230  // It's only us
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);
235  blackboard->update_listener(this);
236  blackboard->close(*i);
237  __tfifs.erase(i);
238  break;
239  }
240  }
241  }
242 }
243 
244 
245 /** Callback function for ROS tf message subscription.
246  * @param msg incoming message
247  */
248 void
249 RosTfThread::tf_message_cb(const ::tf::tfMessage::ConstPtr &msg)
250 {
251  MutexLocker lock(__tf_msg_queue_mutex);
252 
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");
257 
258  if (it == msg_header_map->end()) {
259  logger->log_warn(name(), "Message received without callerid");
260  } else if (it->second != ros::this_node::getName()) {
261  __tf_msg_queues[__active_queue].push(msg);
262  }
263 }
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:211
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:56
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
Definition: tf_thread.cpp:141
double * translation() const
Get translation value.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
Mutex locking helper.
Definition: mutex_locker.h:33
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
A class for handling time.
Definition: time.h:91
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:200
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:186
TransformInterface Fawkes BlackBoard Interface.
virtual void send_transform(const StampedTransform &transform)
Publish transform.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:38
double * rotation() const
Get rotation value.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:691
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:174
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.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
long get_sec() const
Get seconds.
Definition: time.h:110
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
virtual void finalize()
Finalize the thread.
Definition: tf_thread.cpp:94
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:225
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:224
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:203
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
RosTfThread()
Constructor.
Definition: tf_thread.cpp:38
virtual void loop()
Code to execute in the thread.
Definition: tf_thread.cpp:111
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
Definition: tf_thread.cpp:213
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.
Definition: ros.h:48
char * frame() const
Get frame value.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
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.
Definition: tf_thread.cpp:206
Mutex mutual exclusion lock.
Definition: mutex.h:32
virtual void init()
Initialize the thread.
Definition: tf_thread.cpp:58
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
Definition: tf_thread.cpp:178
char * child_frame() const
Get child_frame value.
virtual ~RosTfThread()
Destructor.
Definition: tf_thread.cpp:49
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.