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