diff --git a/python/examples/graz/protobuf_to_ros1.py b/python/examples/graz/protobuf_to_ros1.py index c01d29d1e482fa3929566eed695b9e3a168575fa..5b03013be2fe446e58aab46a36ba6d39c9b55090 100644 --- a/python/examples/graz/protobuf_to_ros1.py +++ b/python/examples/graz/protobuf_to_ros1.py @@ -4,15 +4,20 @@ from google.protobuf.internal.decoder import _DecodeVarint32 import time import argparse import message_specs_pb2 +import rospy +from geometry_msgs import WrenchStamped + def getArgs(): - parser = argparse.ArgumentParser() - parser.description = "get ros1 message, make it a protobuf message, send it via tcp socket" - parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1") - parser.add_argument("--port", type=int, help="host's port", default=6666) + parser = argparse.ArgumentParser() + parser.description = ( + "get ros1 message, make it a protobuf message, send it via tcp socket" + ) + parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1") + parser.add_argument("--port", type=int, help="host's port", default=6666) - args = parser.parse_args() - return args + args = parser.parse_args() + return args def parse_message(buffer): @@ -40,6 +45,7 @@ def parse_message(buffer): if pos >= buffer_len: return msg_in_bytes, pos + args = getArgs() host_addr = (args.host, args.port) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) @@ -52,6 +58,10 @@ while True: time.sleep(0.005) print("NETWORKING CLIENT: connected to server") +# 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) @@ -67,7 +77,17 @@ try: print(pb2_msg.wrench) # here you send construct and send the ros message - time.sleep(0.002) + 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() diff --git a/python/examples/graz/ros1_to_protobuf.py b/python/examples/graz/ros1_to_protobuf.py index 890f7fc7906ab15ec891e7592829e306c5f047cd..9eb487e42b87f20cb420bf1859a163ec6249dc57 100644 --- a/python/examples/graz/ros1_to_protobuf.py +++ b/python/examples/graz/ros1_to_protobuf.py @@ -8,35 +8,46 @@ import rospy from functools import partial - def getArgs(): - parser = argparse.ArgumentParser() - parser.description = "get ros1 message, make it a protobuf message, send it via tcp socket" - parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1") - parser.add_argument("--port", type=int, help="host's port", default=7777) + parser = argparse.ArgumentParser() + parser.description = ( + "get ros1 message, make it a protobuf message, send it via tcp socket" + ) + parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1") + parser.add_argument("--port", type=int, help="host's port", default=7777) - args = parser.parse_args() - return args + args = parser.parse_args() + return args def callback(comm_socket, c): - #print("received callback") + # 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] + 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] + 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) + # 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) + rospy.init_node("ros1_to_ur5e_mapper", anonymous=True) args = getArgs() host_addr = (args.host, args.port) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) @@ -57,13 +68,10 @@ if __name__ == "__main__": 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") +# 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") # - diff --git a/python/examples/graz/to_install_on_ubuntu18.md b/python/examples/graz/to_install_on_ubuntu18.md new file mode 100644 index 0000000000000000000000000000000000000000..fa5d9aa71da75f45e9417617fd98887d8aebd647 --- /dev/null +++ b/python/examples/graz/to_install_on_ubuntu18.md @@ -0,0 +1,2 @@ +sudo update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.6.9 +sudo update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.8.1