diff --git a/python/examples/graz/protobuf_ros1_bridge.py b/python/examples/graz/protobuf_ros1_bridge.py index 4c412c930e627df03350a0e3b1f78083c7646bda..9f2c854d9db457c7cda5ce61f118a1ba11132ff9 100644 --- a/python/examples/graz/protobuf_ros1_bridge.py +++ b/python/examples/graz/protobuf_ros1_bridge.py @@ -167,5 +167,5 @@ if __name__ == "__main__": 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 + wrench_message.header.frame_id = "0" pub.publish(wrench_message) diff --git a/python/examples/graz/protobuf_to_ros1.py b/python/examples/graz/protobuf_to_ros1.py index 0b35f2b2c3a96e478c63cead17a520ccdacc9597..ae1199117f20721dfc2635246cc589664ade3b85 100644 --- a/python/examples/graz/protobuf_to_ros1.py +++ b/python/examples/graz/protobuf_to_ros1.py @@ -50,6 +50,7 @@ def parse_message(buffer): # ros parts +#if False: if importlib.util.find_spec("rospy"): ros1_exists = True rospy.init_node("ur5e_to_ros1_mapper", anonymous=True) @@ -73,8 +74,8 @@ buffer = b"" while True: msg_raw = comm_socket.recv(1024) buffer += msg_raw - print(msg_raw) - print(len(msg_raw)) +# print(msg_raw) +# print(len(msg_raw)) if len(msg_raw) < 1: continue msg_in_bytes, pos = parse_message(buffer) @@ -97,5 +98,5 @@ while True: 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 + wrench_message.header.frame_id = "0" pub.publish(wrench_message) diff --git a/python/examples/graz/ros1_to_protobuf.py b/python/examples/graz/ros1_to_protobuf.py index 23b9a4738f252e6c1d4dffecc386122c32d17ab9..96f820a1d0ad4c7c9a7bf1a26a01f2d6c796c0ee 100644 --- a/python/examples/graz/ros1_to_protobuf.py +++ b/python/examples/graz/ros1_to_protobuf.py @@ -55,12 +55,13 @@ def sendRandomForTest(comm_socket): pb2_msg.position.extend([random.random()] * 3) pb2_msg.rotation.extend([random.random()] * 4) pb2_msg.velocity.extend([random.random()] * 6) - # 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) - time.sleep(0.002) + #time.sleep(0.002) + time.sleep(0.02) if __name__ == "__main__":