def main(): start_pos = Vec2(0.25,1.75) start_speed = Vec2(0.0,-0.1) start = RobotState(start_pos,start_speed,0.0) goal_pos = Vec2(2.75,1.75) goal_speed = Vec2(0.0,0.0) goal = RobotState(goal_pos,goal_speed,0.0) square_vertices = [Vec2(0,0),Vec2(0.5,0),Vec2(0.5,0.5),Vec2(0,0.5)] square = Polygon(square_vertices,True,True) obs1 = Affine.translation(Vec2(0.5,1.0)) * square obs2 = Affine.translation(Vec2(2.0,0.5)) * square env = Environment(Globals.PLAYGROUND_BORDER,[],[obs1,obs2]) #if poly_collides_poly(obs1,obs2): # print "(=" #else: # print ")=" #with PyCallGraph(output = GraphvizOutput()): path = aStar(start,goal,env) #print "start:\n {s}".format(s = start) #print "goal:\n {g}".format(g = goal) #for node in path: # print node while True: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() screen.fill(black) pygame.draw.lines(screen,white,True,convert_to_px_coords(env.border_as_tuple_list()),1) for obs in env.obstacles_as_tuples(): pygame.draw.lines(screen,red,True,convert_to_px_coords(obs),1) pygame.draw.lines(screen,green,False,convert_to_px_coords(Util.path_to_tuples(path)),1) for node in path: draw_state(node) pygame.display.update()
def calculateObjectPosition(x_pixel, y_pixel): # x_pixel offset = +5 and y_pixel offset = -5 # this is not done and should be set somewhere # Robots world coordinates seen from camera frame x_coordinate_robot_position = 34.7 y_coordinate_robot_position = 33.9 # Resolution in cm per pixel resolution_width = 70.0/611.0 resolution_height = 39.5/344.0 # Robots position seen from camera frame T1 = Affine.translation((x_coordinate_robot_position, y_coordinate_robot_position)) # Robots rotating frame opposite to camera frame - degrees rot = Affine.rotation(180) # Getting robot frame robot_frame = T1*rot # Actual distance seen from camera frame actual_distance_width = resolution_width*x_pixel actual_distance_height = resolution_height*y_pixel # Object position seen from camera frame P_yx_camera_frame = Vec2(actual_distance_height, actual_distance_width) # Object position seen from the robot frame P_xyz_robot_frame = ~robot_frame*P_yx_camera_frame return P_xyz_robot_frame
def _get_transform(self, dt): # Set rotation in analogue to direction of velocity position = Affine.translation(self._position + self._velocity * dt) return position
def _get_transform(self, dt): position = Affine.translation(self._position + self._velocity * dt) return position
def _get_transform(self, dt): # Set rotation in analogue to direction of velocity rotation = Affine.rotation(self._angle) position = Affine.translation(self._position_in(dt)) return position * rotation