add output port for static transform and broadcastStaticTransform(s) …

…operations
orocos · May 13, 2019 · c0fcf74 · c0fcf74
1 parent d2f30bf
commit c0fcf74
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 2 deletions.
diff --git a/rtt_tf/include/rtt_tf/tf_interface.h b/rtt_tf/include/rtt_tf/tf_interface.h
@@ -18,13 +18,17 @@ namespace rtt_tf {
       lookupTransformAtTime("lookupTransformAtTime"),
       broadcastTransform("broadcastTransform"),
       broadcastTransforms("broadcastTransforms"),
+      broadcastStaticTransform("broadcastStaticTransform"),
+      broadcastStaticTransforms("broadcastStaticTransforms"),
       canTransform("canTransform"),
       canTransformAtTime("canTransformAtTime")
     {
       owner->requires("tf")->addOperationCaller(lookupTransform);
       owner->requires("tf")->addOperationCaller(lookupTransformAtTime);
       owner->requires("tf")->addOperationCaller(broadcastTransform);
       owner->requires("tf")->addOperationCaller(broadcastTransforms);
+      owner->requires("tf")->addOperationCaller(broadcastStaticTransform);
+      owner->requires("tf")->addOperationCaller(broadcastStaticTransforms);
       owner->requires("tf")->addOperationCaller(canTransform);
       owner->requires("tf")->addOperationCaller(canTransformAtTime);
     }
@@ -36,6 +40,8 @@ namespace rtt_tf {
         lookupTransformAtTime.ready() &&
         broadcastTransform.ready() &&
         broadcastTransforms.ready() &&
+        broadcastStaticTransform.ready() &&
+        broadcastStaticTransforms.ready() &&
         canTransform.ready() &&
         canTransformAtTime.ready();
     }
@@ -44,6 +50,8 @@ namespace rtt_tf {
     RTT::OperationCaller<geometry_msgs::TransformStamped(const std::string&, const std::string&, const ros::Time&)> lookupTransformAtTime;
     RTT::OperationCaller<void(const geometry_msgs::TransformStamped&)> broadcastTransform;
     RTT::OperationCaller<void(const std::vector<geometry_msgs::TransformStamped>&)> broadcastTransforms;
+    RTT::OperationCaller<void(const geometry_msgs::TransformStamped&)> broadcastStaticTransform;
+    RTT::OperationCaller<void(const std::vector<geometry_msgs::TransformStamped>&)> broadcastStaticTransforms;
     RTT::OperationCaller<bool(const std::string&, const std::string&)> canTransform;
     RTT::OperationCaller<bool(const std::string&, const std::string&, const ros::Time&)> canTransformAtTime;
   };

diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp
@@ -122,7 +122,15 @@ namespace rtt_tf
       .arg("transform", "[geometry_msgs::TransformStamped]");
 
     service->addOperation("broadcastTransforms", &RTT_TF::broadcastTransforms, this, RTT::OwnThread)
-      .doc("Broadcast a stamped transform immediately.")
+      .doc("Broadcast stamped transforms immediately.")
+      .arg("transforms", "[std::vector<geometry_msgs::TransformStamped>]");
+
+    service->addOperation("broadcastStaticTransform", &RTT_TF::broadcastStaticTransform, this, RTT::OwnThread)
+      .doc("Broadcast a stamped transform as a static transform immediately.")
+      .arg("transform", "[geometry_msgs::TransformStamped]");
+
+    service->addOperation("broadcastStaticTransforms", &RTT_TF::broadcastStaticTransforms, this, RTT::OwnThread)
+      .doc("Broadcast stamped transforms as static transforms immediately.")
       .arg("transforms", "[std::vector<geometry_msgs::TransformStamped>]");
 
     service->addOperation("canTransform", &RTT_TF::canTransform, this)
@@ -160,7 +168,8 @@ namespace rtt_tf
 
     bool configured = port_tf_static_in.createStream(cp_static)
                     && port_tf_in.createStream(cp)
-                    && port_tf_out.createStream(cp);
+                    && port_tf_out.createStream(cp)
+                    && port_tf_static_out.createStream(cp_static);
 
     if (!configured) {
       cleanupHook();
@@ -212,6 +221,7 @@ namespace rtt_tf
     port_tf_in.disconnect();
     port_tf_out.disconnect();
     port_tf_static_in.disconnect();
+    port_tf_static_out.disconnect();
   }
 
   ros::Time RTT_TF::getLatestCommonTime(
@@ -271,6 +281,19 @@ namespace rtt_tf
     port_tf_out.write(msg_out);
   }
 
+  void RTT_TF::broadcastStaticTransform(const geometry_msgs::TransformStamped& tform)
+  {
+    const std::vector<geometry_msgs::TransformStamped> tforms(1, tform);
+    tf2_msgs::TFMessage msg_out = transformsToMessage(tforms, prop_tf_prefix);
+    port_tf_static_out.write(msg_out);
+  }
+
+  void RTT_TF::broadcastStaticTransforms(const std::vector<geometry_msgs::TransformStamped>& tform)
+  {
+    tf2_msgs::TFMessage msg_out = transformsToMessage(tform, prop_tf_prefix);
+    port_tf_static_out.write(msg_out);
+  }
+
 }//namespace
 
 /*

diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp
@@ -19,6 +19,7 @@ namespace rtt_tf
     RTT::InputPort<tf2_msgs::TFMessage> port_tf_in;
     RTT::InputPort<tf2_msgs::TFMessage> port_tf_static_in;
     RTT::OutputPort<tf2_msgs::TFMessage> port_tf_out;
+    RTT::OutputPort<tf2_msgs::TFMessage> port_tf_static_out;
 
     void internalUpdate(
         tf2_msgs::TFMessage& msg,
@@ -53,6 +54,12 @@ namespace rtt_tf
     void broadcastTransforms(
         const std::vector<geometry_msgs::TransformStamped> &tforms);
 
+    void broadcastStaticTransform(
+        const geometry_msgs::TransformStamped &tform);
+
+    void broadcastStaticTransforms(
+        const std::vector<geometry_msgs::TransformStamped> &tforms);
+
     void addTFOperations(RTT::Service::shared_ptr service);
 
   public: