From eaa9f8543754a0b8e50dbfbf2ba142a71938a55e Mon Sep 17 00:00:00 2001
From: Your Name <marko.guberina@control.lth.se>
Date: Mon, 3 Feb 2025 18:14:23 +0100
Subject: [PATCH] did a thing
---
python/examples/graz/ros1_to_protobuf.py | 79 +++++++++++++++---------
1 file changed, 50 insertions(+), 29 deletions(-)
diff --git a/python/examples/graz/ros1_to_protobuf.py b/python/examples/graz/ros1_to_protobuf.py
index a9659f9..53fa8a7 100644
--- a/python/examples/graz/ros1_to_protobuf.py
+++ b/python/examples/graz/ros1_to_protobuf.py
@@ -2,8 +2,9 @@ import socket
from google.protobuf.internal.encoder import _VarintBytes
import argparse
import message_specs_pb2
-import numpy as np
import time
+from agents_test.msg import Command
+import rospy
@@ -16,31 +17,51 @@ def getArgs():
args = parser.parse_args()
return args
-args = getArgs()
-host_addr = (args.host, args.port)
-s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-s.bind(host_addr)
-
-print("listening on port ", args.port)
-s.listen()
-comm_socket, comm_addr = s.accept()
-# we're only accepting a single connection
-s.close()
-print("NETWORKING_SERVER: accepted a client", comm_addr)
-try:
- while True:
- # listen to a pre-determined ros1 message here
- pb2_msg = message_specs_pb2.joint_angles()
- pb2_msg.q.extend(np.random.random(8))
- print(pb2_msg)
- msg_length = pb2_msg.ByteSize()
- msg_serialized = pb2_msg.SerializeToString()
- msg = _VarintBytes(msg_length) + msg_serialized
- comm_socket.send(msg)
- time.sleep(0.002)
-except KeyboardInterrupt:
- if args.debug_prints:
- print("NETWORKING_SERVER: caugth KeyboardInterrupt, networking server out")
-
-
-comm_socket.close()
+
+def callback(c):
+ print("received callback")
+ position = [c.goal.position.x, c.goal.position.y, c.goal.position.z]
+ orientation = [c.goal.orientation.w, c.goal.orientation.x, c.goal.orientation.y, c.goal.orientation.z]
+ position_vel = [c.goal_dot.position.x, c.goal_dot.position.y, c.goal_dot.position.z]
+ orientation_vel = [c.goal_dot.orientation.w, c.goal_dot.orientation.x, c.goal_dot.orientation.y]
+ pb2_msg = message_specs_pb2.T_goal()
+ pb2_msg.position.extend(position)
+ pb2_msg.rotation.extend(orientation)
+ pb2_msg.velocity.extend(position_vel + orientation_vel)
+ print(pb2_msg)
+ msg_length = pb2_msg.ByteSize()
+ msg_serialized = pb2_msg.SerializeToString()
+ msg = _VarintBytes(msg_length) + msg_serialized
+ comm_socket.send(msg)
+
+if __name__ == "__main__":
+ rospy.init_node('ur5e_mapper', anonymous=True)
+ args = getArgs()
+ host_addr = (args.host, args.port)
+ s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ s.bind(host_addr)
+
+ print("listening on port ", args.port)
+ s.listen()
+ comm_socket, comm_addr = s.accept()
+ # we're only accepting a single connection
+ s.close()
+ print("NETWORKING_SERVER: accepted a client", comm_addr)
+
+ # ros subscriber
+ rospy.Subscriber("/robot_command", Command, callback)
+ rospy.spin()
+
+ comm_socket.close()
+
+
+
+
+#try:
+# while True:
+# # listen to a pre-determined ros1 message here
+#except KeyboardInterrupt:
+# if args.debug_prints:
+# print("NETWORKING_SERVER: caugth KeyboardInterrupt, networking server out")
+#
+
--
GitLab