Stamped message handling

canopy-ros · Mar 10, 2016 · 8dd3989 · 8dd3989
1 parent 925a9f5
commit 8dd3989
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 87 deletions.
diff --git a/launch/example.launch~ b/launch/example.launch~
diff --git a/launch/tf_test.launch b/launch/tf_test.launch
@@ -22,12 +22,14 @@
                 - tf2_msgs/TFMessage
             trusted:
                 - ".*"
+            global_frames:
+                - world
         </rosparam>
 
     </node>
 
     <node pkg="tf" type="static_transform_publisher"
-        name="tf_test" args="1 1 2 1 0 0 1 world base_link 100" />
+        name="tf_test" args="1 1 2 1 0 0 1 world map 100" />
     <!--<node pkg="rostopic" type="rostopic" name="state_pub"
-        args="pub -r 100 /tf tf2_msgs/TFMessage '{transforms: [{header : {stamp: {secs: 1457594405, nsecs: 347724387}, frame_id: world, seq: 0}, transform: {translation: {y: 0.0, x: 1.0, z: 2.0}, rotation: {y: 1.0, x: 0.0, z: 0.0, w: 1.0}}, child_frame_id: base_link}]}'" />-->
+        args="pub -r 100 /tf tf2_msgs/TFMessage '{transforms: [{header : {stamp: {secs: 1457594405, nsecs: 347724387}, frame_id: world, seq: 0}, transform: {translation: {y: 0.0, x: 1.0, z: 2.0}, rotation: {y: 1.0, x: 0.0, z: 0.0, w: 1.0}}, child_frame_id: map}]}'" />-->
 </launch>
diff --git a/src/client_node.py b/src/client_node.py
@@ -16,7 +16,8 @@
 # One instance per client node.
 class CanopyClientNode(object):
 
-    def __init__(self, host, port, name, broadcasting, private_key, description):
+    def __init__(self, host, port, name, broadcasting, private_key,
+        description, global_frames):
         self.host = host
         self.port = port
         self.name = name.replace(" ", "").replace("/", "")
@@ -27,6 +28,7 @@ def __init__(self, host, port, name, broadcasting, private_key, description):
         self.broadcasting = broadcasting
         self.private_key = private_key
         self.description = description
+        self.global_frames = global_frames
         self.pub_man = pm.PublisherManager()
         self.timer = threading.Timer(0.1, self.descriptionSend)
 
@@ -93,22 +95,44 @@ def callback(msg):
             if msg_type == "tf2_msgs/TFMessage":
                 exit = False
                 for t in msg.transforms:
-                    if t.header.frame_id.count("/") > 1:
+                    t = self.modify_stamped_message(t)
+                    if t == False:
                         exit = True
-                        break;
-                    if t.header.frame_id[0] != "/":
-                        t.header.frame_id = "/" + t.header.frame_id
-                        t.child_frame_id = "/" + t.child_frame_id
-                    t.header.frame_id = "/{}{}".format(self.name,
-                                                       t.header.frame_id)
-                    t.child_frame_id = "/{}{}".format(self.name,
-                                                      t.child_frame_id)
+                        break
                 if exit:
                     return
+            else:
+                msg = self.modify_stamped_message(msg)
+                if msg == False:
+                    return
             data["Msg"] = mc.convert_ros_message_to_dictionary(msg)
             self.conn[topic].send_message(data)
         return callback
 
+    def modify_stamped_message(self, message):
+        if hasattr(message, 'child_frame_id'):
+            if (message.child_frame_id.find("/") > 0 or
+                    message.child_frame_id.count("/") > 1):
+                return False
+            if message.child_frame_id not in self.global_frames:
+                if message.child_frame_id[0] != "/":
+                    message.child_frame_id = "/" + message.child_frame_id
+                message.child_frame_id = "{}{}".format(self.name,
+                        message.child_frame_id)
+        if hasattr(message, 'header'):
+            if ((not hasattr(message, 'child_frame_id')) and
+                    message.header.frame_id.find("/") > 0 and
+                    message.header.frame_id.count("/") > 1):
+                return False
+            if message.header.frame_id not in self.global_frames:
+                if (message.header.frame_id.find("/") <= 0 and
+                        message.header.frame_id.count("/") <= 1):
+                    if message.header.frame_id[0] != "/":
+                        message.header.frame_id = "/" + message.header.frame_id
+                    message.header.frame_id = "{}{}".format(self.name,
+                            message.header.frame_id)
+        return message
+
     # Periodic function to send the client description.
     def descriptionSend(self):
         data = dict()
@@ -131,11 +155,12 @@ def descriptionSend(self):
     topics = rospy.get_param("~publishing", [])
     types = rospy.get_param("~types", [])
     trusted = rospy.get_param("~trusted", [])
+    global_frames = rospy.get_param("~global_frames", [])
     host = rospy.get_param("~host")
     port = rospy.get_param("~port")
     private_key = rospy.get_param("~private_key")
     description = rospy.get_param("~description")
     broadcasting = zip(topics, types, trusted)
     rcn = CanopyClientNode(host, port, name, broadcasting, private_key,
-        description)
+        description, global_frames)
     rcn.run()