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 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 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 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])