def __init__(self,
map_frame_id,
map_resolution,
map_width,
map_height,
map_origin_x,
map_origin_y,
map_origin_yaw,
inflate_radius,
unknown_space,
free_space,
c_space,
occupied_space,
optional=None):
self.__pose = None
self.__map = GridMap(map_frame_id, map_resolution, map_width,
map_height, map_origin_x, map_origin_y,
map_origin_yaw, unknown_space)
self.__inflated_map = self.__map
self.__mapping = Mapping(unknown_space, free_space, c_space,
occupied_space, inflate_radius, optional)
self.__update = None
self.__correct_inflated_map = True
def main():
print(__file__ + " start!!")
# RFID positions [x, y]
RFID = np.array([[10.0, 0.0], [10.0, 10.0], [0.0, 15.0], [-5.0, 20.0]])
time = 0.0
Filter = HistogramFilter(DT)
xTrue = np.zeros((4, 1))
grid_map = GridMap()
while SIM_TIME >= time:
time += DT
print("Time:", time)
u = simulator.calc_input()
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = Filter.observation(xTrue, u, RFID)
grid_map = Filter.localization(grid_map, u, z, yaw)
if show_animation:
grid_map.plot(xTrue, RFID, z, time)
print("Done")
def main(args):
map_name = args["map"]
map_size = args["map_size"]
map_offset = args["map_offset"]
map_resolution = args["map_resolution"]
position_list_path = args["position_list"]
number_sample = args["number_sample"]
point_cloud_size = args["point_cloud_size"]
rangefinder_noise = args["rangefinder_noise"]
prefix = args["file_prefix"]
dir_name = args["directory_name"]
points = load_position_list(position_list_path,
map_size=map_size,
map_offset=map_offset)
environment = load_svg_environment("./geometries/" + map_name + ".svg",
map_size, map_offset)
grid_map = GridMap(map_size, map_resolution)
grid_map.load_environment(environment)
scan_match_filter_evaluation.generate_map_data(
grid_map=grid_map,
environment=environment,
map_name=map_name,
sample_count=number_sample,
cloud_size=point_cloud_size,
points=points,
prefix=prefix,
dir_name=dir_name,
rangefinder_noise=rangefinder_noise)
store_param_list(args)
def evaluate():
# init params
sensor_origin = (2, 2) #(5, 5)#(4, 4)#(4, 8)
map_resolution = 0.1
cloud_size = 80 #25 #80 # 80 # 5 - 80 const
map_size = 10
sample_count = 200
# end init
environment = load_svg_environment("./geometries/" + DEFAULT_MAP + ".svg",
map_size, 0.5)
grid_map = GridMap(map_size, map_resolution)
grid_map.load_environment(environment)
def __init__(self, map_frame_id, map_resolution, map_width, map_height,
map_origin_x, map_origin_y, map_origin_yaw, inflate_radius,
unknown_space, free_space, c_space, occupied_space, optional=None):
rospy.init_node('occupancy_grid_handler')
self.__pose = None
self.__map = GridMap(map_frame_id, map_resolution, map_width, map_height,
map_origin_x, map_origin_y, map_origin_yaw)
self.__inflated_map = self.__map
self.__mapping = Mapping(unknown_space, free_space, c_space,
occupied_space, inflate_radius, optional)
self.__odom_sub = message_filters.Subscriber('SVEA5/odom', OdometryROS)
self.__scan_sub = message_filters.Subscriber('scan', LaserScanROS)
self.__ts = message_filters.ApproximateTimeSynchronizer([self.__odom_sub,
self.__scan_sub], 10, 0.01)
self.__ts.registerCallback(self.callback)
self.__map_pub = rospy.Publisher('map', OccupancyGridROS, queue_size=1,
latch=True)
self.__map_updates_pub = rospy.Publisher("map_updates",
OccupancyGridUpdateROS,
queue_size=10)
self.__map_inflated_pub = rospy.Publisher('inflated_map', OccupancyGridROS, queue_size=1, latch=True)
self.publish_map()
rospy.spin()
def main():
obstacles = [
np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) +
np.array([-1.0, 0.5]),
np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) +
np.array([-1.0, 1.0]),
np.array([[0.7, -0.9], [0.8, -0.9], [0.8, -0.3], [0.7, -0.3]]) +
np.array([-1.0, 0.5]),
]
params = Params()
flight_area_vertices = 2 * np.array([[-0.6, 0.8], [-0.9, -0.9],
[0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices)
gridmap.add_obstacles_to_grid_map(obstacles)
# x, y, yaw
pose = [
np.mean(flight_area_vertices[:, 0]),
np.mean(flight_area_vertices[:, 1]), -np.pi / 3
]
traj = pose[:2]
plt.figure(figsize=(10, 10))
gridmap.draw_map(obstacles)
plt.plot(pose[0], pose[1], 'ro', markersize=20, label='Initial position')
# while True:
for _ in range(params.numiters):
dv = 0.1 * params.vel
pose[0] += dv * np.cos(pose[2])
pose[1] += dv * np.sin(pose[2])
pose_grid = gridmap.meters2grid(pose[:2])
boundary = obstacle_check([pose_grid[0], pose_grid[1], pose[2]],
gridmap)
# print(boundary)
if boundary['right'] or boundary['front']:
pose = back_shift(pose, 0.03)
pose[2] -= np.pi / 2 * np.random.uniform(0.2, 0.6)
elif boundary['left']:
pose = back_shift(pose, 0.03)
pose[2] += np.pi / 2 * np.random.uniform(0.2, 0.6)
traj = np.vstack([traj, pose[:2]])
if params.animate:
plt.cla()
gridmap.draw_map(obstacles)
visualize(traj, pose, gridmap)
plt.pause(0.1)
visualize(traj, pose, gridmap)
plt.show()
class Engine():
def __init__(self,
map_frame_id,
map_resolution,
map_width,
map_height,
map_origin_x,
map_origin_y,
map_origin_yaw,
inflate_radius,
unknown_space,
free_space,
c_space,
occupied_space,
optional=None):
self.__pose = None
self.__map = GridMap(map_frame_id, map_resolution, map_width,
map_height, map_origin_x, map_origin_y,
map_origin_yaw, unknown_space)
self.__inflated_map = self.__map
self.__mapping = Mapping(unknown_space, free_space, c_space,
occupied_space, inflate_radius, optional)
self.__update = None
self.__correct_inflated_map = True
def callback(self, pose, scan):
self.__correct_inflated_map = False
self.__map, self.__update = self.__mapping.update_map(
self.__map, pose, scan)
def get_map_data(self):
return self.__map.to_message().data.tolist()
def get_inflated_map_data(self):
if not self.__correct_inflated_map:
# Lazy-evaluation of the inflated map
grid_map = copy.deepcopy(self.__map)
self.__inflated_map = self.__mapping.inflate_map(grid_map)
return self.__inflated_map.to_message().data.tolist()
def get_update(self):
return self.__update
def get_map(self):
return self.__map
def get_inflated_map(self):
if not self.__correct_inflated_map:
# Lazy-evaluation of the inflated map
grid_map = copy.deepcopy(self.__map)
self.__inflated_map = self.__mapping.inflate_map(grid_map)
return self.__inflated_map
def __init__(self,
config_file,
map_config_file,
map_file,
is_clear=False,
is_pub_id=True,
resolution=0.1):
# Member
f = open(config_file)
self.config = yaml.load(f)
f = open(map_config_file)
self.map_config = yaml.load(f)
map_array = mpimg.imread(map_file)
self.grid_map = GridMap(map_array, self.map_config['resolution'])
self.grid_map.print()
self.resolution = resolution # m
self.trajs = []
self.last_id = None
self.cur_id = 0
self.is_pub_id = is_pub_id
# Refine
self.refine()
# Clear grid map.
if is_clear:
self.clear_map()
# Spawn obstacles into the map.
for traj in self.trajs:
x = int(traj[self.cur_id][0] / self.grid_map.resolution + 0.5)
y = int(traj[self.cur_id][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 1.0)
self.last_id = self.cur_id
self.cur_id += 1
def test_get_nodes(self):
gm = GridMap(threshold=3, bit_depth=8)
for x in range(256):
for y in range(256):
gm.add(x, y, str((x, y)))
nodes = gm.get_nodes(0, 0, 255, 255)
self.assertEqual(len(nodes), 256*256)
nodes = gm.get_nodes(16, 32, 31, 47)
self.assertEqual(len(nodes), (16)*(16))
for x in range(16, 32, 1):
for y in range(32, 48, 1):
match = False
for i in range(len(nodes)):
n = nodes[i]
if n.x == x and n.y == y:
match = True
nodes.pop(i)
break
self.assertTrue(match)
import matplotlib.image as mpimg
from grid_map import GridMap
from robot_model import RobotModel
dr = mpimg.imread('/home/kai/Pictures/dr.png')
global_grid_map = GridMap(dr, 1.0)
global_grid_map.print()
robot_model = RobotModel(2.0)
controls = robot_model.get_controls((639, 200), global_grid_map)
print(controls)
# Checking initial position and first goal
print("robot initial state: ", state)
print("robot initial goal : ", goal)
# Checking the target intial position and first goal
print("target initial state: ", target_state)
print("target initial goal : ", target_goal)
# Checking the target intial position and first goal
print("pedestrian initial state: ", pedestrian_state)
print("pedestrian initial goal : ", pedestrian_goal)
flight_area_vertices = [[-12.5, 12.5], [12.5, 12.5], [12.5, -12.5],
[-12.5, -12.5]]
gridmap = GridMap(flight_area_vertices, state[:2])
# fill the obstacles_array
for i in range(len(obstacles)):
tmp = np.array([
obstacles[i].vertices[0], obstacles[i].vertices[1],
obstacles[i].vertices[2], obstacles[i].vertices[3]
])
obstacles_array.append(tmp)
gridmap.add_obstacles_to_grid_map(obstacles_array)
for _ in range(params.numiters):
observation.get_obs(state, target_state, pedestrian_state, params,
target_goal, pedestrian_goal, params_localmap,
human_params_localmap, gridmap)
state, inputs = motion_dwa(state, inputs, goal, obpoints, walls,
img[img > 128] = 255
img[img <= 128] = 0
m = np.asarray(img)
m = cv2.cvtColor(m, cv2.COLOR_RGB2GRAY)
m = m.astype(float) / 255.
img = img.astype(float) / 255.
# Lidar
lmodel = LidarModel(m)
car = KinematicModel()
pos = (100, 200, 0)
car.x = 100
car.y = 200
car.yaw = 0
gm = GridMap([0.5, -0.5, 5.0, -5.0], gsize=3)
while (True):
print("\rState: " + car.state_str(), end="\t")
car.update()
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [61, -120, 120], sdata)
# Map
gm.update_map(pos, [61, -120, 120, 250], sdata)
mimg = gm.adaptive_get_map_prob()
mimg = (255 * mimg).astype(np.uint8)
mimg = cv2.cvtColor(mimg, cv2.COLOR_GRAY2RGB)
mimg_ = cv2.flip(mimg, 0)
cv2.imshow("map", mimg_)
def main():
params = Params()
# initial state = [x(m), y(m), z(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([0, 0.2, params.max_height, np.pi / 2, 0.0, 0.0])
traj = state[:3]
plt.figure(figsize=(10, 10))
flight_area_vertices = define_flight_area(state[:2])
# flight_area_vertices = np.array([[-1, -1], [-0.3, -1], [-0.3, -0.4], [0.3, -0.4], [0.3, -1], [1,-1], [1,1], [-1,1]])
gridmap = GridMap(flight_area_vertices, state[:2])
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = params.sweep_resolution
goal_x2D, goal_y2D = planning(ox, oy, reso)
waypoints = get_3D_waypoints(goal_x2D, goal_y2D, params.min_height,
params.max_height,
params.sweep_resolution / 2.)
goal_x = waypoints[:, 0]
goal_y = waypoints[:, 1]
goal_z = waypoints[:, 2]
# goal = [x, y], m
goali = 0
goal = [goal_x[goali], goal_y[goali], goal_z[goali]]
t_prev_goal = time.time()
fig = plt.figure(figsize=(10, 10))
ax = plt.axes(projection='3d')
ax.set_xlabel('X, [m]')
ax.set_ylabel('Y, [m]')
ax.set_zlabel('Z, [m]')
ax.set_xlim([-2.5, 2.5])
ax.set_ylim([-2.5, 2.5])
ax.set_zlim([0.0, 3.0])
# while True:
for _ in tqdm(range(params.numiters)):
state = motion(state, goal, params)
state = collision_avoidance(state, gridmap, params)
goal_dist = np.linalg.norm(goal - state[:3])
# print('Distance to goal %.2f [m]:' %goal_dist)
t_current = time.time()
if goal_dist < params.goal_tol or (
t_current -
t_prev_goal) > params.time_to_switch_goal: # goal is reached
# print('Switching to the next goal.')
# print('Time from the previous reached goal:', t_current - t_prev_goal)
if goali < len(goal_x) - 1:
goali += 1
else:
break
t_prev_goal = time.time()
goal = [goal_x[goali], goal_y[goali], goal_z[goali]]
traj = np.vstack([traj, state[:3]])
if params.animate:
plt.cla()
ax.plot(goal_x, goal_y, goal_z, ':')
ax.scatter(goal[0],
goal[1],
goal[2],
label='Goal position',
zorder=20)
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2], linewidth=3, color='g')
plot_robot(ax, state, params)
plt.legend()
plt.pause(0.1)
print('Mission is complete!')
plt.cla()
ax.plot(goal_x, goal_y, goal_z, ':')
ax.scatter(goal[0], goal[1], goal[2], label='Goal position', zorder=20)
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2], linewidth=3, color='g')
plot_robot(ax, state, params)
plt.legend()
plt.pause(0.1)
plt.draw()
input('Hit Enter to close all figures')
plt.close('all')
def main():
rospy.init_node('swarm_random_walk')
params = Params()
# Connect to drones and start multirangers:
drones = []
for URI in params.uris:
drone = DroneMultiranger(URI)
drones.append(drone)
time.sleep(3)
for drone in drones:
drone.last_sp = drone.position
drone.traj = drone.last_sp[:2]
# Define flight zones for each UAV:
# flight_area1 = np.array([[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]])/1.5 + np.array([ 0.6, 0.0])
# flight_area2 = np.array([[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]])/1.5 + np.array([-0.6, 0.0])
flight_areas = [
np.array([[-1.0, 1.0], [-1.0, -1.0], [-0.5, -1.0], [-0.5, 1.0]]),
np.array([[-0.5, 1.0], [-0.5, 0.0], [1.0, 0.0], [1.0, 1.0]]),
np.array([[-0.5, 0.0], [-0.5, -1.0], [1.0, -1.0], [1.0, 0.0]]),
]
flight_heights = [0.3, 0.6, 0.9]
for i in range(len(drones)):
drones[i].gridmap_params = GridMap(flight_areas[i])
drones[i].flight_height = flight_heights[i]
if params.toFly:
threads = []
for drone in drones:
th = Thread(target=prepare, args=(drone, ))
threads.append(th)
for th in threads:
th.start()
for th in threads:
th.join()
raw_input('Press Enter to fly...')
threads = []
for drone in drones:
th = Thread(target=exploration_conveyer, args=(
drone,
params,
))
threads.append(th)
for th in threads:
th.start()
for th in threads:
th.join()
plt.figure(figsize=(10, 10))
for drone in drones:
drone.gridmap_params.draw_map()
visualize(drone)
plt.legend()
plt.draw()
plt.pause(0.1)
raw_input('Hit Enter to close all windows')
plt.close('all')
for drone in drones:
drone.cf.commander.send_stop_setpoint()
time.sleep(0.1)
drone.cf.close_link()
def main():
obstacles = [
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 0.5]),
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 1.0]),
# np.array([[0.7, -0.9], [0.8, -0.9], [0.8, -0.3], [0.7, -0.3]]) + np.array([-1.5, 1.0]),
np.array([[-0.3, -0.4], [0.3, -0.4], [0.3, 0.1], [-0.3, 0.1]]) * 0.5
]
# initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([0, 0.2, np.pi / 2, 0.0, 0.0])
traj = state[:2]
params = Params()
# plt.figure(figsize=(10,10))
flight_area_vertices = define_flight_area(state[:2])
posset = []
# flight_area_vertices = np.array([[-1, -1], [-0.3, -1], [-0.3, -0.4], [0.3, -0.4], [0.3, -1], [1,-1], [1,1], [-1,1]])
gridmap = GridMap(flight_area_vertices, state[:2])
gridmap.add_obstacles_to_grid_map(obstacles)
#obstacle x, y coordinates
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = params.sweep_resolution
goal_x, goal_y, gmap, covmap = planning(ox, oy, reso)
# covmap.plot_grid_map(axes[0])
# goal = [x, y], m
goali = 0
goal = [goal_x[goali], goal_y[goali]]
t_prev_goal = time.time()
gridmap.draw_map(obstacles)
iter = 0
# while True:
for _ in range(params.numiters):
state = motion(state, goal, params)
state = collision_avoidance(state, gridmap, params)
posset.append([state[0], state[1]])
update_coveragemap(state, covmap)
goal_dist = np.linalg.norm(goal - state[:2])
# print('Distance to goal %.2f [m]:' %goal_dist)
t_current = time.time()
# if goal_dist < params.goal_tol or (t_current - t_prev_goal) > params.time_to_switch_goal: # goal is reached
if goal_dist < params.goal_tol: # goal is reached
print('Switching to the next goal.')
print('Time from the previous reached goal:',
t_current - t_prev_goal)
if goali < len(goal_x) - 1:
goali += 1
else:
break
t_prev_goal = time.time()
goal = [goal_x[goali], goal_y[goali]]
traj = np.vstack([traj, state[:2]])
if params.animate:
axes[1].cla()
# plt.cla()
gridmap.draw_map(obstacles, axes[1]) #mk
axes[1].plot(goal_x, goal_y)
axes[1].plot(goal[0],
goal[1],
'ro',
markersize=12,
label='Goal position',
zorder=20)
visualize(traj, state, params)
visualize_coverage(posset)
# plt.plot(goal_x, goal_y)
# plt.plot(goal[0], goal[1], 'ro', markersize=12, label='Goal position', zorder=20)
# visualize(traj, state, params)
# visualize_coverage(posset)
covmap.plot_grid_map(axes[0])
plt.pause(0.01)
iter = iter + 1
if iter == 1:
plt.savefig('planned_coverage_path.png', dpi=300)
# covmap2.plot_grid_map(axes[0])
print('Mission is complete!')
plt.plot(goal_x, goal_y)
visualize(traj, state, params)
plt.show()
import rospy
import matplotlib.image as mpimg
from grid_map import GridMap
rospy.init_node('test_grid_map', anonymous=True)
dr = mpimg.imread('/home/kai/catkin_ws/src/drproj/empty.png')
global_grid_map = GridMap(dr, 1.0)
global_grid_map.show('rviz_global_grid_map')
print(global_grid_map.occupancy(1, 1))
global_grid_map.print()
def main():
obstacles = [
np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) +
np.array([-1.0, 0.5]),
np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) +
np.array([-1.0, 1.0]),
np.array([[0.7, -0.9], [0.8, -0.9], [0.8, -0.3], [0.7, -0.3]]) +
np.array([-1.0, 0.5]),
]
params = Params()
flight_area_vertices = 2 * np.array([[-0.6, 0.8], [-0.9, -0.9],
[0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices)
gridmap.add_obstacles_to_grid_map(obstacles)
# goal = [x, y], m
goal = np.array([0.5, 0.9])
# initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([-1.0, -1.0, np.pi / 2, 0.0, 0.0])
traj = state[:2]
plt.figure(figsize=(10, 10))
gridmap.draw_map(obstacles)
# while True:
for _ in range(params.numiters):
state = motion(state, goal, params)
pose_grid = gridmap.meters2grid(state[:2])
boundary = obstacle_check([pose_grid[0], pose_grid[1], state[2]],
gridmap.gmap, params)
# print(boundary)
if boundary['right'] or boundary['front']:
# state = back_shift(state, 0.03)
state = slow_down(state, params)
state = turn_left(state, np.radians(20))
elif boundary['left']:
# state = back_shift(state, 0.03)
state = slow_down(state, params)
state = turn_right(state, np.radians(20))
goal_dist = np.linalg.norm(goal - state[:2])
# print('Distance to goal %.2f [m]:' %goal_dist)
if goal_dist < params.goal_tol: # goal is reached
print('Goal is reached.')
goal = np.array(
[np.random.uniform(-1, 1),
np.random.uniform(-1, 1)])
traj = np.vstack([traj, state[:2]])
if params.animate:
plt.cla()
gridmap.draw_map(obstacles)
plt.plot(goal[0],
goal[1],
'ro',
markersize=20,
label='Goal position')
visualize(traj, state, params)
plt.pause(0.1)
plt.plot(goal[0], goal[1], 'ro', markersize=20, label='Goal position')
visualize(traj, state, params)
plt.show()
def main():
rospy.init_node('random_walk')
params = Params()
flight_area_vertices = [[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]]
# flight_area_vertices=[[-0.5, 0.5], [-0.5, -0.5], [0.5, -0.5], [0.5, 0.5]]
gridmap = GridMap(flight_area_vertices)
drone = DroneMultiranger(params.uri)
time.sleep(3)
drone.pose_home = drone.position
print('Home positions:', drone.pose_home)
if params.toFly:
prepare(drone)
raw_input('Press Enter to fly...')
takeoff(drone, params.flight_height)
# x, y, yaw
pose = [drone.pose_home[0], drone.pose_home[1], 0.0]
# pose = [grdimap.map_center[0], grdimap.map_center[1], 0.0]
traj = pose[:2]
plt.figure(figsize=(8, 8))
# while True:
for _ in range(params.numiters):
dv = 0.1 * params.vel
pose[0] += dv * np.cos(pose[2])
pose[1] += dv * np.sin(pose[2])
pose_grid = gridmap.meters2grid(pose[:2])
border = borders_check([pose_grid[0], pose_grid[1], pose[2]], gridmap)
# print(border)
if border['right'] or border['front']:
print('Border in FRONT or RIGHT')
pose = back_shift(pose, 0.03)
pose[2] -= np.pi / 2 * np.random.uniform(0.2, 0.6)
elif border['left']:
print('Border on the LEFT')
pose = back_shift(pose, 0.03)
pose[2] += np.pi / 2 * np.random.uniform(0.2, 0.6)
if params.toFly:
if is_close(
drone.measurement['front']
) and drone.measurement['left'] > drone.measurement['right']:
print('FRONT RIGHT')
pose = back_shift(pose, 0.05)
pose[2] += np.pi / 2 * np.random.uniform(0.1, 0.8)
if is_close(
drone.measurement['front']
) and drone.measurement['left'] < drone.measurement['right']:
print('FRONT LEFT')
pose = back_shift(pose, 0.05)
pose[2] += np.pi / 2 * np.random.uniform(0.1, 0.8)
if is_close(drone.measurement['left']):
print('LEFT')
pose = right_shift(pose, 0.05)
if is_close(drone.measurement['right']):
print('RIGHT')
pose = left_shift(pose, 0.05)
traj = np.vstack([traj, pose[:2]])
drone.sp = [
pose[0], pose[1], params.flight_height,
np.degrees(pose[2]) % 360
]
if params.check_battery:
try:
if drone.battery_state == 'needs_charging':
print('Going home to CHARGE the battery')
print('Battery status: %.2f [V]' % drone.V_bat)
break
except:
pass
if params.toFly:
fly(drone)
time.sleep(0.1)
else:
plt.cla()
gridmap.draw_map()
visualize(traj, pose, gridmap)
plt.pause(0.1)
gridmap.draw_map()
visualize(traj, pose, gridmap)
if params.toFly:
goTo(drone,
[drone.pose_home[0], drone.pose_home[1], params.flight_height, 0])
hover(drone, 1.0)
land(drone)
plt.show()
def test_add(self):
"""
Cannot assert check add, but if there is an error, it will be thrown.
"""
gm = GridMap(threshold=2, bit_depth=8)
gm.add(14, 7, "hello world")
gm.add(0, 0, "DEADBEEF")
gm.add(255, 255, "meh")
for x in range(256):
for y in range(256):
gm.add(x, y, "")
gm.add(0, 255, None)
gm.add(15, 31, 3.14598)
class MovingObs(object):
def __init__(self,
config_file,
map_config_file,
map_file,
is_clear=False,
is_pub_id=True,
resolution=0.1):
# Member
f = open(config_file)
self.config = yaml.load(f)
f = open(map_config_file)
self.map_config = yaml.load(f)
map_array = mpimg.imread(map_file)
self.grid_map = GridMap(map_array, self.map_config['resolution'])
self.grid_map.print()
self.resolution = resolution # m
self.trajs = []
self.last_id = None
self.cur_id = 0
self.is_pub_id = is_pub_id
# Refine
self.refine()
# Clear grid map.
if is_clear:
self.clear_map()
# Spawn obstacles into the map.
for traj in self.trajs:
x = int(traj[self.cur_id][0] / self.grid_map.resolution + 0.5)
y = int(traj[self.cur_id][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 1.0)
self.last_id = self.cur_id
self.cur_id += 1
def interpolate_two_pts(self, start, end):
'''
Arguments
---------
start (A 'point' represented by list, meter)
end (A 'point' represented by list, meter)
Returns
-------
_ (A list of 'point', including 'start' but excluding 'end')
'''
dx = end[0] - start[0]
dy = end[1] - start[1]
if abs(dx) + abs(dy) == 0:
return []
x_resolution = self.resolution * dx / sqrt(dx**2 + dy**2)
y_resolution = self.resolution * dy / sqrt(dx**2 + dy**2)
max_dis = np.linalg.norm(np.subtract(end, start))
pts = []
pt = start.copy()
while np.linalg.norm(np.subtract(pt, start)) < max_dis:
pts.append(pt.copy())
pt[0] += x_resolution
pt[1] += y_resolution
return pts
def interpolate_multi_pts(self, multi_pts):
pts = []
for i in range(len(multi_pts) - 1):
start = multi_pts[i]
end = multi_pts[i + 1]
pts += self.interpolate_two_pts(start, end)
return pts
def refine(self):
self.trajs = []
if self.config is None:
return
for key in self.config:
self.trajs.append(self.interpolate_multi_pts(self.config[key]))
def run_once(self):
if self.is_pub_id:
pub = rospy.Publisher('cur_moving_obs_id',
Int64,
queue_size=10,
latch=True)
pub.publish(Int64(data=self.cur_id))
for traj in self.trajs:
i = min(self.last_id, len(traj) - 1)
x = int(traj[i][0] / self.grid_map.resolution + 0.5)
y = int(traj[i][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 0.0)
i = min(self.cur_id, len(traj) - 1)
x = int(traj[i][0] / self.grid_map.resolution + 0.5)
y = int(traj[i][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 1.0)
self.last_id = self.cur_id
self.cur_id += 1
def clear_map(self):
for x in range(self.grid_map.max_x):
for y in range(self.grid_map.max_y):
self.grid_map.set_grid(x, y, 0.0)
import matplotlib.image as mpimg
from grid_map import GridMap
rospy.init_node('test_a_star_planner', anonymous=True)
vehicle_config = yaml.load(
open('/home/kai/catkin_ws/src/drproj/vehicle_config.yaml'))
half_length = 0.5 * vehicle_config['length']
half_width = 0.5 * vehicle_config['width']
radius = sqrt(half_length**2 + half_width**2)
robot_model = RobotModel(radius)
global_planner = AStarPlanner(robot_model)
free = mpimg.imread('/home/kai/catkin_ws/src/drproj/free.png')
global_map_config = yaml.load(open('/home/kai/catkin_ws/src/drproj/free.yaml'))
global_map = GridMap(free, global_map_config['resolution'])
global_map.show('rviz_global_grid_map')
global_map.print()
global_mission = yaml.load(
open('/home/kai/catkin_ws/src/drproj/global_mission.yaml'))
start_grid_x = int(global_mission['start'][0] / global_map.resolution + 0.5)
start_grid_y = int(global_mission['start'][1] / global_map.resolution + 0.5)
goal_grid_x = int(global_mission['goal'][0] / global_map.resolution + 0.5)
goal_grid_y = int(global_mission['goal'][1] / global_map.resolution + 0.5)
start = (start_grid_x, start_grid_y)
goal = (goal_grid_x, goal_grid_y)
path = global_planner.get_path(start, goal, global_map)
global_planner.show_path('rviz_global_path', global_map)
print(path)
open_list.append([i, j])
parents[i][j] = x
g_map[i][j] = cur_g
f_map[i][j] = g_map[i][j] + abs(N - i - 1) * 10 + abs(
N - j - 1) * 10
else:
if cur_g < g_map[i][j]:
parents[i][j] = x
g_map[i][j] = cur_g
f_map[i][j] = g_map[i][j] + abs(N - i - 1) * 10 + abs(
N - j - 1) * 10
if [N - 1, N - 1] in open_list:
isSuccess = True
break
#add path
path = []
cur = [N - 1, N - 1]
while True:
path.append(cur)
cur = parents[cur[0]][cur[1]]
if cur == [0, 0]:
break
path.append([0, 0])
print(path)
grid_map = GridMap(map_matrix)
grid_map.add_path(path)
grid_map.show()
self.num_missions = 1
self.time_between_missions = 5
if __name__ == '__main__':
rospy.init_node('random_walk')
params = Params()
drone = DroneMultiranger(params.uri)
time.sleep(3)
drone.goali = 0
drone.pose_home = drone.position
print('Home positions:', drone.pose_home)
flight_area_vertices = define_flight_area(drone.pose_home, params)
# SCALE = 1.5; flight_area_vertices = SCALE * np.array([[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices, drone.pose_home[:2])
# Adding virtual obstacles for debugging
obstacles = [
# np.array([[-0.2, 0.35], [-0.2, -0.35], [0.2, -0.35], [0.2, 0.35]])
]
gridmap.add_virtual_rectangular_obstacles(obstacles)
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
goal_x, goal_y = planning(ox, oy, params.sweep_resolution)
if params.toFly:
prepare(drone)
raw_input('Press Enter to fly...')
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 0.5]),
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 1.0]),
# np.array([[0.7, -0.9], [0.8, -0.9], [0.8, -0.3], [0.7, -0.3]]) + np.array([-1.5, 1.0]),
# np.array([[-0.3, -0.4], [0.3, -0.4], [0.3, 0.1], [-0.3, 0.1]]) * 0.5
]
veh_coords = [[
1, 1, 0, -2
]] # array containing all vehicles in [x_0,y_0,x_fin,y_fin] format
initial_pose = []
initial_pose.append(veh_coords[0][0])
initial_pose.append(veh_coords[0][1])
# Define Search Region
flight_area_vertices = define_flight_area(initial_pose)
gridmap = GridMap(flight_area_vertices, initial_pose)
# gridmap.add_obstacles_to_grid_map(obstacles)
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = 2.0
goal_x, goal_y, gmap, covmap = planning(ox, oy,
reso) #waypoint geneneration
#test for grid map
# gridmap.draw_map(gridobstacles)
# plt.plot(goal_x,goal_y,'o', color='b')
wp_coords = [[]]
print("length of waypoints:", len(goal_x))
plt.figure()
for i in range(len(goal_x)):
# plt.plot(goal_x[i],goal_y[i],'o', color='b')
from grid_map import GridMap
import timeit
if __name__ == "__main__":
gm = GridMap(5, bit_depth=10)
for x in range(1000):
for y in range(1000):
gm.add(x, y, "loc:" + str((x, y)))
gm = gm.sub_grids[1][0]
print(gm)
gm = gm.sub_grids[0][0]
print(gm)
gm = gm.sub_grids[0][0]
print(gm)
gm = gm.sub_grids[0][0]
print(gm)
gm = gm.sub_grids[0][0]
print(gm)
gm = gm.sub_grids[0][0]
