TF support

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

Hide file tree

Showing 3 changed files with 60 additions and 10 deletions.
diff --git a/launch/example.launch b/launch/example.launch
@@ -6,47 +6,44 @@
         <param name="name" value="foo"/>
         <param name="host" value="$(arg host)"/>
         <param name="port" value="$(arg port)"/>
-        <param name="private_key" value="MIICXAIBAAKBgQCTUQ49FJUCVvU5tIag" />
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
         <param name="description" value="foo robot" />
     </node>
 
     <node pkg="canopy_client" type="client_node.py" name="bar_client_node" output="screen">
         <param name="name" value="bar"/>
         <param name="host" value="$(arg host)"/>
         <param name="port" value="$(arg port)"/>
-        <param name="private_key" value="MIICXAIBAAKBgQCTUQ49FJUCVvU5tIag" />
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
         <param name="description" value="bar robot" />
     </node>
 
     <node pkg="canopy_client" type="client_node.py" name="baz_client_node" output="screen">
         <param name="name" value="baz"/>
         <param name="host" value="$(arg host)"/>
         <param name="port" value="$(arg port)"/>
-        <param name="private_key" value="MIICXAIBAAKBgQCTUQ49FJUCVvU5tIag" />
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
         <param name="description" value="baz robot" />
     </node>
 
     <node pkg="canopy_client" type="client_node.py" name="qux_client_node" output="screen">
         <param name="name" value="qux"/>
         <param name="host" value="$(arg host)"/>
         <param name="port" value="$(arg port)"/>
-        <param name="private_key" value="MIICXAIBAAKBgQCTUQ49FJUCVvU5tIag" />
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
         <param name="description" value="qux robot" />
         <rosparam>
             publishing:
                 - /state
                 - /filtered_state
-                - /usb_cam/image_raw/compressed
                 - /test_string
             types:
                 - geometry_msgs/Point
                 - geometry_msgs/Point
-                - sensor_msgs/CompressedImage
                 - std_msgs/String
             trusted:
                 - ".*"
                 - foo bar
-                - foo
                 - "ba[a-z]"
         </rosparam>
 
@@ -58,14 +55,16 @@
         args="pub -r 100 /filtered_state geometry_msgs/Point '{x: 2, y: 1, z: 1}'" />
     <node pkg="rostopic" type="rostopic" name="string_pub"
         args="pub -r 100 /test_string std_msgs/String 'test'" />
-    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <node pkg="tf" type="static_transform_publisher"
+        name="tf_test" args="1 0 0 0 0 0 1 world base_link 100" />
+    <!--<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
         <param name="video_device" value="/dev/video0" />
         <param name="image_width" value="640" />
         <param name="image_height" value="480" />
         <param name="pixel_format" value="yuyv" />
         <param name="camera_frame_id" value="usb_cam" />
         <param name="io_method" value="mmap"/>
-    </node>
+    </node>-->
     <!-- <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen"> -->
     <!--     <remap from="image" to="/qux/usb_cam/image_raw"/> -->
     <!--     <param name="autosize" value="true" /> -->

diff --git a/launch/tf_test.launch b/launch/tf_test.launch
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="host" default="raphaelchang.com" />
+    <arg name="port" default="8080"/>
+    <node pkg="canopy_client" type="client_node.py" name="foo_client_node" output="screen">
+        <param name="name" value="raphael_rcv"/>
+        <param name="host" value="$(arg host)"/>
+        <param name="port" value="$(arg port)"/>
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
+        <param name="description" value="raphael tf test receiver"/>
+    </node>
+    <node pkg="canopy_client" type="client_node.py" name="qux_client_node" output="screen">
+        <param name="name" value="raphael"/>
+        <param name="host" value="$(arg host)"/>
+        <param name="port" value="$(arg port)"/>
+        <param name="private_key" value="PsFXjmWpszr6acSKL" />
+        <param name="description" value="raphael tf test" />
+        <rosparam>
+            publishing:
+                - /tf
+            types:
+                - tf2_msgs/TFMessage
+            trusted:
+                - ".*"
+        </rosparam>
+
+    </node>
+
+    <node pkg="tf" type="static_transform_publisher"
+        name="tf_test" args="1 1 2 1 0 0 1 world base_link 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}]}'" />-->
+</launch>
diff --git a/src/client_node.py b/src/client_node.py
@@ -83,10 +83,28 @@ def callback(msg):
             data = dict()
             data["To"] = trusted.split(' ')
             data["From"] = self.name
-            data["Topic"] = "/{}{}".format(self.name, topic)
+            if topic == "/tf":
+                data["Topic"] = topic
+            else:
+                data["Topic"] = "/{}{}".format(self.name, topic)
             data["Type"] = msg_type
             data["Stamp"] = time.time()
             data["Private_key"] = self.private_key
+            if msg_type == "tf2_msgs/TFMessage":
+                exit = False
+                for t in msg.transforms:
+                    if t.header.frame_id.count("/") > 1:
+                        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)
+                if exit:
+                    return
             data["Msg"] = mc.convert_ros_message_to_dictionary(msg)
             self.conn[topic].send_message(data)
         return callback