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 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_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)