stuff

python/examples/graz/protobuf_to_ros1.py

@@ -5,7 +5,7 @@ import time
 import argparse
 import message_specs_pb2
 import rospy
-from geometry_msgs import WrenchStamped
+from geometry_msgs.msg import WrenchStamped
 
 
 def getArgs():
@@ -33,6 +33,7 @@ def parse_message(buffer):
     """
     pos, next_pos = 0, 0
     buffer_len = len(buffer)
+    print("buffer_len", buffer_len)
     msg_in_bytes = b""
     len_size_offset = 0
     while True:
@@ -46,6 +47,11 @@ def parse_message(buffer):
             return msg_in_bytes, pos
 
 
+
+# ros parts
+#rospy.init_node("ur5e_to_ros1_mapper", anonymous=True)
+#pub = rospy.Publisher("wrench_msg_mapper", WrenchStamped, queue_size=10)
+
 args = getArgs()
 host_addr = (args.host, args.port)
 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
@@ -57,15 +63,16 @@ while True:
     except ConnectionRefusedError:
         time.sleep(0.005)
 print("NETWORKING CLIENT: connected to server")
+s.settimeout(None)
 
-# ros parts
-pub = rospy.Publisher("wrench_msg_mapper", WrenchStamped, queue_size=10)
-rospy.init_node("ur5e_to_ros1_mapper", anonymous=True)
-
-try:
+#try:
 while True:
     msg_raw = s.recv(1024)
     buffer += msg_raw
+    print(msg_raw)
+    print(len(msg_raw))
+    if len(msg_raw) < 1:
+        continue
     msg_in_bytes, pos = parse_message(buffer)
     buffer = buffer[pos:]
 
@@ -77,18 +84,18 @@ try:
 
     print(pb2_msg.wrench)
     # here you send construct and send the ros message
-        wrench_message = WrenchStamped()
-        wrench_message.wrench.force.x = pb2_msg.wrench[0]
-        wrench_message.wrench.force.y = pb2_msg.wrench[1]
-        wrench_message.wrench.force.z = pb2_msg.wrench[2]
-        wrench_message.wrench.torque.x = pb2_msg.wrench[3]
-        wrench_message.wrench.torque.y = pb2_msg.wrench[4]
-        wrench_message.wrench.torque.z = pb2_msg.wrench[5]
-        wrench_message.header.stamp = rospy.Time.now()
-        wrench_message.header.frame_id = 0
-        pub.publish(wrench_message)
+    #wrench_message = WrenchStamped()
+    #wrench_message.wrench.force.x = pb2_msg.wrench[0]
+    #wrench_message.wrench.force.y = pb2_msg.wrench[1]
+    #wrench_message.wrench.force.z = pb2_msg.wrench[2]
+    #wrench_message.wrench.torque.x = pb2_msg.wrench[3]
+    #wrench_message.wrench.torque.y = pb2_msg.wrench[4]
+    #wrench_message.wrench.torque.z = pb2_msg.wrench[5]
+    #wrench_message.header.stamp = rospy.Time.now()
+    #wrench_message.header.frame_id = 0
+    #pub.publish(wrench_message)
     # time.sleep(0.002)
 
-except KeyboardInterrupt:
-    s.close()
-    print("NETWORKING_CLIENT: caught KeyboardInterrupt, i'm out")
+#except KeyboardInterrupt:
+#    s.close()
+#    print("NETWORKING_CLIENT: caught KeyboardInterrupt, i'm out")