def get_motion_service(m_req):
xmega_lock.acquire(True)
print("Lock aquired")
packet = XMEGAPacket()
packet.msg_type = 0x0B
packet.msg_length = 1
print("Sending message")
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
xAccelData, yAccelData, zAccelData, xGyroData, yGyroData, zGyroData = struct.unpack(
"<hhhhhh", response_packet.msg_body)
connector_object.send_ack()
print("Creating response")
service_response = GetMotionResponse()
service_response.xAccelData = xAccelData
service_response.yAccelData = yAccelData
service_response.zAccelData = zAccelData
service_response.xGyroData = xGyroData
service_response.yGyroData = yGyroData
service_response.zGyroData = zGyroData
print("Returning response")
xmega_lock.release()
return service_response
def get_motion_service(m_req):
xmega_lock.acquire(True)
print("Lock aquired")
packet = XMEGAPacket()
packet.msg_type = 0x0B
packet.msg_length = 1
print("Sending message")
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
xAccelData, yAccelData, zAccelData, xGyroData, yGyroData, zGyroData = struct.unpack("<hhhhhh", response_packet.msg_body)
connector_object.send_ack()
print( "Creating response")
service_response = GetMotionResponse()
service_response.xAccelData = xAccelData
service_response.yAccelData = yAccelData
service_response.zAccelData = zAccelData
service_response.xGyroData = xGyroData
service_response.yGyroData = yGyroData
service_response.zGyroData = zGyroData
print( "Returning response")
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 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 get_heading_service(head_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x0A
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
xData, zData, yData = struct.unpack("<hhh", response_packet.msg_body)
connector_object.send_ack()
service_response = GetHeadingResponse()
service_response.xData = xData
service_response.zData = zData
service_response.yData = yData
xmega_lock.release()
return service_response
def get_heading_service(head_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x0A
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
xData, zData, yData = struct.unpack("<hhh", response_packet.msg_body)
connector_object.send_ack()
service_response = GetHeadingResponse()
service_response.xData = xData
service_response.zData = zData
service_response.yData = yData
xmega_lock.release()
return service_response
def get_odometry_service(odo_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x05
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
wheel1, wheel2, wheel3, wheel4 = struct.unpack("<iiii", response_packet.msg_body)
connector_object.send_ack()
service_response = GetOdometryResponse()
service_response.wheel1 = wheel1 / 1000.
service_response.wheel2 = wheel2 / 1000.
service_response.wheel3 = wheel3 / 1000.
service_response.wheel4 = wheel4 / 1000.
xmega_lock.release()
return service_response
def get_odometry_service(odo_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x05
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
wheel1, wheel2, wheel3, wheel4 = struct.unpack("<iiii",
response_packet.msg_body)
connector_object.send_ack()
service_response = GetOdometryResponse()
service_response.wheel1 = wheel1 / 1000.
service_response.wheel2 = wheel2 / 1000.
service_response.wheel3 = wheel3 / 1000.
service_response.wheel4 = wheel4 / 1000.
xmega_lock.release()
return service_response
def get_color_service(color_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x09
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
red, green, blue, clear = struct.unpack("<HHHH", response_packet.msg_body)
print "red:{0} green:{1} blue:{2} clear:{3}".format(red, green, blue, clear)
connector_object.send_ack()
service_response = GetColorResponse()
service_response.red = red
service_response.green = green
service_response.blue = blue
service_response.clear = clear
xmega_lock.release()
return service_response
def get_color_service(color_req):
xmega_lock.acquire(True)
packet = XMEGAPacket()
packet.msg_type = 0x09
packet.msg_length = 1
connector_object.send_packet(packet)
response_packet = connector_object.read_packet()
red, green, blue, clear = struct.unpack("<HHHH", response_packet.msg_body)
print "red:{0} green:{1} blue:{2} clear:{3}".format(
red, green, blue, clear)
connector_object.send_ack()
service_response = GetColorResponse()
service_response.red = red
service_response.green = green
service_response.blue = blue
service_response.clear = clear
xmega_lock.release()
return service_response
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
def send_ack(self):
ack_packet = XMEGAPacket()
ack_packet.msg_type = 0x00
ack_packet.msg_length = 0x01
connector_object.send_packet(ack_packet)
def send_ack(self):
ack_packet = XMEGAPacket()
ack_packet.msg_type = 0x00
ack_packet.msg_length = 0x01
connector_object.send_packet(ack_packet)
