def test_sendEmptyWayPointCmd(self):
logging.info("Sending empty command")
cmd_packet = comm_packet_pb2.CommandPacket()
# OK to send an empty command as long as it's of type CommandPacket
# Just don't expect a response
response = self.testArticle.commandArduino(cmd_packet)
print response
def commandWayPoint(self, way_point):
logging.info("Attempting to command WayPoint : " + way_point.Name)
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = way_point.Heading
cmd_packet.WayPointCmd.Distance = way_point.Distance
cmd_packet.WayPointCmd.Name = way_point.Name
self.SerialComm.commandArduino(cmd_packet)
def test_commandControlSignal(self):
logging.info("Sending control signal command")
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 2.3456
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
def unpackCmdRcvd(self, cmd):
logging.debug("Bytes to be unpacked :")
logging.debug(":".join("{:02x}".format(ord(c)) for c in cmd))
# Remove the footer (last four bytes...)
cmd = cmd[:-4]
cmd_rcvd = comm_packet_pb2.CommandPacket()
cmd_rcvd.ParseFromString(cmd)
return cmd_rcvd
def test_getActiveWayPoint(self):
logging.info("Sending get active waypoint command")
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = WP_GET_ACTIVE
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
logging.info("The active waypoint is : " + response.ActiveWayPoint)
def __init__(self, serialPort):
QThread.__init__(self)
self.serialPort = serialPort
self.serialComm = SerialCommunication(serialPort)
self.cmd = comm_packet_pb2.CommandPacket()
control_signal_cmd = self.cmd.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 0.0
def getActiveWayPoint(self):
cmd_packet = comm_packet_pb2.CommandPacket()
request_active_waypoint = cmd_packet.RoverCmds.add()
request_active_waypoint.Id = WP_GET_ACTIVE
response = self.SerialComm.commandArduino(cmd_packet)
if response:
return response.ActiveWayPoint
else:
return "None"
def do_get_active_waypoint(self, args):
""" Gets the name of the active way point"""
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = WP_GET_ACTIVE
response = self.serialComm.commandArduino(cmd_packet)
if response:
print "The active waypoint is : " + response.ActiveWayPoint
else:
print " Command error"
return
def helper_SendSineWaveControlSignal(self,
amplitude=1.0,
frequency=1.0,
duration=1.0,
stepSize=0.01):
sine_wave = self.helper_generateSineWave(amplitude, frequency,
duration, stepSize)
for sample in sine_wave:
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = sample
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
def send_waypoint(self):
try:
print "WayPoint Name : "
way_point_name = raw_input()
print "Heading : "
way_point_heading = float(raw_input())
print "Distance : "
way_point_distance = float(raw_input())
except ValueError:
print "Invalid waypoint. Name must be a string less than 15 characters. Heading and distance must be a float."
way_point_cmd = comm_packet_pb2.CommandPacket()
way_point_cmd.WayPointCmd.Name = way_point_name
way_point_cmd.WayPointCmd.Heading = way_point_heading
way_point_cmd.WayPointCmd.Distance = way_point_distance
response = self.serialComm.commandArduino(way_point_cmd)
if self.isValidWayPoint(response):
print "WayPoint " + way_point_name + " successfully sent and processed."
else:
print "WayPoint " + way_point_name + " command was rejected."
def do_start_control(self, args):
"""Sends a sine wave control signal to the robot."""
try:
print " This command will send a sine wave to the robot. Paramterize the sine wave "
print "Enter amplitude : "
amplitude = float(raw_input())
print "Enter frequency : "
frequency = float(raw_input())
except ValueError:
print "Invalid amplitude or frequency. Both must be floats."
sine_wave = self.generateSineWave(amplitude, frequency)
for sample in sine_wave:
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = sample
response = self.testArticle.commandArduino(cmd_packet)
if response:
print " Response signal : " + str(
response.RoverStatus[1].Value)
def test_sendNoDistanceWayPointCmd(self):
logging.info("Sending no distance way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = 45.0
cmd_packet.WayPointCmd.Name = "WayPoint A"
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
#!/usr/bin/python import unittest import logging, sys import numpy as np import matplotlib.pyplot as plt from time import sleep # Need a serial communication component for this test from SerialCommunication import SerialCommunication from CmdResponseDefinitions import * import comm_packet_pb2 # Define a list of way-points for the purpose of testing test_way_point_1 = comm_packet_pb2.CommandPacket() test_way_point_1.WayPointCmd.Heading = 45 test_way_point_1.WayPointCmd.Distance = 0.5 test_way_point_1.WayPointCmd.Name = "WayPoint A" test_way_point_2 = comm_packet_pb2.CommandPacket() test_way_point_2.WayPointCmd.Heading = 90 test_way_point_2.WayPointCmd.Distance = 2.0 test_way_point_2.WayPointCmd.Name = "WayPoint B" test_way_point_3 = comm_packet_pb2.CommandPacket() test_way_point_3.WayPointCmd.Heading = 180 test_way_point_3.WayPointCmd.Distance = 3.0 test_way_point_3.WayPointCmd.Name = "WayPoint C" test_route = [test_way_point_1, test_way_point_2, test_way_point_3]
