def draw(self): """Draws all visible tiles""" def adjustForStupidWindowCoordinates(y): return render.window.height - self.tileDimensions[1] - y def is_even(n): return n%2 == 0 tilewidth, tileheight = self.tileDimensions visiblearea = self.get_visible_area() if visiblearea == None: return xvisible, yvisible = visiblearea for y in range(yvisible[0], yvisible[1]+1): shift = 0 if not is_even(y): shift = tilewidth/2 for x in range(xvisible[0], xvisible[1]+1): sprite = self.tilemap.getSprite(x, y) if not sprite: continue #*wrto == "with respect to" spritePosX_wrto_map = (x*tilewidth) + shift spritePosY_wrto_map = y*(tileheight/2) spritePosInWindow = vectors.subtract( [spritePosX_wrto_map, spritePosY_wrto_map], self.viewPosition ) spritePosInWindow[1] = adjustForStupidWindowCoordinates(spritePosInWindow[1]) sprite.set_position(spritePosInWindow[0], spritePosInWindow[1]) sprite.draw()
def draw(self): """Draws all visible tiles""" def adjustForStupidWindowCoordinates(y): return render.window.height - self.tileDimensions[1] - y def is_even(n): return n % 2 == 0 tilewidth, tileheight = self.tileDimensions visiblearea = self.get_visible_area() if visiblearea == None: return xvisible, yvisible = visiblearea for y in range(yvisible[0], yvisible[1] + 1): shift = 0 if not is_even(y): shift = tilewidth / 2 for x in range(xvisible[0], xvisible[1] + 1): sprite = self.tilemap.getSprite(x, y) if not sprite: continue #*wrto == "with respect to" spritePosX_wrto_map = (x * tilewidth) + shift spritePosY_wrto_map = y * (tileheight / 2) spritePosInWindow = vectors.subtract( [spritePosX_wrto_map, spritePosY_wrto_map], self.viewPosition) spritePosInWindow[1] = adjustForStupidWindowCoordinates( spritePosInWindow[1]) sprite.set_position(spritePosInWindow[0], spritePosInWindow[1]) sprite.draw()
def find_widget(self, pos): for widget in self.subwidgets[::-1]: if widget.visible: r = widget.rect if r.collidepoint(pos): return widget.find_widget(subtract(pos, r.topleft)) return self
def draw(self): """Draws all visible tiles""" def adjustForStupidWindowCoordinates(y): return render.window.height - tileheight - y tilewidth, tileheight = self.getTileDimensions() visiblearea = self.get_visible_area() if visiblearea == None: return xvisible, yvisible = visiblearea for y in range(yvisible[0], yvisible[1]+1): #we need to move the tiles over (because they aren't squares) #to render them properly. shift = self.rowshift((tilewidth/2), y) for x in range(xvisible[0], xvisible[1]+1): #tilemap gets us the correct sprite based on our coordinates. sprite = self.tilemap.getSprite(x + self.rowshift(0.5, y), y) if not sprite: continue #*wrto == "with respect to" spritePosX_wrto_map = (x*tilewidth) + shift spritePosY_wrto_map = y*(tileheight/2) spritePosInWindow = vectors.subtract( [spritePosX_wrto_map, spritePosY_wrto_map], self.viewPosition ) spritePosInWindow[1] = adjustForStupidWindowCoordinates(spritePosInWindow[1]) sprite.set_position(spritePosInWindow[0], spritePosInWindow[1]) sprite.scale = self.scalefactor sprite.draw()
def global_to_local(self, p): return subtract(p, self.local_to_global_offset())
def move_follower(robot, robots, max_speed, pid_leader, pid_follower, distances): global at_goal # Find what the other robots are in the system. for i in range(0, len(robots)): if robots[i].get_node_in_system() == 0: leader = robots[i] elif (robots[i].get_node_in_system() != 0) and (robots[i] is not robot): follower = robots[i] # Desired distances to keep to the other robots. Leader and follower are either correctly assigned or something else # is terribly wrong. Shouldn't initialise them as a safety since that wouldn't allow you to identify the problem. desired_distance_leader = distances[leader.get_node_in_system()][ robot.get_node_in_system()] desired_distance_follower = distances[follower.get_node_in_system()][ robot.get_node_in_system()] # The robots' positions. robot_position = robot.get_position() follower_position = follower.get_position() leader_position = leader.get_position() # This follower's orientation robot_orientation = robot.get_orientation() # The orientation of the leader leader_orientation = leader.get_orientation() # Calculate the actual distances to the other robots. distance_leader = vectors.distance_points(leader_position, robot_position) distance_follower = vectors.distance_points(follower_position, robot_position) # Use PID-controller to go to a position which is at the desired distance from both the other robots. error_leader = distance_leader - desired_distance_leader error_follower = distance_follower - desired_distance_follower u_leader = pid_leader.pid(error_leader) u_follower = pid_follower.pid(error_follower) # u is always going to be a positive value. As long as the robots don't overshoot this shouldn't be a problem, but # if they do they might keep moving and even crash into the leader. Have this in consideration. u = math.sqrt(u_follower**2 + u_leader**2) # Create vectors to the leader, the other follower, the orientation of the leader and from these decide on a # direction vector, which this follower should move along. The further away the robot is from either the leader or # the other follower, the bigger the tendency is to move towards that robot. If it's more or less at the right # distance from the other robots it is favorable to move in the orientation of the leader, which is implemented by # the usage of the variable scale which is an exponential decaying function squared and scaled by 2/3, in other # words the follower will only move in the direction of the leader's orientation if it's more or less perfectly in # the right position. if u_leader != 0 and u_follower != 0: vector_leader = vectors.multiply( vectors.normalise(vectors.subtract(leader_position, robot_position)), u_leader * u_follower) else: vector_leader = vectors.multiply( vectors.normalise(vectors.subtract(leader_position, robot_position)), 0.001) if u_follower != 0: vector_follower = vectors.multiply( vectors.normalise( vectors.subtract(follower_position, robot_position)), u_follower) else: vector_follower = vectors.multiply( vectors.normalise( vectors.subtract(follower_position, robot_position)), 0.001) scale = (math.exp(-u)**2) * 2 / 3 vector_orientation_leader = vectors.multiply( [math.cos(leader_orientation), math.sin(leader_orientation)], scale) direction_vector = vectors.add(vectors.add(vector_follower, vector_leader), vector_orientation_leader) # Calculate a goal angle from the direction vector. goal_angle = math.atan2(direction_vector[1], direction_vector[0]) if goal_angle < 0: goal_angle += 2 * math.pi # Calculate a target angle from the goal angle and the orientation of this robot. target_angle = angles.angle_difference(goal_angle, robot_orientation) # Spin the robot towards the desired orientation. In general a P-regulator should be enough. twist.angular.z = target_angle * 0.5 # Move the robot forward. The further away it is from the goal, as well as earlier error and predicted future error # by the PID is considered in the variable u. Also the robot will move by full speed when oriented correctly, but # slower the further away it is from its desired orientation given by target_angle. twist.linear.x = u * math.fabs(math.pi - math.fabs(target_angle)) / math.pi # If the robot is within an acceptable range, named tol, it is considered "in position". The robot will still keep # moving though until all robots are at the goal, which is taken care of later. tol = 0.05 if math.fabs(error_follower) < tol and math.fabs(error_leader) < tol: robot.set_at_position(True) else: robot.set_at_position(False) # Make sure the robot is not moving too fast. if twist.linear.x > max_speed: twist.linear.x = max_speed if u_leader < 0: twist.linear.x = 0.07 # If all the robots are at goal we have to stop moving of course. if at_goal: twist.linear.x = 0 twist.angular.z = 0 # If the robots are colliding it would be a good thing to just stop before an accident happens. if distance_leader < 0.7 or distance_follower < 0.5: twist.linear.x = 0 twist.angular.z = 0 robot.pub.publish(twist)
def loadMapMap(self, filepath): """This loads the image file with map data""" im = Image.open(filepath) import vectors self.mapDimensions = vectors.subtract(im.size, (1, 1)) self.mapimage = im.load()
def test_can_subtract_vectors(self): vector_a = [5, 3] vector_b = [4, 1] self.assertEqual(vectors.subtract(vector_a, vector_b), [1, 2])