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);