def read_packet(self):
rospy.logdebug("Reading packet from XMEGA")
packet = XMEGAPacket()
data = ''
start_char_count = 0
message_len = 0
while True:
if start_char_count < 3:
if self._serial.read(1) == '^':
start_char_count += 1
rospy.logdebug(
"Reading from XMEGA - start character received (%d of 3)",
start_char_count)
continue
else:
message_len = ord(self._serial.read(1))
rospy.logdebug("Reading from XMEGA - packet length: %s",
hex(message_len))
data = self._serial.read(message_len)
packet.header.stamp = rospy.Time.now()
packet.msg_type = data[0]
rospy.logdebug("Reading from XMEGA - packet type: %s",
ord(packet.msg_type))
packet.msg_length = message_len
packet.msg_body = data[1:]
rospy.logdebug(
"Reading from XMEGA - packet body: %s -- hex: %s",
packet.msg_body, packet.msg_body.encode("hex"))
return packet
def set_wheel_speed_service(ws_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x04
wheel1 = int(ws_req.wheel1 * 1000.0)
wheel2 = int(ws_req.wheel2 * 1000.0)
wheel3 = int(ws_req.wheel3 * 1000.0)
wheel4 = int(ws_req.wheel4 * 1000.0)
packet.msg_body = struct.pack('<llll', wheel1, wheel2, wheel3, wheel4)
packet.msg_length = len(packet.msg_body) + 1
connector_object.send_packet(packet)
xmega_lock.release()
return SetWheelSpeedsResponse()
def set_wheel_speed_service(ws_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x04
wheel1 = int(ws_req.wheel1 * 1000.0)
wheel2 = int(ws_req.wheel2 * 1000.0)
wheel3 = int(ws_req.wheel3 * 1000.0)
wheel4 = int(ws_req.wheel4 * 1000.0)
packet.msg_body = struct.pack('<llll', wheel1, wheel2, wheel3, wheel4)
packet.msg_length = len(packet.msg_body) + 1
connector_object.send_packet(packet)
xmega_lock.release()
return SetWheelSpeedsResponse()
def echo_service(echo_request):
xmega_lock.acquire(True) # wait until lock can be acquired before proceeding
rospy.loginfo("XMEGA echo - about to echo: %s", echo_request.send)
packet = XMEGAPacket()
packet.msg_body = echo_request.send
packet.msg_type = 0x02 # 0x02 echo request, 0x03 echo reply
packet.msg_length = len(packet.msg_body) + 1
connector_object.send_packet(packet)
rospy.loginfo("XMEGA echo - sent echo request packet")
response_packet = connector_object.read_packet()
rospy.loginfo("XMEGA echo - received echo response packet")
rospy.loginfo("XMEGA echo - sending ack packet")
connector_object.send_ack()
rospy.loginfo("XMEGA echo - sent ack packet")
service_response = EchoResponse()
service_response.recv = response_packet.msg_body
rospy.loginfo("XMEGA echo - received response: %s", service_response.recv)
xmega_lock.release()
return service_response
def read_packet(self):
rospy.logdebug("Reading packet from XMEGA")
packet = XMEGAPacket()
data = ''
start_char_count = 0
message_len = 0
while True:
if start_char_count < 3:
if self._serial.read(1) == '^':
start_char_count += 1
rospy.logdebug("Reading from XMEGA - start character received (%d of 3)", start_char_count)
continue
else:
message_len = ord(self._serial.read(1))
rospy.logdebug("Reading from XMEGA - packet length: %s", hex(message_len))
data = self._serial.read(message_len)
packet.header.stamp = rospy.Time.now()
packet.msg_type = data[0]
rospy.logdebug("Reading from XMEGA - packet type: %s", ord(packet.msg_type))
packet.msg_length = message_len
packet.msg_body = data[1:]
rospy.logdebug("Reading from XMEGA - packet body: %s -- hex: %s", packet.msg_body, packet.msg_body.encode("hex"))
return packet
def echo_service(echo_request):
xmega_lock.acquire(
True) # wait until lock can be acquired before proceeding
rospy.loginfo("XMEGA echo - about to echo: %s", echo_request.send)
packet = XMEGAPacket()
packet.msg_body = echo_request.send
packet.msg_type = 0x02 # 0x02 echo request, 0x03 echo reply
packet.msg_length = len(packet.msg_body) + 1
connector_object.send_packet(packet)
rospy.loginfo("XMEGA echo - sent echo request packet")
response_packet = connector_object.read_packet()
rospy.loginfo("XMEGA echo - received echo response packet")
rospy.loginfo("XMEGA echo - sending ack packet")
connector_object.send_ack()
rospy.loginfo("XMEGA echo - sent ack packet")
service_response = EchoResponse()
service_response.recv = response_packet.msg_body
rospy.loginfo("XMEGA echo - received response: %s", service_response.recv)
xmega_lock.release()
return service_response
