def move_leader(robot, goal_list, max_speed):
global at_goal
global iterations
goal_position = goal_list[iterations]
# Position of robot in format [x,y]
robot_position = robot.get_position()
# Orientation of robot in radians from 0 to 2pi. Given in ACW direction from the positive x-axis.
robot_orientation = robot.get_orientation()
# The angle of the goal position given in radians in the same way as the orientation of the robot.
goal_angle = angles.angle_between_points(goal_position, robot_position)
# Target angle to aim for.
target_angle = angles.angle_difference(goal_angle, robot_orientation)
# Distance to the goal.
distance = vectors.distance_points(goal_position, robot_position)
# Spin the robot towards the desired orientation. In general a P-regulator should be enough.
twist.angular.z = 0.5 * target_angle
# 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.
if iterations == len(goal_list) - 1:
twist.linear.x = distance * (
(math.pi - math.fabs(target_angle)) / math.pi)
else:
twist.linear.x = max_speed * (
(math.pi - math.fabs(target_angle)) / math.pi)
# If the robot is in position (within a margin), don't spin. This is due to the fact that the orientation it had
# earlier should be good enough, and it might end up spinning in circles if it gets too close to the target.
# Also let the robot know whether it is in position or not.
tol = 0.2
if distance < tol:
if iterations == len(goal_list) - 1:
robot.set_at_position(True)
twist.angular.z = 0
twist.linear.x = 0
else:
iterations += 1
elif distance >= tol:
robot.set_at_position(False)
# If the robot is moving too fast, slow down please. It shouldn't be possible to get a negative speed but in case
# that happens, just set the speed to 0.
if twist.linear.x > max_speed:
twist.linear.x = max_speed
elif twist.linear.x < 0:
twist.linear.x = 0
# 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
robot.pub.publish(twist)
def move_leader(robot, goal, max_speed):
global iterations
goal_position = goal[iterations]
print goal_position
global at_goal
# Position of robot in format [x,y]
robot_position = robot.get_position_simulation()
# Orientation of robot in radians from 0 to 2pi
robot_orientation = robot.get_orientation()
# The angle of the goal position given in radians in the same way as the orientation
# of the robot.
goal_ang = angles.angle_between_points(goal_position, robot_position)
tar_ang = angles.angle_difference(goal_ang, robot_orientation)
# Distance to the goal from current position.
distance = vectors.distance_points(goal_position, robot_position)
# A tolerance level to decide whether the robot is at the goal or not.
tol = 0.1
twist.angular.z = 0.5 * tar_ang
if iterations == len(goal) - 1:
twist.linear.x = distance * (
(math.pi - math.fabs(tar_ang)) / math.pi)**2
else:
twist.linear.x = max_speed * (
(math.pi - math.fabs(tar_ang)) / math.pi)**2
if distance < tol:
if iterations == len(goal) - 1:
robot.set_at_position(True)
twist.angular.z = 0
else:
iterations += 1
elif distance >= tol:
robot.set_at_position(False)
# Make sure the robot is not moving too fast and neither backwards.
if twist.linear.x > max_speed:
twist.linear.x = max_speed
elif twist.linear.x < 0:
twist.linear.x = 0
# If all robots are at goal, then stop.
if at_goal:
twist.linear.x = 0
twist.angular.z = 0
robot.pub.publish(twist)
# This code can be uncommented/commented if you would like to store information about the robots so that you can use
# it later.
"""
def move_robot_to_goal(robot, goal_position, max_speed):
global at_goal
# Position of robot in format [x,y]
robot_position = robot.get_position_simulation()
# Orientation of robot in radians from 0 to 2pi
robot_orientation = robot.get_orientation()
# The angle of the goal position given in radians in the same way as the orientation
# of the robot.
goal_angle = angles.angle_between_points(goal_position, robot_position)
target_angle = angles.angle_difference(goal_angle, robot_orientation)
distance = vectors.distance_points(goal_position, robot_position)
# A tolerance level to decide whether the robot is at the goal or not.
tol = 0.1
twist.angular.z = 0.5 * target_angle
twist.linear.x = distance * (
(math.pi - math.fabs(target_angle)) / math.pi)**2
if distance < tol:
robot.set_at_position(True)
twist.angular.z = 0
elif distance >= tol:
robot.set_at_position(False)
# Make sure the robot is not moving too fast and neither backwards.
if twist.linear.x > max_speed:
twist.linear.x = max_speed
elif twist.linear.x < 0:
twist.linear.x = 0
# If all robots are at goal, then stop.
if at_goal:
twist.linear.x = 0
twist.angular.z = 0
robot.pub.publish(twist)
def set_leader(goal, robots, simulation):
positions = [0] * len(robots)
for i in range(0, len(robots)):
if simulation:
positions[i] = robots[i].get_position_simulation()
else:
positions[i] = robots[i].get_position()
if vectors.distance_points(goal, positions[0]) < vectors.distance_points(goal, positions[1]) and \
vectors.distance_points(goal, positions[0]) < vectors.distance_points(goal, positions[2]):
robots[0].set_node_in_system(0)
robots[1].set_node_in_system(1)
robots[2].set_node_in_system(2)
elif vectors.distance_points(goal, positions[1]) < vectors.distance_points(goal, positions[0]) and \
vectors.distance_points(goal, positions[1]) < vectors.distance_points(goal, positions[2]):
robots[0].set_node_in_system(1)
robots[1].set_node_in_system(0)
robots[2].set_node_in_system(2)
else:
robots[0].set_node_in_system(2)
robots[1].set_node_in_system(1)
robots[2].set_node_in_system(0)
def find_path(goal, robots, minx, maxx, miny, maxy):
global subscriber
subscriber = rospy.Subscriber("/position", Pos, path_callback, robots)
# Make sure we've collected the data by waiting a while (the subscriber will unsubscribe itself when done).
time.sleep(1)
# Set the robot closest to the target as leader.
leader_assignment.set_leader(goal, robots, False)
# Find the robots' positions.
for i in range(0, len(robots)):
if robots[i].get_node_in_system() == 0:
leader_position = robots[i].get_position()
elif robots[i].get_node_in_system() == 1:
follower_1_position = robots[i].get_position()
elif robots[i].get_node_in_system() == 2:
follower_2_position = robots[i].get_position()
# Now let's create a matrix representing the space in which the robots operate.
# Number of intervals per meter.
scale = 10
nx = int((maxx - minx) * scale)
ny = int((maxy - miny) * scale)
# A matrix filled with zeros representing the area that the camera sees.
matrix = [[0 for j in range(ny)] for i in range(nx)]
# The start and goal positions represented in the matrix.
start_matrix = (int((leader_position[0] - minx) * scale),
int((leader_position[1] - miny) * scale))
goal_matrix = (int((goal[0] - minx) * scale), int(
(goal[1] - miny) * scale))
# Place followers in matrix and add some margins around them so that the leader at least not is aiming straight
# towards them.
follower_1_position_matrix = [
int((follower_1_position[0] - minx) * scale),
int((follower_1_position[1] - miny) * scale)
]
follower_2_position_matrix = [
int((follower_2_position[0] - minx) * scale),
int((follower_2_position[1] - miny) * scale)
]
# Set size of the followers so that they are represented correctly in the matrix. Number in meters, later scaled.
width_follower = 0.7 * scale
# Place ones in the matrix where the followers are + a region around them so that the leader will not move there.
for i in range(nx):
for j in range(ny):
if vectors.distance_points(follower_1_position_matrix, [i,j]) <= width_follower or \
vectors.distance_points(follower_2_position_matrix, [i, j]) <= width_follower:
matrix[i][j] = 1
# Place obstacles in matrix and add some space around them so that they are not just one matrix element wide.
global obstacles
width_obstacle = 1 * scale
if obstacles[0] != 0:
obstacle_0_position_matrix = [
int((obstacles[0][0] - minx) * scale),
int((obstacles[0][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_0_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
if obstacles[1] != 0:
obstacle_1_position_matrix = [
int((obstacles[1][0] - minx) * scale),
int((obstacles[1][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_1_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
# Find the shortest path using the A* algorithm
cam_area = numpy.array(matrix)
path = astar.astar(cam_area, goal_matrix, start_matrix)
# Shift the path so that it corresponds to the actual area that is considered.
if path is not False:
for i in range(len(path)):
path[i] = vectors.add(vectors.multiply(path[i], 1. / scale),
[minx, miny])
# It seems to be a good idea to not tell the robot to go to a point really close to it, so just cut out the first
# 5 or so points. Also add the goal position at the end since it is not given from the A* algorithm.
short_path = [0 for i in range(len(path) - 4)]
for i in range(5, len(path)):
short_path[i - 5] = path[i]
short_path[len(short_path) - 1] = goal
return short_path
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 find_path(goal, robots, minx, maxx, miny, maxy):
# Set the robot closest to the target as leader. Either check them here or go to a formation at first.
leader_assignment.set_leader(goal, robots, True)
# Find the robots' positions.
for i in range(0, len(robots)):
if robots[i].get_node_in_system() == 0:
leader_position = robots[i].get_position_simulation()
elif robots[i].get_node_in_system() == 1:
follower_1_position = robots[i].get_position_simulation()
elif robots[i].get_node_in_system() == 2:
follower_2_position = robots[i].get_position_simulation()
# Now let's create a matrix representing the space in which the robots operate.
# 10 intervals per meter
scale = 10
nx = int((maxx - minx) * scale)
ny = int((maxy - miny) * scale)
matrix = [[0 for i in range(ny)] for j in range(nx)]
# Place the goal and start in the matrix (or get their representation at least).
start_matrix = (int((leader_position[0] - minx) * scale),
int((leader_position[1] - miny) * scale))
goal_matrix = (int((goal[0] - minx) * scale), int(
(goal[1] - miny) * scale))
# Place followers in matrix and add some margins around them so that the leader at least not is aiming straight
# towards them.
follower_1_position_matrix = [
int((follower_1_position[0] - minx) * scale),
int((follower_1_position[1] - miny) * scale)
]
follower_2_position_matrix = [
int((follower_2_position[0] - minx) * scale),
int((follower_2_position[1] - miny) * scale)
]
# How big are they (in meters * scale)?
width_follower = 0.8 * scale
for i in range(nx):
for j in range(ny):
if vectors.distance_points(follower_1_position_matrix, [i,j]) <= width_follower or \
vectors.distance_points(follower_2_position_matrix, [i, j]) <= width_follower:
matrix[i][j] = 1
# Place obstacles in matrix and add some space around them so that they are not just one matrix element wide.
global obstacles
width_obstacle = 1.5 * scale
if obstacles[0] != [0]:
obstacle_0_position_matrix = [
int((obstacles[0][0] - minx) * scale),
int((obstacles[0][1] - miny) * scale)
]
print obstacle_0_position_matrix
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_0_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
if obstacles[1] != [0]:
obstacle_1_position_matrix = [
int((obstacles[1][0] - minx) * scale),
int((obstacles[1][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_1_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
# Find the shortest path using the A* algorithm
sim_area = numpy.array(matrix)
path = astar.astar(sim_area, goal_matrix, start_matrix)
# Shift the path so that it corresponds to the actual area that is considered.
if path is not False:
for i in range(len(path)):
path[i] = vectors.add(vectors.multiply(path[i], 1. / scale),
[minx, miny])
# It seems to be a good idea to not tell the robot to go to a point really close to it, so just cut out the first
# 5 or so points. Also add the goal position at the end since it is not given from the A* algorithm.
short_path = [0 for i in range(len(path) - 4)]
for i in range(4, len(path) - 1):
short_path[i - 4] = path[i]
short_path[len(short_path) - 1] = goal
return short_path
def assign_nodes(robots, nodes, simulation):
# These are the cost variables, one for each robot
c_0 = 1
c_1 = 1
c_2 = 1
# Distances is a matrix containing information about the distance between
# all robots to all nodes, represented as:
# Distance from robot A to node B = distances[A][B]
distances = [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
if simulation:
for i in range(0, len(robots)):
for j in range(0, len(nodes)):
distances[i][j] = vectors.distance_points(
robots[i].get_position_simulation(), nodes[j])
else:
for i in range(0, len(robots)):
for j in range(0, len(nodes)):
distances[i][j] = vectors.distance_points(
robots[i].get_position(), nodes[j])
# These are all potential outcomes of the robot-to-node assignment, calculated as an overall cost with distance
# travelled * cost variable. It is designed so that no two robots go to the same node, and no robot goes to more
# than one node (duh).
# *** OBS *** CALCULATED MANUALLY AND SHOULD NOT BE TAMPERED WITH
# ------------------------------------------------------------------------------------------------------------------
# *** YOU ARE NOW ENTERING THE DANGER ZONE ***
# ------------------------------------------------------------------------------------------------------------------
v1 = distances[0][0] * c_0 + distances[1][1] * c_1 + distances[2][2] * c_2
v2 = distances[2][0] * c_2 + distances[1][1] * c_1 + distances[0][2] * c_0
v3 = distances[1][0] * c_1 + distances[2][1] * c_2 + distances[0][2] * c_0
v4 = distances[0][0] * c_0 + distances[2][1] * c_2 + distances[1][2] * c_1
v5 = distances[1][0] * c_1 + distances[0][1] * c_0 + distances[2][2] * c_2
v6 = distances[2][0] * c_2 + distances[0][1] * c_0 + distances[1][2] * c_1
# ------------------------------------------------------------------------------------------------------------------
# *** YOU ARE NOW LEAVING THE DANGER ZONE ***
# ------------------------------------------------------------------------------------------------------------------
# List used in determination of which "version" offers minimal cost
list_of_distances = [v1, v2, v3, v4, v5, v6]
# Variable "version_index" is used in saving minimal cost path and finally returning the correct dictionary. Should
# not be modified!
version_index = 1
# Minimization happens here. It should not be modified.
for i in range(0, 6):
if list_of_distances[i] < list_of_distances[version_index - 1]:
version_index = i + 1
# Dictionary containing all possible final robot-to-node assignments. The dictionary contains 6 dictionaries, where
# each internal dictionary is a possible outcome of the assignment.
# *** OBS *** CALCULATED MANUALLY AND SHOULD NOT BE TAMPERED WITH
# -------------------------------------------------------------------
# *** YOU ARE NOW ENTERING THE DANGER ZONE ***
# --------------------------------------------------------------------
dict = {
1: {
"robot_0": 0,
"robot_1": 1,
"robot_2": 2
},
2: {
"robot_0": 2,
"robot_1": 1,
"robot_2": 0
},
3: {
"robot_0": 2,
"robot_1": 0,
"robot_2": 1
},
4: {
"robot_0": 0,
"robot_1": 2,
"robot_2": 1
},
5: {
"robot_0": 1,
"robot_1": 0,
"robot_2": 2
},
6: {
"robot_0": 1,
"robot_1": 2,
"robot_2": 0
}
}
# -------------------------------------------------------------------
# *** YOU ARE NOW LEAVING THE DANGER ZONE ***
# --------------------------------------------------------------------
nodes = dict.get(version_index)
robots[0].set_node_in_system(nodes.get("robot_0"))
robots[1].set_node_in_system(nodes.get("robot_1"))
robots[2].set_node_in_system(nodes.get("robot_2"))