def calc_min_neighbor_dist(self, robot_name): agents_copy = copy.copy(self.agents) current_agent = self.agents[robot_name] current_agent_pos_2d = self.sim.getAgentPosition(current_agent) current_agent_pos = Point(current_agent_pos_2d[0], current_agent_pos_2d[1], 0) agents_copy.pop(robot_name) min_dist = float('inf') for agent_name in agents_copy: agent_pos_2d = self.sim.getAgentPosition(self.agents[agent_name]) agent_pos = Point(agent_pos_2d[0], agent_pos_2d[1], 0) dist = current_agent_pos.get_distance_to(agent_pos) if dist < min_dist: min_dist = dist return min_dist
def run_orca(self): print('\nORCA3D for ' + str(len(self.agents)) + ' agents is running.') cont_flag = True while cont_flag: cont_flag = False self.sim.doStep() for key in self.robots.keys(): if not self.robots[key].finished_planning: cont_flag = True pos = self.sim.getAgentPosition(self.agents[key]) z = self.find_z(pos[0], pos[1]) robot_pos = Point(pos[0], pos[1], z) #gc.spawn_sdf_model(key + '_v_' + str(len(self.final_paths[key])), gc_const.VERTICE_PATH, robot_pos) self.final_paths[key].append(robot_pos) self.goal_achievement_check(key, robot_pos) self.robots[key].last_point = robot_pos print('ORCA3D for ' + str(len(self.agents)) + ' agents is completed!') self.waypoints_pub = {} for key in self.robots.keys(): self.waypoints_pub[key] = rospy.Publisher('/sim_' + key + '/waypoints_array', Path, queue_size=10) final_path = self.final_paths[key] path = deleting_intermediate_points(final_path) gc.visualise_path(path, gc_const.PATH_COLORS[int(self.agents[key])], key + '_v_') msg = prepare_path_msg(key, final_path) self.waypoints_pub[key].publish(msg) sleep(1) for key in self.robots.keys(): self.robots[key].start()
def convert_to_path(msg): path = [] for state in msg: x = state.x y = state.y z = state.z p = Point(x, y, z) path.append(p) return path
def get_model_position(model_name): object_state = get_model_state(model_name) if object_state: x = object_state.pose.position.x y = object_state.pose.position.y z = object_state.pose.position.z pose = Point(x, y, z) return pose else: return None
def get_link_position(link_name): rospy.wait_for_service('/gazebo/get_link_state') try: get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) link_state = get_link_state(link_name, 'world') pos = link_state.link_state.pose.position x = pos.x y = pos.y z = pos.z state = Point(x, y, z) return state except rospy.ServiceException, e: print "Service call failed: %s" % e return None
def find_z(self, x, y): p = Point(x, y, 0) j = (x + (const.MAP_HEIGHT / 2)) // self.x_step i = ((const.MAP_WIDTH / 2) - y) // self.y_step cell_id = (str(int(i)), str(int(j))) if cell_id in self.cells.keys(): n_cell = self.cells[cell_id] edge = self.find_nearest_edge(p, n_cell) p3 = self.heightmap[edge[0]] p4 = self.heightmap[edge[1]] new_p = n_cell.pos.find_intersection_of_lines(p, p3, p4) z = n_cell.pos.find_z_coord(new_p, p.x, p.y) return z else: print(p, cell_id) print('Cell id doesnt exist!') return None
def find_nearest_edge(self, p3, cell): current_edge = None min_dist = float('inf') for edge in cell.edges: p1 = self.heightmap[edge[0]] p2 = self.heightmap[edge[1]] k = ((p2.y - p1.y) * (p3.x - p1.x) - (p2.x - p1.x) * (p3.y - p1.y)) / ((p2.y - p1.y)**2 + (p2.x - p1.x)**2) x4 = p3.x - k * (p2.y - p1.y) y4 = p3.y + k * (p2.x - p1.x) z4 = p1.find_z_coord(p2, x4, y4) p4 = Point(x4, y4, z4) dist = p4.get_2d_distance(p3) if dist < min_dist: min_dist = dist current_edge = edge return current_edge
def convert_to_point(msg): x = msg.x y = msg.y z = msg.z point = Point(x, y, z) return point
def convert_to_point(msg_data): x = msg_data.x y = msg_data.y z = msg_data.z point = Point(x, y, z) return point