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