def test_should_navigate_along_list_of_waypoints_with_logging(self):
waypoint1 = Waypoint(Position(11, 11), 10)
waypoint2 = Waypoint(Position(13, 13), 10)
follower = Follower(self.exchange, [waypoint1, waypoint2],
self.mock_logger)
self.event_source.start()
self.mock_logger.info.assert_has_calls([
call('Follower, next waypoint +11.000000,+11.000000'),
call(
'Navigator, steering to +11.000000,+11.000000, bearing 44.4, distance 155941.2m, review after 20s'
),
call('Navigator, arrived at +11.000000,+11.000000'),
call('Follower, next waypoint +13.000000,+13.000000'),
call(
'Navigator, steering to +13.000000,+13.000000, bearing 44.2, distance 155399.6m, review after 20s'
),
call('Navigator, arrived at +13.000000,+13.000000'),
call(
'**************************************************************'
),
call('Follower, all waypoints reached, navigation complete'),
call(
'**************************************************************'
)
])
def test_with_multiple_obstacles(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[3:4, 0:15] = True
grid[13:14, 5:20] = True
queries = [[Waypoint(0, 0, 0), Waypoint(19, 19, 3)]]
self._run_test(grid, queries)
def test_with_no_solution(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[15:16, 0:10] = True
grid[15:20, 10:11] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 2, 3)]
]
self._run_test_with_no_solution(grid, queries)
def test_should_navigate_to_the_next_waypoint_when_a_waypoint_is_reached(
self):
self.listen(EventName.navigate)
waypoint1 = Waypoint(Position(1, 1), 5)
waypoint2 = Waypoint(Position(2, 2), 5)
follower = Follower(self.exchange, [waypoint1, waypoint2],
self.mock_logger)
self.exchange.publish(Event(EventName.start))
self.exchange.publish(Event(EventName.arrived, waypoint1))
self.assertEqual(self.last_event.waypoint, waypoint2)
def test_with_complex_obstacles(self):
grid = np.zeros((20, 20)).astype(np.bool)
grid[3:4, 5:20] = True
grid[7:8, 0:15] = True
grid[13:14, 5:20] = True
grid[11:14, 4:5] = True
grid[14:17, 7:8] = True
grid[15:20, 12:13] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 19, 3)]
]
self._run_test(grid, queries)
def __init__(self,
coords=[-1, -1],
start=[-1, -1],
target=[-1, -1],
par=None):
self.position = Waypoint(coordinates=coords)
self.start = Waypoint(coordinates=start)
self.target = Waypoint(coordinates=target)
self.path = []
self.g = 0
self.h = 0
self.f = 0
self.parent = par
def test_with_random_obstacle(self):
grid = np.zeros((20, 20)).astype(np.bool)
for variable in range(1,4):
rand_a = random.randint(1, 10)
rand_b = random.randint(0, 5)
rand_c = random.randint(5, 10)
rand_d = random.randint(0, 5)
grid[rand_a:rand_a + rand_b, rand_c:rand_c + rand_d] = True
queries = [
[Waypoint(0, 0, 0), Waypoint(19, 19, 3)],
]
self._run_test(grid, queries)
def compute_man2(data):
glob = ["E", "N", "S", "W"]
loc = ["L", "R"]
ship = Ship()
waypoint = Waypoint()
for dire in data:
print(f"Waypoint Position: {waypoint.get_x()},{waypoint.get_y()}")
if dire[0] in glob:
waypoint.shift(dire[0], dire[1])
elif dire[0] in loc:
waypoint.rotate(dire[0], dire[1])
elif dire[0] == "F":
x = waypoint.get_x() * dire[1]
y = waypoint.get_y() * dire[1]
ship.alter_global("E", x)
ship.alter_global("N", y)
print(ship.compute_man())
def test_should_navigate_to_next_waypoint_with_kink_in_route(self):
destination = Waypoint(Position(10.03, 10.03), 10)
gps = FakeMovingGPS([
Position(10, 10),
Position(10.01, 10.01),
Position(10.025, 10.015),
Position(10.03, 10.03)
])
sensors = FakeSensors(gps, 1, 45)
steerer = Steerer(self.servo, self.logger, CONFIG['steerer'])
helm = Helm(self.exchange, sensors, steerer, self.logger,
CONFIG['helm'])
navigator = Navigator(sensors, Globe(), self.exchange, self.logger,
CONFIG['navigator'])
self.exchange.publish(Event(EventName.navigate, waypoint=destination))
self.ticks(number=14, duration=200)
self.logger.info.assert_has_calls([
call(
'Navigator, steering to +10.030000,+10.030000, bearing 44.6, distance 4681.8m, review after 600s'
),
call(
'Navigator, steering to +10.030000,+10.030000, bearing 44.6, distance 3121.2m, review after 600s'
),
call(
'Navigator, steering to +10.030000,+10.030000, bearing 71.3, distance 1734.0m, review after 600s'
),
call('Navigator, arrived at +10.030000,+10.030000')
])
def iact31(self):
self.write('ACT_31_00\r\n')
line = self.readline()
if line == 'No Data\r\n':
return
while True:
if line == ' Done\r\n':
break
m = re.match(
r'\A(.*?);([NS])\s+(\d+)\'(\d+\.\d+);([EW])\s+(\d+)\'(\d+\.\d+);\s*(\d+);\s*(\d+)\r\n',
line)
if m:
name = m.group(1)
lat = int(m.group(3)) + float(m.group(4)) / 60.0
if m.group(2) == 'S':
lat = -lat
lon = int(m.group(6)) + float(m.group(7)) / 60.0
if m.group(5) == 'W':
lon = -lon
alt = int(m.group(8))
radius = int(m.group(9))
yield Waypoint(name, lat, lon, alt, radius=radius)
else:
raise ProtocolError('unexpected response %r' % line)
line = self.readline()
def __init__(self, ORB):
self.ORB = ORB
logger.info('Drone Type:{}'.format(config.drone['UAV']))
logger.info('MainController:{}'.format(config.drone['MainController']))
self._model = config.drone['Model']
# Aileron :[No.ch, low ,mid, high ,var, sign, rate]
self.AIL = config.channels['AIL']
# Elevator:[No.ch, low ,mid, high ,var, sign, rate]
self.ELE = config.channels['ELE']
# Throttle:[No.ch, low ,mid, high ,var, sign, rate]
self.THR = config.channels['THR']
# Rudder :[No.ch, low ,mid, high ,var, sign, rate]
self.RUD = config.channels['RUD']
# Mode :[No.ch , low , Loiter, high]
self.mode = config.channels['Mode']
if config.drone['Model'] == 'HELI':
# GYRO Rate :[No.ch , 0 , pwm]
self.Rate = config.channels['Rate']
# PITCH :[No.ch , 0 , pwm]
self.PIT = config.channels['PIT']
else:
# Aux1 :[No.ch , 0 , pwm]
self.Aux1 = config.channels['Aux1']
# Aux2 :[No.ch , 0 , pwm]
self.Aux2 = config.channels['Aux2']
# Switch :[No.ch , 0 , pwm]
self.Switch = config.channels['Switch']
self.wp = Waypoint(ORB)
self.update_home()
self.init_altitude()
def test_should_steer_repeatedly_during_navigation(self):
logger = Mock()
destination = Waypoint(Position(10.0003, 10.0003), 10)
gps = FakeMovingGPS([
Position(10, 10),
Position(10.0001, 10.00015),
Position(10.00025, 10.0002),
Position(10.0003, 10.0003)
])
sensors = FakeSensors(gps, 1, 45)
steerer = Steerer(self.servo, logger, CONFIG['steerer'])
helm = Helm(self.exchange, sensors, steerer, logger, CONFIG['helm'])
navigator = Navigator(sensors, Globe(), self.exchange, logger,
CONFIG['navigator'])
self.exchange.publish(Event(EventName.navigate, waypoint=destination))
self.ticks(number=7, duration=20)
logger.debug.assert_has_calls([
call(
'Navigator, distance from waypoint +46.819018, combined tolerance +10.000000'
),
call(
'Navigator, distance from waypoint +27.647432, combined tolerance +10.000000'
),
call(
'Steerer, steering 36.4, heading 45.0, rate of turn +1.0, rudder +0.0, new rudder +4.6'
),
call(
'Steerer, steering 36.4, heading 45.0, rate of turn +1.0, rudder +4.6, new rudder +9.2'
),
call(
'Navigator, distance from waypoint +12.281099, combined tolerance +10.000000'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder +9.2, new rudder +0.4'
),
call(
'Navigator, distance from waypoint +0.000000, combined tolerance +10.000000'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder +0.4, new rudder -8.3'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder -8.3, new rudder -17.1'
)
])
logger.info.assert_has_calls([
call(
'Navigator, steering to +10.000300,+10.000300, bearing 44.6, distance 46.8m, review after 23s'
),
call(
'Navigator, steering to +10.000300,+10.000300, bearing 36.4, distance 27.6m, review after 13s'
),
call(
'Navigator, steering to +10.000300,+10.000300, bearing 63.1, distance 12.3m, review after 6s'
),
call('Navigator, arrived at +10.000300,+10.000300')
])
def generate_circle(self,
radius,
center,
num_points,
max_forward_speed,
theta_offset=0.0,
heading_offset=0.0,
append=False):
if radius <= 0:
print 'Invalid radius, value=', radius
return False
if num_points <= 0:
print 'Invalid number of samples, value=', num_points
return False
if max_forward_speed <= 0:
print 'Invalid absolute maximum velocity, value=', max_forward_speed
return False
if not append:
# Clear current list
self.clear_waypoints()
step_theta = 2 * np.pi / num_points
for i in range(num_points):
angle = i * step_theta + theta_offset
x = np.cos(angle) * radius + center.x
y = np.sin(angle) * radius + center.y
z = center.z
wp = Waypoint(x, y, z, max_forward_speed, heading_offset)
self.add_waypoint(wp)
return True
def __init__(self, file):
try:
self.filename = file.name
except AttributeError:
self.filename = '(unknown)'
element = parse(file)
namespace = re.match('\{(.*)\}', element.getroot().tag).group(1)
ele_tag_name = '{%s}ele' % namespace
name_tag_name = '{%s}name' % namespace
time_tag_name = '{%s}time' % namespace
self.coords = []
for trkpt in element.findall('/{%s}trk/{%s}trkseg/{%s}trkpt'
% (namespace, namespace, namespace)):
lat = math.pi * float(trkpt.get('lat')) / 180.0
lon = math.pi * float(trkpt.get('lon')) / 180.0
ele_tag = trkpt.find(ele_tag_name)
ele = 0 if ele_tag is None else float(ele_tag.text)
time = trkpt.find(time_tag_name)
if time is None:
continue
dt = datetime.strptime(time.text, GPX_DATETIME_FORMAT)
coord = Coord(lat, lon, ele, dt)
self.coords.append(coord)
self.waypoints = []
for wpt in element.findall('/{%s}wpt' % namespace):
name = wpt.find(name_tag_name).text
lat = math.pi * float(wpt.get('lat')) / 180.0
lon = math.pi * float(wpt.get('lon')) / 180.0
ele_tag = wpt.find(ele_tag_name)
ele = 0 if ele_tag is None else float(ele_tag.text)
waypoint = Waypoint(name, lat, lon, ele)
self.waypoints.append(waypoint)
def __init__(self,
speed=0.15,
lane_offset=140,
wait_period=10,
hard_coded_turns=True):
self.hard_coded_turns = hard_coded_turns
self.speed = speed
self.pid = SteeringPid(lane_offset, kp=0.1, ki=0.006, kd=1.2)
self.intersections_pid = SteeringPid(1, kp=0.1, ki=0.006, kd=1.2)
self.current_turn = None
self.current_turn_direction = None
self.handling_intersection = False
self.inter_start_time = time.time()
self.go_straight = False
self.angle = 0
self.waypoint = Waypoint()
self.car = Car_Control()
self.detector = Image_Process()
self.vs = cv2.VideoCapture(
"/dev/video2",
cv2.CAP_V4L) # TODO: figure out a good way to do this
self.intersections = Intersections(wait_period=wait_period)
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.pipeline.start(self.config)
def post_waypoint() -> str:
post = request.get_json() if request.is_json else json.loads(
request.get_data())
position = post['position']
lat = position['latitude']
lon = position['longitude']
controller.add_waypoint(Waypoint(x=lon, y=lat))
return '{"status": "success"}'
def test_should_signal_a_navigate_event_using_the_first_waypoint(self):
self.listen(EventName.navigate)
firstwaypoint = Waypoint(Position(1, 1), 5)
follower = Follower(self.exchange, [firstwaypoint], self.mock_logger)
self.exchange.publish(Event(EventName.start))
self.assertEqual(self.last_event.name, EventName.navigate)
self.assertEqual(self.last_event.waypoint, firstwaypoint)
def test_should_steer_to_waypoint_if_outside_tolerance(self):
self.listen(EventName.set_course)
waypoint = Waypoint(Position(53.0001,-1.9999),5)
expected_bearing = self.globe.bearing(self.current_position,waypoint.position)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.set_course),1,"expected set course event following navigate")
self.assertEqual(self.last_event.heading,expected_bearing)
def test_should_not_steer_and_log_arrival_if_arrived(self):
self.listen(EventName.arrived)
self.listen(EventName.set_course)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,Waypoint(self.current_position,0)))
self.assertEqual(self.event_count(EventName.set_course),0,"expected no event to set course if we have arrived")
self.assertEqual(self.event_count(EventName.arrived),1,"expected arrived event if we have arrived")
self.mock_logger.info.assert_called_with('Navigator, arrived at {:+f},{:+f}'.format(self.current_position.latitude,self.current_position.longitude))
def test_should_use_minimum_steer_time_if_time_calculation_returns_small_value(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(53.0001,-1.9999),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 100
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,MIN_TIME_TO_STEER)
def get_valid_neighbours(self,current, grid):
valid_neighbours = []
for i in [0,1,-1]:
for j in [0,1,-1]:
for k in [0,1,2,3]:
new_waypoint = Waypoint(current.x + i, current.y + j, (current.orientation + k) % 4)
if pv.is_valid_waypoint(new_waypoint, grid) and pv.is_valid_transition(current, new_waypoint):
valid_neighbours.append(new_waypoint)
return valid_neighbours
def test_should_allow_a_tolerance_and_consider_errors_when_calculating_if_we_have_reached_waypoint(self):
self.listen(EventName.arrived)
self.listen(EventName.set_course)
waypoint = Waypoint(Position(53.0001,-1.9999),10)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.arrived),1,"expected arrived event if we have arrived")
self.mock_logger.info.assert_called_with('Navigator, arrived at {:+f},{:+f}'.format(waypoint.latitude,waypoint.longitude))
def __init__(self, filename=None, data_path=None):
# Based in Django's approach -> http://code.djangoproject.com/svn/django/trunk/django/__init__.py
self.version = __import__('pytrainer').get_version()
#Process command line options
self.startup_options = self.get_options()
#Setup logging
self.environment = Environment(platform.get_platform(),
self.startup_options.conf_dir)
self.environment.create_directories()
self.set_logging(self.startup_options.log_level,
self.startup_options.log_type)
logging.debug('>>')
logging.debug("pytrainer version %s" % (self.version))
self.data_path = data_path
self.date = Date()
self.ddbb = None
# Checking profile
logging.debug('Checking configuration and profile...')
self.profile = Profile(self.environment, self.data_path, self)
self.uc = UC()
self.windowmain = None
self.ddbb = DDBB(self.profile, self)
logging.debug('connecting to DDBB')
self.ddbb.connect()
initialize_data(self.ddbb, self.environment.conf_dir)
self._sport_service = SportService(self.ddbb)
self.record = Record(self._sport_service, data_path, self)
self.athlete = Athlete(data_path, self)
self.stats = Stats(self._sport_service, self)
pool_size = self.profile.getIntValue("pytraining",
"activitypool_size",
default=1)
self.activitypool = ActivityPool(self, size=pool_size)
#preparamos la ventana principal
self.windowmain = Main(self._sport_service,
data_path,
self,
self.version,
gpxDir=self.profile.gpxdir)
self.date = Date(self.windowmain.calendar)
self.waypoint = Waypoint(data_path, self)
self.extension = Extension(data_path, self)
self.plugins = Plugins(data_path, self)
self.importdata = Importdata(self._sport_service, data_path, self,
self.profile)
self.loadPlugins()
self.loadExtensions()
self.windowmain.setup()
self.windowmain.on_calendar_selected(None)
self.refreshMainSportList()
self.windowmain.run()
logging.debug('<<')
def test_should_use_a_minimum_speed_for_calculation_preventing_divide_by_zero_error(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(53.001,-2.001),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 0
expected_time = expected_time_to_steer(self.current_position,waypoint.position,0.01)
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,expected_time)
def test_should_use_maximum_steer_time_if_its_a_long_way_to_go(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(60.0001,10),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 1
expected_bearing = self.globe.bearing(self.current_position,waypoint.position)
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,MAX_TIME_TO_STEER)
def poll_events(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
raise InterruptedError('Program closed by user')
elif event.type == pygame.KEYDOWN and event.key == pygame.K_a:
self.draw_adjusted = not self.draw_adjusted
elif event.type == pygame.KEYDOWN and event.key == pygame.K_s:
self.draw_exact = not self.draw_exact
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
x, y = pygame.mouse.get_pos()
x, y = self.cart_to_coords(x, y)
self.add_waypoint(Waypoint(x, y))
def _create_waypoints_collection(self):
waypoints = [self.start_waypoint()]
if isinstance(self.primitive(), Path):
lane = self.start_waypoint().lane()
road_node = self.start_waypoint().road_node()
for element in self.primitive():
new_waypoint = Waypoint(lane, element.end_point(),
element.end_heading(), road_node)
waypoints.append(new_waypoint)
waypoints.pop(-1)
waypoints.append(self.end_waypoint())
return waypoints
def ipbrwps(self):
for m in self.ieach('PBRWPS,', PBRWPS_RE):
lat = int(m.group(1)) + float(m.group(2)) / 60
if m.group(3) == 'S':
lat *= -1
lon = int(m.group(4)) + float(m.group(5)) / 60
if m.group(6) == 'W':
lon *= -1
id = m.group(7)
name = m.group(8)
alt = int(m.group(9))
yield Waypoint(name, lat, lon, alt, id=id)
def children(self, wp):
children = []
# Forward
forward = vector[wp.orientation]
new_wp = Waypoint(wp.x + forward[0], wp.y + forward[1], wp.orientation)
children.append(new_wp)
# Forward Right
right = vector[(wp.orientation + 1) % 4]
new_wp = Waypoint(
wp.x + forward[0] + right[0], wp.y + forward[1] + right[1], (wp.orientation + 1) % 4)
children.append(new_wp)
# Forward Left
left = vector[(wp.orientation - 1) % 4]
new_wp = Waypoint(
wp.x + forward[0] + left[0], wp.y + forward[1] + left[1], (wp.orientation - 1) % 4)
children.append(new_wp)
# Backwards
left = vector[(wp.orientation - 1) % 4]
new_wp = Waypoint(wp.x - forward[0], wp.y - forward[1], wp.orientation)
children.append(new_wp)
# Backwards Right
new_wp = Waypoint(
wp.x - forward[0] - left[0], wp.y - forward[1] - left[1], (wp.orientation - 1) % 4)
children.append(new_wp)
# Backwards Left
new_wp = Waypoint(
wp.x - forward[0] - right[0], wp.y - forward[1] - right[1], (wp.orientation + 1) % 4)
children.append(new_wp)
return children
def read_from_file(self, filename):
if not os.path.isfile(filename):
print 'Invalid waypoint filename, file', filename
return False
try:
self.clear_waypoints()
with open(filename, 'r') as wp_file:
wps = yaml.load(wp_file)
if isinstance(wps, list):
for wp_data in wps:
wp = Waypoint(
x=wp_data['point'][0],
y=wp_data['point'][1],
z=wp_data['point'][2],
max_forward_speed=wp_data['max_forward_speed'],
heading_offset=wp_data['heading'],
use_fixed_heading=wp_data['use_fixed_heading'],
inertial_frame_id='world')
self.add_waypoint(wp)
self._inertial_frame_id = 'world'
else:
assert 'inertial_frame_id' in wps
assert 'waypoints' in wps
assert wps['inertial_frame_id'] in ['world', 'world_ned']
self._inertial_frame_id = wps['inertial_frame_id']
for wp_data in wps['waypoints']:
wp = Waypoint(
x=wp_data['point'][0],
y=wp_data['point'][1],
z=wp_data['point'][2],
max_forward_speed=wp_data['max_forward_speed'],
heading_offset=wp_data['heading'],
use_fixed_heading=wp_data['use_fixed_heading'],
inertial_frame_id=wps['inertial_frame_id'])
self.add_waypoint(wp)
except Exception, e:
print 'Error while loading the file'
print str(e)
return False
