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 orientation_callback(data, args):
robots = args[0]
goal_orientation = args[1]
tol = 0.05 # Tolerance in radians for accepting the robot as being in the correct orientation.
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]
}
for i in range(0, len(robots)):
robot = robots[i]
# If not able to detect the tag, use last information recieved.
if camera.get(1) is not None:
position_back = camera.get(i * 2 + 1)
robot.set_position(position_back)
if camera.get(2) is not None:
position_front = camera.get(i * 2 + 2)
robot.set_front_position(position_front)
position_front = robot.get_front_position()
position_back = robot.get_position()
orientation = math.atan2(position_front[1] - position_back[1],
position_front[0] - position_back[0])
if orientation < 0:
orientation += 2 * math.pi
target_angle = angles.angle_difference(goal_orientation, orientation)
if math.fabs(target_angle) > tol:
twist.angular.z = 0.4 * target_angle
robot.set_at_position(False)
else:
twist.angular.z = 0
robot.set_at_position(True)
robot.pub.publish(twist)
def orientation_callback(data, args):
robot = args[0]
goal_orientation = args[1]
tol = 0.01 # Tolerance in radians for accepting the robot as being in the correct orientation.
orientation = transformCoordinates.from_quaternion_to_radians(data)
if orientation < 0:
orientation += 2 * math.pi
target_angle = angles.angle_difference(goal_orientation, orientation)
if math.fabs(target_angle) > tol:
twist.angular.z = target_angle
robot.set_at_position(False)
else:
twist.angular.z = 0
robot.set_at_position(True)
robot.pub.publish(twist)
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 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)