def ex_dynamic_lane_polygon_generation_muiltiple_obstacle3():
# Basic setup
trajectory = np.array([[0.0, 0.0], [2.0, 1.0], [5.0, 0.0], [7.0, 0.0],
[10.0, 0.5], [13.0, 0.0], [15.0, 0.0]])
example_with_traj(trajectory,
[Obstacle((14.7, 0.1), 0.7),
Obstacle((8.7, 0.1), 0.7)])
def ex_dynamic_lane_polygon_generation_vehicle_model():
# Basic setup
trajectory = np.array([[0.0, 0.0], [2.0, 1.0], [5.0, 0.0], [7.0, 0.0],
[10.0, 0.5], [13.0, 0.0], [15.0, 0.0]])
model = VehicleModel(length=2.7, width=1.5)
example_with_traj(trajectory,
[Obstacle((14.7, 0.1), 0.7),
Obstacle((8.7, 0.1), 0.7)],
model=model)
def cb_obstacle_detection(self, data):
if self.is_initialized():
for o in self.obstacle_list:
o[2] -= 1
sum_decay = 0
for o in self.obstacle_list:
sum_decay += o[2]
if (sum_decay <= 0):
self.lane_polygon.reset_obstacle_list()
self.cnt_obstacles = 0
self.obstacle_list = []
else:
for i, o in enumerate(data.obstacles):
match = False
for gl_o in self.obstacle_list:
if np.abs(gl_o[1][0] - o.global_pose.position.x) < 0.5 and \
np.abs(gl_o[1][1] - o.global_pose.position.y) < 0.5:
match = True
gl_o[2] += 1
if gl_o[2] > 40:
gl_o[2] = 40
if not match:
p = np.array([o.pose.position.x, o.pose.position.y])
gl_pos = np.array(
[o.global_pose.position.x, o.global_pose.position.y])
ob = Obstacle(p, o.radius)
self.obstacle_deque.append(ob)
self.obstacle_list.append([ob, gl_pos, 1])
# TODO: migrate this part to a time-oriented version
# if self.cnt_obstacles != len(self.obstacle_list):
if self.cnt_obstacles != len(
self.obstacle_list
) and self.current_state == self.states["RELAY"]:
# TODO: this is temporal
self.cnt_obstacles = len(self.obstacle_list)
# self.planning_point = self.closest_waypoint + self.lookahead_points - 14
print(self.obstacle_list)
if not self.feasible_plan or self.current_state == self.states[
"RELAY"]:
self.event_plan()
self.current_state = self.states["REPLANNING"]
if len(self.final_lane.waypoints) > 0:
self.feasible_plan = True
self.planning_point = self.search_for_waypoint(
self.final_lane.waypoints[-1].pose.pose.position)
else:
self.feasible_plan = False
if self.cnt_obstacles == 0 and self.closest_waypoint > self.planning_point and self.current_state == \
self.states["REPLANNING"]:
self.current_state = self.states["RELAY"]
self.prev_obstacle_event = rospy.Time.now()
def cb_obstacle_detection(self, data):
if self.is_initialized():
for i, o in enumerate(data.obstacles):
gl_pos = np.array([o.global_pose.position.x, o.global_pose.position.y])
ob = Obstacle(gl_pos, o.radius)
self.obstacle_deque.append(ob)
self.obstacle_list.append([ob, gl_pos, 1])
if not self.feasible_plan or self.current_state == self.states["RELAY"]:
self.event_plan()
self.current_state = self.states["REPLANNING"]
if len(self.final_lane.waypoints) > 0:
self.feasible_plan = True
self.planning_point = self.search_for_waypoint(self.final_lane.waypoints[-1].pose.pose.position)
else:
self.feasible_plan = False
if self.closest_waypoint > self.planning_point \
and self.current_state == self.states["REPLANNING"]:
self.current_state = self.states["RELAY"]
"""
def ex_dynamic_lane_polygon_generation_different_starting_lane():
# Basic setup
trajectory = np.array([[0.0, 0.0], [2.0, 1.0], [5.0, 0.0], [7.0, 0.0],
[10.0, 0.5], [13.0, 0.0], [15.0, 0.0]])
example_with_traj(trajectory, [Obstacle((1.7, 0.1), 0.7)], start_lane=3)
def ex_dynamic_lane_polygon_generation_straight():
# Basic setup
trajectory = np.array([[0.0, 0.0], [2.0, 1.0], [5.0, 0.0], [7.0, 0.0],
[10.0, 0.5], [13.0, 0.0], [15.0, 0.0]])
example_with_traj(trajectory, [Obstacle((10.1, 0.1), 0.7)])
def ex_dynamic_lane_polygon_generation():
# Basic setup
trajectory = np.array([[0.0, 0.0], [0.5, 0.0], [1.0, 0.0], [2.0, 1.0],
[5.0, 3.0], [7.0, 5.0], [10.0, 8.0], [7.0, 11.0],
[5.0, 14.0]])
example_with_traj(trajectory, [Obstacle((10.1, 8.1), 0.7)])