def rx_receive(self):
packet_reader = PacketReader()
# rate = rospy.Rate(10000)
while not rospy.is_shutdown():
data = bytearray([])
for i in range(100):
try:
_data, address = self.sock.recvfrom(4096)
data = data + bytearray(_data)
except socket.error as e:
break
if data:
# data = bytearray(data)
packets = packet_reader.receive_bytes(data)
for packet in packets:
device_id = packet[0]
packet_id = packet[1]
data = packet[2]
ros_packet = Packet()
ros_packet.device_id = device_id
ros_packet.packet_id = packet_id
ros_packet.data = list(data)
# print(data)
# rospy.loginfo("Publishing {}".format(ros_packet))
self.rx_publisher.publish(ros_packet)
# rate.sleep()
pass
def rx_receive(self):
packet_reader = PacketReader()
# rate = rospy.Rate(10000)
while not rospy.is_shutdown():
try:
data = bytearray(self.serial_port.read())
except serial.SerialException:
rospy.logdebug("Error reading from serial")
continue
if data:
packets = packet_reader.receive_bytes(data)
for packet in packets:
device_id = packet[0]
packet_id = packet[1]
data = packet[2]
ros_packet = Packet()
ros_packet.device_id = device_id
ros_packet.packet_id = packet_id
ros_packet.data = list(data)
self.rx_publisher.publish(ros_packet)
# Read from serial and transmit
pass
# To TARE THe sensor send an ATI_FT Packet to Device 0x0D
tare_packet = \
BPLProtocol.encode_packet(0x0D, PacketID.ATI_FT_READING, BPLProtocol.encode_floats([0., 0., 0., 0., 0., 0.]))
sock.sendto(tare_packet, manipulator_address)
print("Tared the FT Sensor")
time.sleep(0.5)
# To Read the packers request from the ATI_FT Packet from device 0x0D
request_timeout = 0.1 # seconds
pr = PacketReader()
readings = 0
while True:
request_packet = BPLProtocol.encode_packet(
0x0D, PacketID.REQUEST, bytes([PacketID.ATI_FT_READING]))
start_time = time.time()
sock.sendto(request_packet, manipulator_address)
packet_received = False
while time.time() - start_time < request_timeout:
try:
recv_bytes, address = sock.recvfrom(4096)
except BaseException:
from bplprotocol import BPLProtocol, PacketID, PacketReader
import time
# install pyserial with pip install pyserial
import serial
if __name__ == '__main__':
packet_reader = PacketReader()
serial_port_name = "COM10"
request_timeout = 0.5 # Seconds
serial_port = serial.Serial(serial_port_name,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0)
# Requesting Information
device_id = 0x01 # Jaws
print(f"Requesting Position from Device {device_id}")
# Request POSITION from the jaws
serial_port.write(
BPLProtocol.encode_packet(device_id, PacketID.REQUEST,
bytes([PacketID.POSITION])))