rtt_tf: fix transformation from TF to geometry msgs

This patch fixes a memory bug with the transfromations from
geometry messages to TF.
The tests implemented work properly.

Author: spd-intermodalics

rtt_tf/rtt_tf-component.cpp

@@ -80,7 +80,7 @@ namespace rtt_tf
   {
     tf2_msgs::TFMessage msg;
     // resolve names and copy transforms to message
-    msg.transforms.reserve(tforms.size());
+    msg.transforms.resize(tforms.size());
     std::transform(tforms.begin(), tforms.end(), msg.transforms.begin(), PrefixResolver(prefix));
     return msg;
   }
@@ -349,13 +349,17 @@ namespace rtt_tf
 
 void RTT_TF::listTrackers()
 {
-  RTT::log(RTT::Info) << "Listing existing trackers" << RTT::endlog();
+  Logger::In(this->getName());
+  const RTT::Logger::LogLevel config_level = RTT::Logger::Instance()->getLogLevel();
+  RTT::Logger::Instance()->setLogLevel(RTT::Logger::LogLevel::Info);
+  RTT::log() << "Listing existing trackers" << RTT::endlog();
   RTT::log(RTT::Info) << "(Source) <-> (Target) : [PortName]" << RTT::endlog();
   RTT::log(RTT::Info) << "----------------------------------" << RTT::endlog();
   for (const auto& entry : ports_trackers) {
     RTT::log(RTT::Info) << entry.first.second << " <-> " << entry.first.first <<
       " : [" <<entry.second->getName() << "]" << RTT::endlog();
   }
+  RTT::Logger::Instance()->setLogLevel(config_level);
 }
 }//namespace
 

rtt_tf/tests/broadcaster_test.cpp

@@ -46,8 +46,8 @@ class BroadcasterComponent : public RTT::TaskContext {
     tform_.header.frame_id = "/world";
     tform_.header.stamp = ros::Time(((double)RTT::os::TimeService::Instance()->getNSecs())*1E-9);
     tform_.child_frame_id = "rel_rtt_tf_test";
-    tform_.transform.translation.x = cos(t);
-    tform_.transform.translation.y = sin(t);
+    tform_.transform.translation.x = sin(t);
+    tform_.transform.translation.y = cos(t);
     broadcaster_(tform_);
   }
   void stopHook() { }
@@ -78,6 +78,7 @@ int ORO_main(int argc, char** argv) {
     // Connect components and deployer
     deployer.connectPeers(&bcaster);
     deployer.setActivity("bcaster",0.02,0,ORO_SCHED_OTHER);
+    // deployer.setActivity("tf",0.1,0,ORO_SCHED_OTHER);
 
     // Connect services between the tf component and the broadcaster
     bcaster.connectServices(tf_component);

rtt_tf/tests/lookup_test.cpp

@@ -33,7 +33,7 @@ class LookupComponent : public RTT::TaskContext {
   }
   virtual ~LookupComponent()  { }
   bool configureHook() { 
-    subscribe_transform_("/world","/rtt_tf_test");
+    subscribe_transform_("rel_rtt_tf_test","rtt_tf_test");
     return true;
   }
   bool startHook() {
@@ -75,6 +75,7 @@ int ORO_main(int argc, char** argv) {
     // Connect components and deployer
     deployer.connectPeers(&lookup);
     deployer.setActivity("lookup",0.02,0,ORO_SCHED_OTHER);
+    deployer.setActivity("tf",0.1,0,ORO_SCHED_OTHER);
 
     // Connect services between the tf component and the broadcaster
     lookup.connectServices(tf_component);