def update(robot_id): # print "updating", robot_id robot.set_linear_velocity(robot_id, 1, 0, 0) robot.set_angular_velocity(robot_id, 0, 0, .1) # print robot.gazebo_pose(robot_id) # print robot.neighbors(robot_id) # print "pose:", robot.pose(robot_id) # print "search_area:", robot.search_area(robot_id) # print "camera",robot_id,":", robot.camera(robot_id) for neigh in robot.neighbors(robot_id): # print "->", neigh, robot.send_to(robot_id, "hi ", neigh) robot.send_to(robot_id, "hi ", neigh)
def update(robot_id): try: global old_phi ########## Get pose ##################### rx, ry, rz, rrx, rry, rrz = robot.gazebo_pose(robot_id) rho = math.sqrt(rx ** 2 + ry ** 2) phi = math.atan2(ry, rx) # Distance between two angles angle_distance = lambda phi1, phi2: math.atan2(math.sin(phi1 - phi2), math.cos(phi1 - phi2)) # delta phi dphi = angle_distance(phi, old_phi[robot_id]) # if old_phi[robot_id] == 0: # phi = dphi + 2 * math.pi # else: phi = old_phi[robot_id] + dphi old_phi[robot_id] = phi # Share location # for neigh in robot.neighbors(robot_id): # if neigh == "boo": # continue # # print "->", neigh, robot.send_to(robot_id, "hi ", neigh) # robot.send_to(robot_id, str(phi), neigh) # print "fff" ############# Send location to the neighbors ############## # Next in ring next = (robot_id + 1) % n next_ip = IP_FORMAT % ((robot_id + 2) % (n + 1)) if (robot_id + 2) % (n + 1) != 0 else IP_FORMAT % 1 # robot.send_to(robot_id, str(phi), next_ip) # # Before in ring before = (robot_id - 1) % n before_ip = IP_FORMAT % (robot_id % (n + 1)) if robot_id != 0 else IP_FORMAT % n # robot.send_to(robot_id, str(phi), before_ip) robot_ip = IP_FORMAT % ((robot_id + 1) % (n + 1)) on_data_received(next, robot_ip, next_ip, 55, str(phi)) on_data_received(before, robot_ip, before_ip, 55, str(phi)) # print robot_id, inbox # No inbox if robot_id not in inbox: print "Warning: no inbox for", robot_id return if before_ip not in inbox[robot_id] or next_ip not in inbox[robot_id]: print "Warning: no data from neighbors for", robot_id, "neighbours:", inbox[robot_id], (before_ip, next_ip) return if inbox[robot_id][before_ip] is None or inbox[robot_id][next_ip] is None: print "Warning: missed neighbor for", robot_id return ############### Controller ############## # Average Phi from neighbors back1 = float(inbox[robot_id][before_ip]) front1 = float(inbox[robot_id][next_ip]) phi_av = (back1 + front1) / 2. # phi_av = back1 + angle_distance(front1, back1) / 2. if robot_id == 0: phi_av -= math.pi if robot_id == n - 1: phi_av += math.pi # Control input dot_rho = kp * (R - rho) dot_phi = Omeg + kphi * (phi_av - phi) dot_alt = ka * (A - rz) # print robot_id, (front1, back1), angle_distance(front1, back1), phi_av, phi # print robot_id, math.degrees(phi_av - phi), (math.degrees(back1), math.degrees(front1)), \ # (math.degrees(phi_av), math.degrees(phi)) # Convert to cartesian coordinates dot_x = dot_rho * math.cos(phi) - rho * dot_phi * math.sin(phi) dot_y = dot_rho * math.sin(phi) + rho * dot_phi * math.cos(phi) # robot.set_linear_velocity(robot_id, dot_x, dot_y, dot_alt) # Print metric if robot_id == 1: target_phi = 2 * math.pi / n metrics = [(angle_distance(old_phi[i], old_phi[i - 1]) - target_phi) ** 2 for i in range(len(old_phi))] total_metric = sum(metrics) # robot.gzmsg(robot_id, "metric: %f" % total_metric) # Feedback linearization d = .1 vi = dot_x * math.cos(rrz) + dot_y * math.sin(rrz) wi = - dot_x * math.sin(rrz) / d + dot_y * math.cos(rrz) / d robot.set_linear_velocity(robot_id, vi, 0, 0) robot.set_angular_velocity(robot_id, 0, 0, wi) except: print "Unexpected error:", sys.exc_info() raise