def aruco_target_assignment(self):
"""
Target assignment and path planning function for group of robots
"""
paths = {}
for robot_id in self.robots.keys():
if not self.robots[robot_id].path_created:
target_id = const.platform_target[robot_id]
tmp_targets = self.targets.copy()
tmp_robots = self.robots.copy()
if target_id in list(tmp_targets.keys()):
img_target_position = tmp_targets[target_id].center
target_position = Point(
img_target_position.x, img_target_position.y
).remap_to_ompl_coord_system().get_xy()
img_robot_position = tmp_robots[robot_id].center
robot_position = Point(
img_robot_position.x, img_robot_position.y
).remap_to_ompl_coord_system().get_xy()
full_obstacles = self.get_full_obstacles(
robot_id, target_id)
paths[robot_id] = self.plan(robot_position,
target_position,
const.PLANNER_RANGE,
full_obstacles)
return paths
def get_obstacles_from_any_objects(self, objects_dict):
full_obstacles = []
for obstacle in objects_dict.values():
if self.source == "markers_analizer":
corners_list = list(
Point(xy.x, xy.y).remap_to_ompl_coord_system().get_xy()
for xy in obstacle.corners)
else:
corners_list = list(
Point(xy.x, xy.y).get_xy() for xy in obstacle.corners)
path = Path(np.array(corners_list))
full_obstacles.append(path)
return full_obstacles
def path_to_point_list(self, path):
states = path.getStates()
path_lst = []
for state in states:
x = state.getX()
y = state.getY()
cv_point = Point(x, y).remap_to_img_coord_system()
path_lst.append(cv_point)
return path_lst
def draw_map(self):
columns = self.map.column.values()
rows = self.map.row.values()
merge_list = lambda ll: [el for lst in ll for el in lst]
columns = merge_list(columns)
rows = merge_list(rows)
for c in columns:
for r in rows:
self.draw_point(Point(c, r), color=(45, 205, 255), size=2)
def cv_callback(event,x,y,flags,param):
global X, Y
global sector_x, sector_y
if event == cv2.EVENT_LBUTTONUP:
X, Y = x, y
elif event == cv2.EVENT_MOUSEMOVE:
r, c = map.get_point_position_on_map(Point(x, y))
point = map.get_sector_center(r, c)
sector_x, sector_y = point.x, point.y
def get_robot_direction_vector(self, robot_handle):
direction_point = self.get_object_child(robot_handle, 15)
robot_position = self.get_object_position(robot_handle)
dir_point_position = self.get_object_position(direction_point)
direction_vector = (dir_point_position.x - robot_position.x, \
dir_point_position.y - robot_position.y)
direction_vector_mod = sqrt(direction_vector[0] ** 2 \
+ direction_vector[1] ** 2)
norm_direction_vector = (direction_vector[0] / direction_vector_mod, \
direction_vector[1] / direction_vector_mod)
return Point(*norm_direction_vector)
def get_object_position(self, object_handle):
"""
Function that returns position of object on the scene in V-REP
"""
res, object_position = vrep.simxGetObjectPosition(self.client_id, object_handle, -1, \
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
return Point(object_position[0], object_position[1])
else:
print('Remote function call failed with result {0}.'.format(res))
return ()
def get_boundary_points(self, object_handle):
"""
Function that returns boundary points of object's (obstacle) boundary box
"""
points = []
obstacle_position = self.get_object_position(object_handle)
ret, orient = vrep.simxGetObjectOrientation(self.client_id, object_handle, -1, \
vrep.simx_opmode_blocking)
ret, x_1 = vrep.simxGetObjectFloatParameter(self.client_id, object_handle, 15, \
vrep.simx_opmode_blocking)
ret, y_1 = vrep.simxGetObjectFloatParameter(self.client_id, object_handle, 16, \
vrep.simx_opmode_blocking)
ret, x_2 = vrep.simxGetObjectFloatParameter(self.client_id, object_handle, 18, \
vrep.simx_opmode_blocking)
ret, y_2 = vrep.simxGetObjectFloatParameter(self.client_id, object_handle, 19, \
vrep.simx_opmode_blocking)
angle = orient[2]
# Extension of boundaries, so that the robots moves without collisions
x_1 = x_1 - 0.3
x_2 = x_2 + 0.3
y_1 = y_1 - 0.3
y_2 = y_2 + 0.3
p_1 = (x_1 * cos(angle) - y_1 * sin(angle) + obstacle_position.x, y_1 * \
cos(angle) + x_1 * sin(angle) + obstacle_position.y)
points.append(Point(*p_1))
p_2 = (x_1 * cos(angle) - y_2 * sin(angle) + obstacle_position.x, y_2 * \
cos(angle) + x_1 * sin(angle) + obstacle_position.y)
points.append(Point(*p_2))
p_3 = (x_2 * cos(angle) - y_2 * sin(angle) + obstacle_position.x, y_2 * \
cos(angle) + x_2 * sin(angle) + obstacle_position.y)
points.append(Point(*p_3))
p_4 = (x_2 * cos(angle) - y_1 * sin(angle) + obstacle_position.x, y_1 * \
cos(angle) + x_2 * sin(angle) + obstacle_position.y)
points.append(Point(*p_4))
return points
def obj_callback(msg_data):
global was_robot_data
if not was_robot_data:
final_msg = AllPathes()
global X, Y, OK
for robot in msg_data.robots:
if not robot.path_created:
OK = False
path = []
while not OK:
cv2.putText(image, "Select sector. (You need {} points)".format(path_points_num), \
(20, 20), cv2.FONT_HERSHEY_PLAIN, 2, (255, 50, 40), 2)
cv2.circle(image, (int(robot.center.x), int(robot.center.y)), 5, (255, 255, 10), 4)
# if sector_x and sector_y: # !!! uncomment for use sectors
# draw_rectangle((sector_x, sector_y), map.sector_w) # !!! uncomment for use sectors
if len(path):
for pt in path:
cv2.circle(image, (int(pt.x), int(pt.y)), 5, (255, 100, 50), 4)
cv2.imshow("image_to_set_path", image)
cv2.waitKey(100)
if X and Y:
path_point = map.get_point_position_on_map(Point(X, Y))
cv_path_point = map.get_sector_center(*path_point)
if len(path) < path_points_num:
path.append(Point(X, Y)) # !!! comment for use sectors
# path.append(cv_path_point) # !!! uncomment for use sectors
X, Y = None, None
else:
OK = not OK
X, Y = None, None
msg = create_msg(robot.id, path)
final_msg.paths_list.append(msg)
else:
OK = True
paths_data_publisher.publish(final_msg)
cv2.destroyAllWindows()
was_robot_data = True
def create_mesh(self, row_num, col_num):
x_min = -5
x_max = 5
y_min = -5
y_max = 5
x_range = x_max - x_min
y_range = y_max - y_min
cell_x_size = float(x_range) / float(col_num)
cell_y_size = float(y_range) / float(row_num)
cells_list = []
for row in range(row_num):
cells_list.append([])
for col in range(col_num):
x = x_min + cell_x_size * (0.5 + col)
y = y_min + cell_y_size * (0.5 + row)
cell_pos = Point(x, y)
cells_list[row].append(cell_pos)
return cells_list