target_pose = get_2d_pose(goal) current_pose = robot.get_position(vision_sensor)[:2] + robot.get_orientation()[-1:] current_pose = (-current_pose[0], current_pose[1], current_pose[2]) print(current_pose) print(get_2d_pose(robot)) pr.step() origin_x, _, origin_y, _, _, _ = vision_sensor.get_bounding_box() # Setup occ_grid occ_grid = OccupancyGrid(origin_x, origin_y) depth = vision_sensor.capture_depth() occ_grid.fromDepth(depth) ''' dij_solver = Dijkstra(occ_grid, current_pose, target_pose) moves = dij_solver.solve() for i in moves: pr.step() set_2d_pose(robot, (i[0], i[1], current_pose[2])) time.sleep(0.05) ''' ''' # RRT Solver rrt_solver = RRT(pr, robot, current_pose, target_pose, [-0.5,0.5], [-0.5,0.5]) moves = rrt_solver.solve()
