From 93d0c01df9d3a4eb750d3e991a9c1a78411e954a Mon Sep 17 00:00:00 2001
From: Your Name <marko.guberina@control.lth.se>
Date: Tue, 4 Feb 2025 15:22:23 +0100
Subject: [PATCH] stuff
---
python/examples/graz/protobuf_to_ros1.py | 71 +++++++++++++-----------
1 file changed, 39 insertions(+), 32 deletions(-)
diff --git a/python/examples/graz/protobuf_to_ros1.py b/python/examples/graz/protobuf_to_ros1.py
index 5b03013..2c3afcc 100644
--- a/python/examples/graz/protobuf_to_ros1.py
+++ b/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,38 +63,39 @@ 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:
- while True:
- msg_raw = s.recv(1024)
- buffer += msg_raw
- msg_in_bytes, pos = parse_message(buffer)
- buffer = buffer[pos:]
+#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:]
- next_pos, pos = 0, 0
- next_pos, pos = _DecodeVarint32(msg_in_bytes, pos)
- pb2_msg_in_bytes_cut = msg_in_bytes[pos : pos + next_pos]
- pb2_msg = message_specs_pb2.wrench()
- pb2_msg.ParseFromString(pb2_msg_in_bytes_cut)
+ next_pos, pos = 0, 0
+ next_pos, pos = _DecodeVarint32(msg_in_bytes, pos)
+ pb2_msg_in_bytes_cut = msg_in_bytes[pos : pos + next_pos]
+ pb2_msg = message_specs_pb2.wrench()
+ pb2_msg.ParseFromString(pb2_msg_in_bytes_cut)
- 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)
- # time.sleep(0.002)
+ 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)
+ # 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")
--
GitLab