-
Notifications
You must be signed in to change notification settings - Fork 0
/
mapping.py
112 lines (91 loc) · 3.95 KB
/
mapping.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import pygame
import sys
import time
import math
import argparse
from RobotLib.FrontEnd import *
from RobotLib.IO import *
from RobotLib.Math import *
from Robot import Robot
from OccupancyGrid import OccupancyGrid
#from ObstacleMap import ObstacleMap as OccupancyGrid
from RangeFinder import RangeFinder
import numpy as np
import operator
class MyFrontEnd(FrontEnd):
def __init__(self,omap_path,sparki):
self.omap = OccupancyGrid(omap_path)
FrontEnd.__init__(self,self.omap.width,self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width*0.5
self.robot.y = self.omap.height*0.5
def keydown(self,key):
# update velocities based on key pressed
if key == pygame.K_UP: # set positive linear velocity
self.robot.lin_vel = 20.0
elif key == pygame.K_DOWN: # set negative linear velocity
self.robot.lin_vel = -20.0
elif key == pygame.K_LEFT: # set positive angular velocity
self.robot.ang_vel = 100.*math.pi/180.
elif key == pygame.K_RIGHT: # set negative angular velocity
self.robot.ang_vel = -100.*math.pi/180.
elif key == pygame.K_k: # set positive servo angle
self.robot.sonar_angle = 45.*math.pi/180.
elif key == pygame.K_l: # set negative servo angle
self.robot.sonar_angle = -45.*math.pi/180.
def keyup(self,key):
# update velocities based on key released
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero linear velocity
self.robot.lin_vel = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero angular velocity
self.robot.ang_vel = 0
elif key == pygame.K_k or key == pygame.K_l: # set zero servo angle
self.robot.sonar_angle = 0
def draw(self,surface):
# draw obstacle map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def update(self,time_delta):
T_sonar_map = self.robot.get_robot_map_transform()*self.robot.get_sonar_robot_transform()
# Get sonar x and y for center point of sonar in map frame.
#print(T_sonar_map)
#print(T_sonar_map.item(2))
#print(T_sonar_map.item(5))
x = T_sonar_map.item(2)
y = T_sonar_map.item(5)
if self.sparki.port == '':
# calculate sonar distance from map
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
# Send range finder measurement to RangeFinder class
#print(self.robot.sonar_distance)
#print(self.omap.log_odds)
rangefinder = RangeFinder(self.robot.sonar_distance, self.omap.log_odds, x, y)
log_odds = rangefinder.update(self.robot.sonar_distance, self.omap.log_odds, x, y)
self.omap.log_odds = log_odds
else:
# get current rangefinder reading
self.robot.sonar_distance = self.sparki.dist
# calculate servo setting
servo = int(self.robot.sonar_angle*180./math.pi)
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors()
# send command
self.sparki.send_command(left_speed,left_dir,right_speed,right_dir,servo)
# update robot position
self.robot.update(time_delta)
def main():
# parse arguments
parser = argparse.ArgumentParser(description='Bayesian mapping demo')
parser.add_argument('--omap', type=str, default='map.png', help='path to obstacle map png file')
parser.add_argument('--port', type=str, default='', help='port for serial communication')
args = parser.parse_args()
with SparkiSerial(port=args.port) as sparki:
# make frontend
frontend = MyFrontEnd(args.omap,sparki)
# run frontend
frontend.run()
if __name__ == '__main__':
main()