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 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_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 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