def _rate_predictions(self, predictions): assert len(predictions) > 0 (px, py) = predictions[-1] if len(self._yield_set) > 0: robot_name = list(self._yield_set)[0] rx = self._states[robot_name]['x'] ry = self._states[robot_name]['y'] return 1.0 - MeasurementUtils.distance(px, py, rx, ry) / (self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM * 0.5) else: tx, ty = self._target[0] return MeasurementUtils.distance(px, py, tx, ty)
def _rate_predictions(self, predictions): assert len(predictions) > 0 (px, py) = predictions[-1] if len(self._yield_set) > 0: robot_name = list(self._yield_set)[0] rx = self._states[robot_name]['x'] ry = self._states[robot_name]['y'] return 1.0 - MeasurementUtils.distance(px, py, rx, ry) / (self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM * 0.5) else: tx, ty = self._target[0] return MeasurementUtils.distance(px, py, tx, ty)
def _compute_fear_factors(self): self._fear_factors = {} for robot_name, state in self._states.items(): x = state['x'] y = state['y'] theta = state['theta'] fear_factor = state['fear_factor'] for robot_name2, state2 in self._states.items(): if robot_name == robot_name2: continue x2 = state2['x'] y2 = state2['y'] theta2 = state2['theta'] dist = MeasurementUtils.distance(x, y, x2, y2) if dist > FearfulAlgorithm.FF_RADIUS: continue angle_diff = abs(MeasurementUtils.normalize_angle(theta - theta2)) if angle_diff > 0.5 * math.pi: continue fear_factor += (1 - dist / FearfulAlgorithm.FF_RADIUS) * math.cos(angle_diff) \ * self._states[robot_name]['fear_factor'] self._fear_factors[robot_name] = fear_factor
def _compute_fear_factors(self): self._fear_factors = {} for robot_name, state in self._states.items(): x = state['x'] y = state['y'] theta = state['theta'] fear_factor = state['fear_factor'] for robot_name2, state2 in self._states.items(): if robot_name == robot_name2: continue x2 = state2['x'] y2 = state2['y'] theta2 = state2['theta'] dist = MeasurementUtils.distance(x, y, x2, y2) if dist > FearfulAlgorithm.FF_RADIUS: continue angle_diff = abs(MeasurementUtils.normalize_angle(theta - theta2)) if angle_diff > 0.5 * math.pi: continue fear_factor += (1 - dist / FearfulAlgorithm.FF_RADIUS) * math.cos(angle_diff) \ * self._states[robot_name]['fear_factor'] self._fear_factors[robot_name] = fear_factor
def _cut_predictions(predictions, target): (tx, ty) = target for i in range(len(predictions)): (px, py) = predictions[i] if MeasurementUtils.distance(px, py, tx, ty) < 0.2: return predictions[:i+1] + [(px, py)] * (len(predictions) - i - 1) return predictions
def _cut_predictions(predictions, target): (tx, ty) = target for i in range(len(predictions)): (px, py) = predictions[i] if MeasurementUtils.distance(px, py, tx, ty) < 0.2: return predictions[:i+1] + [(px, py)] * (len(predictions) - i - 1) return predictions
def _target_distance(self): assert type(self._target) == type([]) assert len(self._target) > 0 (tx, ty) = self._target[0] mx = self._own_robot['x'] my = self._own_robot['y'] return MeasurementUtils.distance(tx, ty, mx, my)
def _target_distance(self): assert type(self._target) == type([]) assert len(self._target) > 0 (tx, ty) = self._target[0] mx = self._own_robot['x'] my = self._own_robot['y'] return MeasurementUtils.distance(tx, ty, mx, my)
def _navigate2(self, avail_time): dist = None x = self._own_robot['x'] y = self._own_robot['y'] theta = self._own_robot['theta'] Otheta = self._own_robot['org_theta'] Otheta = 1.57 - Otheta; v = FearfulAlgorithm.CRUISE_SPEED if self._target is None: tx, ty = self._controller.get_new_target() self._target = self._maze_util.find_path((x, y), (tx, ty)) target1 = self._target[0] tx, ty = target1 bearing = math.atan2(ty - y, tx - x) target_dist = MeasurementUtils.distance(x, y, tx, ty) Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,bearing,False,target_dist) #self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr)) #if bearing < 0: # bearing = bearing * (-1) #target_dist = MeasurementUtils.distance(x, y, tx, ty) #Vl = 0.0 #Vr = 0.0 #self._logger.info('ORG THETA: %f' % ( Otheta)) #self._logger.info("FindIntersction1 Vl: %f; Vr: %f; v: %f; theta: %f; bearing: %f; target_distance: %f; sub: %f" % (Vl, Vr, v, Otheta, bearing, target_dist, Otheta - bearing)) #Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,theta,bearing,False,target_dist) #if Otheta < -3.14: # Otheta = Otheta + 6.28 #elif Otheta > 3.14: # Otheta = Otheta - 6.28 #self._logger.info("FindIntersction2 Vl: %f; Vr: %f; v: %f; theta: %f; bearing: %f; target_distance: %f; sub: %f" % (Vl, Vr, v, Otheta, bearing, target_dist, Otheta + bearing )) #self._logger.info("Test!!!!") #self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr)) return
def _distances_to_robots(self): distances = [] x = self._own_robot['x'] y = self._own_robot['y'] for robot_name, state in self._states.items(): if robot_name == self._robot_name: continue rx = state['x'] ry = state['y'] distance = MeasurementUtils.distance(x, y, rx, ry) distances.append((robot_name, distance)) distances.sort(key=itemgetter(1)) return distances
def _distances_to_robots(self): distances = [] x = self._own_robot['x'] y = self._own_robot['y'] for robot_name, state in self._states.items(): if robot_name == self._robot_name: continue rx = state['x'] ry = state['y'] distance = MeasurementUtils.distance(x, y, rx, ry) distances.append((robot_name, distance)) distances.sort(key=itemgetter(1)) return distances
def _find_intersections(self, own_predictions, radius, ignored_num): assert len(own_predictions) > 0 for (robot_name, other_predictions) in self._predictions.items(): if self._robot_name == robot_name: continue if self._fear_factors[self._robot_name] > self._fear_factors[robot_name]: continue for ((mx, my), (ox, oy)) in zip(own_predictions[ignored_num:], other_predictions[ignored_num:]): dst = MeasurementUtils.distance(mx, my, ox, oy) if dst <= radius: return True maze = self._maze_util.find_intersection(own_predictions, 0.25) return maze
def _find_intersections(self, own_predictions, radius, ignored_num): assert len(own_predictions) > 0 for (robot_name, other_predictions) in self._predictions.items(): if self._robot_name == robot_name: continue if self._states[self._robot_name]['fear_factor'] > self._states[robot_name]['fear_factor']: continue for ((mx, my), (ox, oy)) in zip(own_predictions[ignored_num:], other_predictions[ignored_num:]): dst = MeasurementUtils.distance(mx, my, ox, oy) if dst <= radius: return True maze = self._maze_util.find_intersection(own_predictions, 0.25) return maze
def _update_states(self): new_states = self._controller.request_states() if not self._running or len(new_states) == 0: return False for (robot_name, new_state) in new_states.items(): org_x = new_state.get_x() org_y = new_state.get_y() org_theta = new_state.get_theta() timestamp = new_state.get_timestamp() fear_factor = new_state.get_fear_factor() if robot_name not in self._states: location_kalman = LocationKalman(org_x, org_y, self.INTERVAL) angle_kalman = AngleKalman(org_theta, self.INTERVAL) state = {'location_kalman': location_kalman, 'angle_kalman': angle_kalman, 'timestamp': timestamp, 'x': org_x, 'y': org_y, 'vx': 0.0, 'vy': 0.0, 'v': 0.0, 'theta': org_theta, 'omega': 0.0, 'epsilon': 0.0, 'fear_factor': fear_factor} self._states[robot_name] = state state = self._states[robot_name] location_kalman = state['location_kalman'] angle_kalman = state['angle_kalman'] if timestamp > state['timestamp']: location_kalman.step(org_x, org_y) angle_kalman.step(org_theta) else: location_kalman.missing_step() angle_kalman.missing_step() (x, y, vx, vy) = location_kalman.get_means() (theta, omega, epsilon) = angle_kalman.get_means() v = math.sqrt(vx * vx + vy * vy) if robot_name == self._robot_name: self._odometer += MeasurementUtils.distance(state['x'], state['y'], x, y) state['x'] = x state['y'] = y state['vx'] = vx state['vy'] = vy state['v'] = v state['theta'] = theta state['omega'] = omega state['epsilon'] = epsilon state['timestamp'] = timestamp state['fear_factor'] = fear_factor if self._f is not None and robot_name == self._robot_name: self._f.write("%i %f %f %f %f %f %f %f %f\n" % (timestamp, x, org_x, y, org_y, v, theta, org_theta, omega)) if self._robot_name not in self._states: return False self._own_robot = self._states[self._robot_name] return True
def _navigate(self, avail_time): dist = None x = self._own_robot['x'] y = self._own_robot['y'] theta = self._own_robot['theta'] v = SimpleAlgorithm.CRUISE_SPEED if self._target is None: tx, ty = self._controller.get_new_target() self._target = self._maze_util.find_path((x, y), (tx, ty)) if not self._target_reached and self._target_distance() < 0.1: if len(self._target) > 1: self._target.pop(0) else: new_target = self._controller.get_new_target() if new_target is None or new_target != self._target[0]: tx, ty = new_target self._target = self._maze_util.find_path((x, y), (tx, ty)) else: self._target_reached = True self._logger.info("Target reached") self.send_finish_msg() space_id = self._maze_util.find_space_by_point((x, y)) if space_id != self._space_id: if self._space_id is not None: tx, ty = self._target[-1] self._target = self._maze_util.find_path((x, y), (tx, ty)) self._space_id = space_id if self._avoid_close_objects(x, y, theta): return target = self._target[0] tx, ty = target bearing = MeasurementUtils.angle(x, y, tx, ty) preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0) preds = SimpleAlgorithm._cut_predictions(preds, target) self._predictions[self._robot_name] = preds target_dist = MeasurementUtils.distance(x, y, tx, ty) if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0): self._own_fear_factor = self._base_fear_factor v, omega = SimpleAlgorithm._navigate_fun(v, theta, bearing, False) if target_dist < 0.2: v *= target_dist if target_dist < 0.05: v = 0 Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega) self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr)) return for robot_name in list(self._yield_set): rx = self._states[robot_name]['x'] ry = self._states[robot_name]['y'] if MeasurementUtils.distance(x, y, rx, ry) > 2.0: self._yield_set.remove(robot_name) best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None self._predictions[self._robot_name] = None if self._last_params is not None: best_v, best_bearing, best_midpoint = self._last_params best_preds = PredictionUtils.predict_positions( x, y, best_v, best_bearing, 0.0) best_preds = SimpleAlgorithm._cut_predictions( best_preds, best_midpoint) best_rate = self._rate_predictions(best_preds) if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1): best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None try_stop = True variants_stop_time = time.time() + avail_time - 0.001 var_num = 0 while time.time() < variants_stop_time: var_num += 1 if try_stop: rand_v = 0.0 rand_bearing = theta rand_radius = 0.0 try_stop = False else: rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0) rand_bearing = random.uniform(-math.pi, math.pi) rand_radius = random.uniform( 0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM) mp_x = rand_radius * math.cos(rand_bearing) + x mp_y = rand_radius * math.sin(rand_bearing) + y rand_midpoint = mp_x, mp_y rand_preds = PredictionUtils.predict_positions( x, y, rand_v, rand_bearing, 0.0) rand_preds = SimpleAlgorithm._cut_predictions( rand_preds, rand_midpoint) rand_rate = self._rate_predictions(rand_preds) if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5, 1): continue if best_rate is None or rand_rate < best_rate - 0.5: best_v, best_bearing, best_preds, best_rate, best_midpoint \ = rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint self._last_params = best_v, best_bearing, best_midpoint if best_preds is None: self._send_stop_command() return else: self._predictions[self._robot_name] = best_preds best_v, omega = SimpleAlgorithm._navigate_fun( best_v, theta, best_bearing, False) mp_x, mp_y = best_midpoint midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y) if midpoint_dist < 0.5: best_v *= midpoint_dist Vl, Vr = SimpleAlgorithm._get_wheel_speeds(best_v, omega) self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
def _navigate(self, avail_time): dist = None x = self._own_robot['x'] y = self._own_robot['y'] theta = self._own_robot['theta'] v = SimpleAlgorithm.CRUISE_SPEED if self._target is None: tx, ty = self._controller.get_new_target() self._target = self._maze_util.find_path((x, y), (tx, ty)) if not self._target_reached and self._target_distance() < 0.1: if len(self._target) > 1: self._target.pop(0) else: new_target = self._controller.get_new_target() if new_target is None or new_target != self._target[0]: tx, ty = new_target self._target = self._maze_util.find_path((x, y), (tx, ty)) else: self._target_reached = True self._logger.info("Target reached") self.send_finish_msg() space_id = self._maze_util.find_space_by_point((x, y)) if space_id != self._space_id: if self._space_id is not None: tx, ty = self._target[-1] self._target = self._maze_util.find_path((x, y), (tx, ty)) self._space_id = space_id if self._avoid_close_objects(x, y, theta): return target = self._target[0] tx, ty = target bearing = MeasurementUtils.angle(x, y, tx, ty) preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0) preds = SimpleAlgorithm._cut_predictions(preds, target) self._predictions[self._robot_name] = preds target_dist = MeasurementUtils.distance(x, y, tx, ty) if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0): self._own_fear_factor = self._base_fear_factor v, omega = SimpleAlgorithm._navigate_fun(v, theta, bearing, False) if target_dist < 0.2: v *= target_dist if target_dist < 0.05: v = 0 Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega) self._send_robot_command( RobotCommand(Vl, Vr, Vl, Vr)) return for robot_name in list(self._yield_set): rx = self._states[robot_name]['x'] ry = self._states[robot_name]['y'] if MeasurementUtils.distance(x, y, rx, ry) > 2.0: self._yield_set.remove(robot_name) best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None self._predictions[self._robot_name] = None if self._last_params is not None: best_v, best_bearing, best_midpoint = self._last_params best_preds = PredictionUtils.predict_positions(x, y, best_v, best_bearing, 0.0) best_preds = SimpleAlgorithm._cut_predictions(best_preds, best_midpoint) best_rate = self._rate_predictions(best_preds) if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1): best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None try_stop = True variants_stop_time = time.time() + avail_time - 0.001 var_num = 0 while time.time() < variants_stop_time: var_num += 1 if try_stop: rand_v = 0.0 rand_bearing = theta rand_radius = 0.0 try_stop = False else: rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0) rand_bearing = random.uniform(-math.pi, math.pi) rand_radius = random.uniform(0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM) mp_x = rand_radius * math.cos(rand_bearing) + x mp_y = rand_radius * math.sin(rand_bearing) + y rand_midpoint = mp_x, mp_y rand_preds = PredictionUtils.predict_positions(x, y, rand_v, rand_bearing, 0.0) rand_preds = SimpleAlgorithm._cut_predictions(rand_preds, rand_midpoint) rand_rate = self._rate_predictions(rand_preds) if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5, 1): continue if best_rate is None or rand_rate < best_rate - 0.5: best_v, best_bearing, best_preds, best_rate, best_midpoint \ = rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint self._last_params = best_v, best_bearing, best_midpoint if best_preds is None: self._send_stop_command() return else: self._predictions[self._robot_name] = best_preds best_v, omega = SimpleAlgorithm._navigate_fun(best_v, theta, best_bearing, False) mp_x, mp_y = best_midpoint midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y) if midpoint_dist < 0.5: best_v *= midpoint_dist Vl, Vr = SimpleAlgorithm._get_wheel_speeds(best_v, omega) self._send_robot_command( RobotCommand(Vl, Vr, Vl, Vr))
def _navigate(self, avail_time): dist = None #self._logger.info("Nowy kod") x = self._own_robot['x'] y = self._own_robot['y'] theta = self._own_robot['theta'] Otheta = self._own_robot['org_theta'] Otheta = 1.57 - Otheta v = FearfulAlgorithm.CRUISE_SPEED if self._target is None: tx, ty = self._controller.get_new_target() self._target = self._maze_util.find_path((x, y), (tx, ty)) if not self._target_reached and self._target_distance() < FearfulAlgorithm.DISTANCE_GOAL: if len(self._target) > 1: self._target.pop(0) else: new_target = self._controller.get_new_target() if new_target is None or new_target != self._target[0]: tx, ty = new_target self._target = self._maze_util.find_path((x, y), (tx, ty)) else: self._target_reached = True self._logger.info("Target reached") self.send_finish_msg() space_id = self._maze_util.find_space_by_point((x, y)) if space_id != self._space_id: if self._space_id is not None: tx, ty = self._target[-1] self._target = self._maze_util.find_path((x, y), (tx, ty)) self._space_id = space_id # ominiecie robota lub robotow if self._avoid_close_objects(x, y, theta,Otheta): return target = self._target[0] tx, ty = target bearing = MeasurementUtils.angle(x, y, tx, ty) bearingNew = math.atan2(ty - y, tx - x) #tylko dla nawigacji preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0) preds = FearfulAlgorithm._cut_predictions(preds, target) self._predictions[self._robot_name] = preds target_dist = MeasurementUtils.distance(x, y, tx, ty) if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0): self._own_fear_factor = self._base_fear_factor self._logger.info("find intersectio") Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,bearingNew,False,target_dist) self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr)) #v, omega = FearfulAlgorithm._navigate_fun(v, theta, bearing, False) #if target_dist < 0.2: # v *= target_dist #if target_dist < 0.05: # v = 0 ##Vl, Vr = FearfulAlgorithm._get_wheel_speeds(v, omega) #Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,Otheta,bearing,False,target_dist,tx - x,ty - y) #self._logger.info("FindIntersction Vl: %f; Vr: %f; v: %f; theta: %f; bearing: %f; target_distance: %f; sub: %f,x: %f, y: %f" % (Vl, Vr, v, Otheta, bearing, target_dist, Otheta - bearing,x,y)) #self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr)) return for robot_name in list(self._yield_set): rx = self._states[robot_name]['x'] ry = self._states[robot_name]['y'] if MeasurementUtils.distance(x, y, rx, ry) > 2.0: self._yield_set.remove(robot_name) best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None self._predictions[self._robot_name] = None if self._last_params is not None: best_v, best_bearing, best_midpoint = self._last_params best_preds = PredictionUtils.predict_positions(x, y, best_v, best_bearing, 0.0) best_preds = FearfulAlgorithm._cut_predictions(best_preds, best_midpoint) best_rate = self._rate_predictions(best_preds) if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1): best_v = None best_bearing = None best_midpoint = None best_rate = None best_preds = None try_stop = True variants_stop_time = time.time() + avail_time - 0.001 var_num = 0 while time.time() < variants_stop_time: var_num += 1 if try_stop: rand_v = 0.0 rand_bearing = theta rand_radius = 0.0 try_stop = False else: rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0) rand_bearing = random.uniform(-math.pi, math.pi) rand_radius = random.uniform(0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM) mp_x = rand_radius * math.cos(rand_bearing) + x mp_y = rand_radius * math.sin(rand_bearing) + y rand_midpoint = mp_x, mp_y rand_preds = PredictionUtils.predict_positions(x, y, rand_v, rand_bearing, 0.0) rand_preds = FearfulAlgorithm._cut_predictions(rand_preds, rand_midpoint) rand_rate = self._rate_predictions(rand_preds) if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5, 1): continue if best_rate is None or rand_rate < best_rate - 0.5: best_v, best_bearing, best_preds, best_rate, best_midpoint \ = rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint self._last_params = best_v, best_bearing, best_midpoint if best_preds is None: self._logger.info("best pred is None -->>>>> Stop command:") self._send_stop_command() return else: self._predictions[self._robot_name] = best_preds #best_v, omega = FearfulAlgorithm._navigate_fun(best_v, theta, best_bearing, False) mp_x, mp_y = best_midpoint midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y) #if midpoint_dist < 0.5: # best_v *= midpoint_dist #Vl, Vr = FearfulAlgorithm._get_wheel_speeds(best_v, omega) ##Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,Otheta,best_bearing,False,target_dist, mp_x - x, mp_y - y) ##self._logger.info("Prediction Vl: %f; Vr: %f; v: %f; theta: %f; best_bearing: %f; target_distance: %f; sub: %f,x: %f,y: %f" % (Vl, Vr, v, Otheta, best_bearing, target_dist,Otheta - best_bearing,x,y)) self._logger.info("Prediction:") Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,best_bearing,False,target_dist) self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
def _update_states(self): new_states = self._controller.request_states() if not self._running or len(new_states) == 0: return False for (robot_name, new_state) in new_states.items(): org_x = new_state.get_x() org_y = new_state.get_y() org_theta = new_state.get_theta() timestamp = new_state.get_timestamp() fear_factor = new_state.get_fear_factor() self._logger.debug("org_theta: %f" % (org_theta)) if robot_name not in self._states: location_kalman = LocationKalman(org_x, org_y, self.INTERVAL) angle_kalman = AngleKalman(org_theta, self.INTERVAL) state = {'location_kalman': location_kalman, 'angle_kalman': angle_kalman, 'timestamp': timestamp, 'x': org_x, 'y': org_y, 'vx': 0.0, 'vy': 0.0, 'v': 0.0, 'theta': org_theta, 'omega': 0.0, 'epsilon': 0.0, 'fear_factor': fear_factor, 'org_theta': org_theta} self._states[robot_name] = state state = self._states[robot_name] location_kalman = state['location_kalman'] angle_kalman = state['angle_kalman'] if timestamp > state['timestamp']: location_kalman.step(org_x, org_y) angle_kalman.step(org_theta) else: location_kalman.missing_step() angle_kalman.missing_step() (x, y, vx, vy) = location_kalman.get_means() (theta, omega, epsilon) = angle_kalman.get_means() v = math.sqrt(vx * vx + vy * vy) if robot_name == self._robot_name: self._odometer += MeasurementUtils.distance(state['x'], state['y'], x, y) state['x'] = x state['y'] = y state['vx'] = vx state['vy'] = vy state['v'] = v state['theta'] = theta state['omega'] = omega state['epsilon'] = epsilon state['timestamp'] = timestamp state['fear_factor'] = fear_factor state['org_theta'] = org_theta if self._f is not None and robot_name == self._robot_name: self._f.write("%i %f %f %f %f %f %f %f %f\n" % (timestamp, x, org_x, y, org_y, v, theta, org_theta, omega)) if self._robot_name not in self._states: return False self._own_robot = self._states[self._robot_name] return True