def select_next_action(self):
self._min_cur_radar_scans = sys.maxsize
humans = self._create_human_data()
obstacles = self._create_obstacles_data()
self.debug_info['min_proximities'].append(self._min_cur_radar_scans)
location = self._gps.location()
## we might need to limit te magnitude of this vector
v_0 = self._get_target_direction_input_data()
v_0 = v_0 * 0.306 / math.sqrt(np.sum(np.power(v_0, 2)))
if self.count == 0:
v = 0.05 * Vector.unit_vec_from_radians(
Vector.radians_between(location, self._target.position))
theta = math.atan2(v[1], v[0])
self._old_position = self._start_pos
else:
v = location - self._old_position
theta = math.atan2(v[1], v[0])
self._old_position = location
force = self.force(v_0, v, np.array([location[0], location[1], theta]),
humans, obstacles)
#force = force*0.306/math.sqrt(np.sum(np.power(force,2)))
force_norm = math.sqrt(np.sum(np.power(force, 2)))
force = force * 0.306 / force_norm if force_norm > 0.306 else force
v_next = v + force * 1
speed = math.sqrt(np.sum(np.power(v_next, 2)))
direction = math.atan2(v_next[1], v_next[0]) * 180 / math.pi
self.count += 1
return RobotControlInput(speed, direction)
def select_next_action(self):
self._step_num += 1
msgs = self._sensors['recv'].get_recent_bcasts()
for msg in msgs:
if msg['msg']['action'] == 'cleared_node' and msg['msg'][
'node'] in self._node_props:
self._update_node_props_from_visit(msg['msg']['node'],
msg['msg']['visit_time'],
msg['msg']['avg_reward'])
for node in self._node_props:
self._node_props[node]['expected_reward'] += self._node_props[
node]['RAR']
if self._gps.distance_to(self._cur_path[-1].location) <= 1:
if self._cur_path[-1] in self._node_props:
self._visit_cur_node(self._cur_path[-1])
final_node = self._cur_path.pop()
while len(self._cur_path) == 0:
self._choose_next_path(nearest_node=final_node)
direction = self._gps.angle_to(self._cur_path[-1].location)
speed = min(self._gps.distance_to(self._cur_path[-1].location), 15)
return RobotControlInput(speed, direction)
def select_next_action(self):
radar_data = self._radar.scan(self._gps.location())
self.debug_info['min_proximities'].append(np.min(radar_data))
self._movement.step(1)
next_pos = self._movement.get_pos()
direction = self._gps.angle_to(next_pos)
speed = self._gps.distance_to(next_pos)
return RobotControlInput(speed, direction)
def select_next_action(self): radar_data = self._radar.scan(self._gps.location()); radar_data_size = len(radar_data) target_angle = self._gps.angle_to(self._target.position) intervals = self._split_radar_to_intervals(radar_data); self.debug_info['intervals'] = intervals; direction = self._gps.angle_to(self._target.position); if(not np.all(radar_data >= self._range_limit/1.05)): tmp_heading = self._heading + (self._gps.angle_to(self._target.position) - self._heading)*0.00 chosen_interval = self._get_closest_interval(intervals, self._heading); direction = ((chosen_interval[0] + chosen_interval[1]) / 2 - self._heading) * 1 + self._heading self._heading = direction; speed = self._range_limit; # Go as fast as possible without leaving the observable zone return RobotControlInput(speed, direction);
def _select_next_action_local(self):
next_waypoint = self._node_list[np.random.randint(len(
self._node_list))]
if self._cur_path_index < len(self._cur_path):
next_waypoint = self._cur_path[self._cur_path_index].to_node
else:
print("Next waypoint out of bounds in select_next_action_local",
file=sys.stderr)
angle = Vector.getAngleBetweenPoints(self._gps.location(),
next_waypoint.pos)
speed = Vector.getDistanceBetweenPoints(self._gps.location(),
next_waypoint.pos)
if self._normal_speed < speed:
speed = self._normal_speed
return RobotControlInput(speed, angle)
def select_next_action(self):
self._mapper.add_observation()
self.debug_info['mapdata'] = self._mapper.get_grid_data()
self._invalidateNodes()
robot_location = self._gps.location()
if not (self._radar._env.get_obsflags(robot_location)
& ObsFlag.DYNAMIC_OBSTACLE
or self._mapper.get_grid_data()[int(robot_location[0])][int(
robot_location[1])] & ObsFlag.STATIC_OBSTACLE):
if any([
n.flag == 1 for n in self._solution
]) or (len(self._solution) > 0
and self._collides(robot_location, self._solution[0].data)):
self._regrow_rrt()
self._extract_solution()
while 0 < len(self._solution) and Vector.distance_between(
robot_location,
self._solution[0].data) < self._waypoint_threshold:
del self._solution[0]
if len(self._solution) > 0:
direction = Vector.degrees_between(robot_location,
self._solution[0].data)
dist = min(
self._maxstepsize,
Vector.distance_between(robot_location,
self._solution[0].data))
self.debug_info["path"] = self._solution
else:
#No valid path found
self._has_given_up = True
dist = 0
direction = np.random.uniform(low=0, high=360)
elif self._mapper.get_grid_data()[int(robot_location[0])][int(
robot_location[1])] & ObsFlag.STATIC_OBSTACLE:
self._has_given_up = True
dist = 0
direction = np.random.uniform(low=0, high=360)
else:
#If robot is inside dynamic obstacle
dist = 0
direction = np.random.uniform(low=0, high=360)
self.debug_info['rrt_tree'] = self._rrt
return RobotControlInput(dist, direction)
def select_next_action(self):
predicted_values = None
if self._net_type == 'perl':
input_dict = self._create_perl_input_data()
predicted_values = self._model.eval((input_dict, [
True if Vector.distance_between(
self._gps.location(), self._start_pos) < 0.01 else False
]))[0][0]
else:
input_dict = self._create_grp_input_data()
predicted_values = self._model.eval(input_dict)[0]
direction = np.argmax(predicted_values[0:360]) * 360 / 359
speed = predicted_values[360] * 0.31
#print(self._gps.location(), direction, speed, True if Vector.distance_between(self._gps.location(), self._start_pos) < 0.01 else False)
return RobotControlInput(speed, direction)
def select_next_action(self):
return RobotControlInput(0, 0)
def select_next_action(self):
# Variable naming note: some variables have _pdf on their
# name, which is meant to indicate that they represent some
# kind of probability distribution over the possible
# movement directions
self.stepNum += 1
targetpoint_pdf = self._create_targetpoint_pdf()
# The combined PDF will store the combination of all the
# PDFs, but for now that's just the targetpoint PDF
combined_pdf = targetpoint_pdf
# Scan the radar to get obstacle information
raw_radar_data = self._radar.scan(self._gps.location())
raw_dynamic_radar_data = self._radar.scan_dynamic_obstacles(
self._gps.location())
if self._with_predictor:
self._obstacle_predictor.add_observation(
self._gps.location(), raw_radar_data, raw_dynamic_radar_data,
self._obstacle_predictor_dynobs_getter_func)
normalized_radar_data = raw_radar_data / self._radar.radius
if (0 < self._cmdargs.radar_noise_level):
normalized_radar_data += self._gaussian_noise(
self._cmdargs.radar_noise_level, normalized_radar_data.size)
# Add the obstacle distribution into the combined PDF
combined_pdf = self._combine_pdfs(combined_pdf, normalized_radar_data)
# Process memory
if self._cmdargs.enable_memory:
mem_bias_pdf = self._create_memory_bias_pdf()
# Add the memory distribution to the combined PDF
combined_pdf = self._combine_pdfs(combined_pdf, mem_bias_pdf)
obstacle_predictor_pdf = np.zeros(self._radar.get_data_size())
if self._with_predictor:
obstacle_predictor_pdf = self._create_obstacle_predictor_pdf()
combined_pdf = self._combine_pdfs(combined_pdf,
obstacle_predictor_pdf)
# Possibly smooth the combined distribution, to avoid
# having sudden jumps in value
if (self._cmdargs.enable_pdf_smoothing_filter):
combined_pdf = self._putfilter(combined_pdf)
combined_pdf = np.maximum(combined_pdf, 0)
direction = self._pdf_angle_selector(
combined_pdf) * self._PDF.degree_resolution
# Set the speed
speed = self._select_speed(direction, raw_dynamic_radar_data)
if (self._cmdargs.show_real_time_plot):
self._update_plot([
combined_pdf, targetpoint_pdf, mem_bias_pdf,
normalized_radar_data, obstacle_predictor_pdf
])
return RobotControlInput(speed, direction)
def select_next_action(self):
state = self._get_state()
action = self._get_action(state)
return RobotControlInput(action[1], action[0])
def select_next_action(self):
return RobotControlInput(float('inf'), self._gps.angle_to(self._target.position));
def select_next_action(self):
#Scan the radar
self._dynamic_radar_data = self._radar.scan_dynamic_obstacles(
self._gps.location())
self._radar_data = self._radar.scan(self._gps.location())
# Give the current observation to the obstacle motion predictor
self.debug_info[
'future_obstacles'] = self._obstacle_predictor.add_observation(
self._gps.location(), self._radar_data,
self._dynamic_radar_data,
self._obstacle_predictor_dynobs_getter_func)
if self._last_solution_node.data[:2] != (int(
self._gps.location()[0]), int(self._gps.location()[1])):
self._solution.insert(0, self._last_solution_node)
self._pruneAndPrepend()
if not self._rrt.hasGoal(self._targetpos, self._goalThreshold):
self._rrt = Tree(
Node((self._gps.location()[0], self._gps.location()[1],
self._time)))
qgoal = Node(
(self._targetpos[0], self._targetpos[1],
self._minTimeMultiplier *
self._minTime(self._gps.location(), self._targetpos)))
self._status = self._grow_rrt(self._rrt, qgoal,
self._goalThreshold, True)
self._extract_solution()
if self._status == 0:
#There is a valid path from robot to goal
while 0 < len(self._solution) and (
Vector.distance_between(self._gps.location(),
self._solution[0].data[:2]) < 1.5
or self._solution[0].data[2] <= self._time):
del self._solution[0]
timeDiff = self._solution[0].data[2] - self._time
#Should always be > 0
direction = Vector.degrees_between(self._gps.location(),
self._solution[0].data[:2])
dist = min(
self._maxstepsize,
Vector.distance_between(self._gps.location(),
self._solution[0].data[:2]) / timeDiff)
self.debug_info["path"] = self._solution
self._last_solution_node = self._solution[0]
self._time += 1
if self._status == 1:
#No valid path found
self._has_given_up = True
dist = 0
direction = np.random.uniform(low=0, high=360)
if self._status == 2:
#Robot or goal is inside dynamic obstacle
dist = 0
direction = np.random.uniform(low=0, high=360)
return RobotControlInput(dist, direction)
