def formation_callback(data, args):
global nodes_assigned
robots = args[0]
goal_positions = args[1]
camera = {
data.tagid1: [data.x1, data.y1],
data.tagid2: [data.x2, data.y2],
data.tagid3: [data.x3, data.y3],
data.tagid4: [data.x4, data.y4],
data.tagid5: [data.x5, data.y5],
data.tagid6: [data.x6, data.y6],
data.tagid7: [data.x7, data.y7],
data.tagid8: [data.x8, data.y8]
}
# Assign where the robots belong in the system (shortest total distance required to travel to be in formation).
simulation = False
robot_to_node_assignment.assign_nodes(robots, goal_positions, simulation)
# Move all the robots in the system to their desired goal.
for i in range(0, len(robots)):
robot = robots[i]
# Convert the tags' positions to meters. If not able to detect the tag, use last information recieved.
if camera.get(1) is not None:
robot_position_back = vectors.multiply(camera.get(i * 2 + 1),
0.001)
robot.set_position(robot_position_back)
if camera.get(2) is not None:
robot_position_front = vectors.multiply(camera.get(i * 2 + 2),
0.001)
robot.set_front_position(robot_position_front)
robot_position_front = robot.get_front_position()
robot_position_back = robot.get_position()
# Calculate orientation of robot, it should take a value from 0 to 2pi
robot_orientation = math.atan2(
robot_position_front[1] - robot_position_back[1],
robot_position_front[0] - robot_position_back[0])
if robot_orientation < 0:
robot_orientation += 2 * math.pi
# Set orientation of robot, so that it can be used later on when needed.
robot.set_orientation(robot_orientation)
# Find the specific robot's goal using information calculated in the node assignment algorithm.
goal_position = goal_positions[robot.get_node_in_system()]
# It is always a good idea to limit the speed when using real robots with risk of collisions.
max_speed = 0.2
move_robot_to_goal(robot, goal_position, max_speed)
def path_callback(data, robots):
camera = {
data.tagid1: [data.x1, data.y1],
data.tagid2: [data.x2, data.y2],
data.tagid3: [data.x3, data.y3],
data.tagid4: [data.x4, data.y4],
data.tagid5: [data.x5, data.y5],
data.tagid6: [data.x6, data.y6],
data.tagid7: [data.x7, data.y7],
data.tagid8: [data.x8, data.y8]
}
if camera.get(1) is not None:
robot_0_position = vectors.multiply(camera.get(1), 0.001)
robots[0].set_position(robot_0_position)
else:
print "Can't see robot 0"
if camera.get(3) is not None:
robot_1_position = vectors.multiply(camera.get(3), 0.001)
robots[1].set_position(robot_1_position)
else:
print "Can't see robot 1"
if camera.get(5) is not None:
robot_2_position = vectors.multiply(camera.get(5), 0.001)
robots[2].set_position(robot_2_position)
else:
print "Can't see robot 2"
global obstacles
if camera.get(7) is not None:
obstacle_0_position = vectors.multiply(camera.get(7), 0.001)
obstacles[0] = obstacle_0_position
else:
obstacles[0] = 0
if camera.get(8) is not None:
obstacle_1_position = vectors.multiply(camera.get(8), 0.001)
obstacles[1] = obstacle_1_position
else:
obstacles[1] = 0
global subscriber
subscriber.unregister()
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 move_formation_callback(data, args):
camera = {
data.tagid1: [data.x1, data.y1],
data.tagid2: [data.x2, data.y2],
data.tagid3: [data.x3, data.y3],
data.tagid4: [data.x4, data.y4],
data.tagid5: [data.x5, data.y5],
data.tagid6: [data.x6, data.y6],
data.tagid7: [data.x7, data.y7],
data.tagid8: [data.x8, data.y8]
}
robots = args[0]
goal = args[1]
distances = args[2]
pid_list = args[3]
# Calculate the positions and orientations of the robots in the system and store them in their respective objects.
for i in range(0, len(robots)):
robot = robots[i]
# Convert the tags' positions to meters. If not able to detect the tag, use last information recieved.
if camera.get(1) is not None:
robot_position_back = vectors.multiply(camera.get(i * 2 + 1),
0.001)
robot.set_position(robot_position_back)
if camera.get(2) is not None:
robot_position_front = vectors.multiply(camera.get(i * 2 + 2),
0.001)
robot.set_front_position(robot_position_front)
robot_position_front = robot.get_front_position()
robot_position_back = robot.get_position()
# Calculate orientation of robot, it should take a value from 0 to 2pi
robot_orientation = math.atan2(
robot_position_front[1] - robot_position_back[1],
robot_position_front[0] - robot_position_back[0])
if robot_orientation < 0:
robot_orientation += 2 * math.pi
# Set orientation of robot, so that it can be used later on when needed.
robot.set_orientation(robot_orientation)
# Check if the robot is a leader or follower and take action accordingly.
for i in range(0, len(robots)):
max_speed_leader = 0.1
max_speed_follower = 0.2
if robots[i].get_node_in_system() == 0:
move_leader(robots[i], goal, max_speed_leader)
elif robots[i].get_node_in_system() == 1:
pid_leader = pid_list[0]
pid_follower = pid_list[1]
move_follower(robots[i], robots, max_speed_follower, pid_leader,
pid_follower, distances)
elif robots[i].get_node_in_system() == 2:
pid_leader = pid_list[2]
pid_follower = pid_list[3]
move_follower(robots[i], robots, max_speed_follower, pid_leader,
pid_follower, distances)
else:
print "---WARNING--- THIS ROBOTS IS NEITHER LEADER NOR FOLLOWER. ---WARNING---"
def update(self, slowvalue):
for p in self.particles:
p.pos = vectors.add_vec(
p.pos, vectors.multiply(config.dt * slowvalue, p.v))
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