class CarNode(): def __init__(self): rospy.init_node('car_sim') rospy.Subscriber('twist2d', Twist, self.update_command) dt = rospy.get_param('~dt', default=0.05) noisy = rospy.get_param('~noisy', default=False) self.rate = rospy.Rate(1. / dt) self.car = CarModel(dt=dt) self.speed, self.steering = 0, 0 self.last_time = rospy.Time.now() self.pose_pub = rospy.Publisher('car_pose', Pose2D, queue_size=1) def update_command(self, msg): self.last_time = rospy.Time.now() self.speed = msg.linear.x self.steering = msg.angular.z def check_command_validity(self): """ Check if the command is not too old, else resets it to 0 """ if rospy.Time.now() - self.last_time > rospy.Duration(2): self.speed = 0 self.steering = 0 def work(self): while not rospy.is_shutdown(): self.check_command_validity() self.car.sim_step(speed=self.speed, steering=self.steering) self.pose_pub.publish(Pose2D(*self.car.X)) self.rate.sleep()
def __init__(self): rospy.init_node('car_sim') rospy.Subscriber('twist2d', Twist, self.update_command) dt = rospy.get_param('~dt', default=0.05) noisy = rospy.get_param('~noisy', default=False) self.rate = rospy.Rate(1. / dt) self.car = CarModel(dt=dt) self.speed, self.steering = 0, 0 self.last_time = rospy.Time.now() self.pose_pub = rospy.Publisher('car_pose', Pose2D, queue_size=1)
def load_car_models(self): """ Loads all car models from car_models.csv. Returns ------- None. """ cast = Cast("Car Model") self.car_models = dict() csv_helper = CSVHelper("data", "car_models.csv") for row in csv_helper.data: uid = cast.to_positive_int(row[0], "Uid") cast.uid = uid car_model_name = str(row[1]).strip(" ").strip('"') car_consumption = cast.to_positive_float(row[2], "Car consumption") drag_coeff = cast.to_positive_float(row[3], "Drag coeff") frontal_area = cast.to_positive_float(row[4], "Frontal area") mass = cast.to_positive_float(row[5], "Mass") battery_capacity \ = cast.to_positive_float(row[6], "Battery capacity") charger_capacity \ = cast.to_positive_float(row[7], "Charger capacity") cm = CarModel(uid, car_model_name, car_consumption, drag_coeff, frontal_area, mass, battery_capacity, charger_capacity) self.car_models[uid] = cm
def process(price_line): print price_line price_line = price_line.strip().split(',') category = price_line[0].decode('utf-8') name = price_line[1].decode('utf-8') art = price_line[2].decode('utf-8') cars = price_line[3].decode('utf-8').split(' ') color = price_line[4].decode('utf-8') price = price_line[9].decode('utf-8') if price == '': price = 0 profit = price_line[11].decode('utf-8') category = Category.add_and_get(category) color = Color.add_and_get(color) for c in cars: car = CarModel.add_and_get(c) if car: Item(category=category, car_model=car, color=color, name=name, art=art, price=price, profit=profit).save()
def solver3(): car_info = loadData(sys.argv[1]) grid_map = initialEmptyGridMap(car_info) barrier = getBarrier(car_info) #plotGridMap(barrier, 1000, 1000 + HORIZON) test_grid = np.copy(grid_map) car_model = CarModel() source = (0,0) max_time_steps = sorted(car_info.keys())[-1] current_time_step = 0 output_file = sys.argv[3] death = 0 cross = 0 clearFile(output_file) last_valid_came_from = {} f = open(output_file, 'a') (t, y) = source s = '{0}\n{1} {2}\n#\n'.format(0*GRID_T,CHICKEN_X,(0-NUM_GRIDS_BELOW_ZERO)*GRID_Y) f.write(s) f.close() while current_time_step < max_time_steps: incremental_car_history = {} incremental_car_history[current_time_step] = car_info[current_time_step] print 'Online Source: '+repr(source) car_model.updateSolver3(incremental_car_history ) speeds, intervals = car_model.getGaussianParams() #print speeds #print intervals #prin "\n#######################################################################\n" #car_model.printModel() updateGridMapSolver3(grid_map, current_time_step, incremental_car_history, car_model,speeds, intervals, barrier) #if current_time_step % 1000 == 0: # print speeds # print intervals #print car_model.initial_pos_xs #print car_model.initial_times # plotGridMap(grid_map, current_time_step, current_time_step + 500) came_from, sub_goal = oneStepAction(grid_map, source) #only need one step and no overlap then just transfer in source if sub_goal == (-1,-1): goal = last_valid_came_from[sorted(last_valid_came_from.keys())[-1]] while goal != None: current = last_valid_came_from[goal] if current[0] == current_time_step+1: source = current break goal = current else: last_valid_came_from = came_from source = sub_goal f = open(output_file, 'a') (t, y) = source s = '{0}\n{1} {2}\n#\n'.format(t*GRID_T,CHICKEN_X,(y-NUM_GRIDS_BELOW_ZERO)*GRID_Y) f.write(s) f.close() #check Excution addBlocks(test_grid, {source[0]:car_info[source[0]]}) if (test_grid[source[1]][current_time_step+1] != 0): print "death!", source, test_grid[source[1]][source[0]] death += 1 source = (current_time_step+1, 0) if (source[1] >= NUM_GRIDS_Y - EXTRA_GRIDS): cross += 1 #reach goal source = (current_time_step+1, 0) current_time_step += 1 f = open(output_file, 'a') (t,y) = source s = '{0}\n{1} {2}\n#\n'.format(t*GRID_T,CHICKEN_X,(y-NUM_GRIDS_BELOW_ZERO)*GRID_Y) f.write(s) f.close() getScore(cross, death) print 'write to {0} file'.format(output_file)
def solver2(): car_info = loadData(sys.argv[1]) grid_map = initialEmptyGridMap(car_info) test_grid = np.copy(grid_map) car_model = CarModel() source = (0,0) max_time_steps = sorted(car_info.keys())[-1] current_time_step = 0 output_file = sys.argv[3] death = 0 cross = 0 clearFile(output_file) needUpdateGrid = True last_valid_came_from = {} f = open(output_file, 'a') (t, y) = source s = '{0}\n{1} {2}\n#\n'.format(0*GRID_T,CHICKEN_X,(0-NUM_GRIDS_BELOW_ZERO)*GRID_Y) f.write(s) f.close() while current_time_step < max_time_steps: incremental_car_history = {} incremental_car_history[current_time_step] = car_info[current_time_step] if car_model.enough_info == False: print 'Online Source: '+repr(source) car_model.updateSolver2(incremental_car_history ) #car_model.printModel() updateGridMap(grid_map, current_time_step, incremental_car_history, car_model, car_model.enough_info) #plotGridMap(grid_map, current_time_step, current_time_step + 500) came_from, sub_goal = oneStepAction(grid_map, source) #only need one step and no overlab then just transfer in source if sub_goal == (-1,-1): goal = last_valid_came_from[sorted(last_valid_came_from.keys())[-1]] while goal != None: current = last_valid_came_from[goal] if current[0] == current_time_step+1: source = current break goal = current else: last_valid_came_from = came_from source = sub_goal f = open(output_file, 'a') (t, y) = source s = '{0}\n{1} {2}\n#\n'.format(t*GRID_T,CHICKEN_X,(y-NUM_GRIDS_BELOW_ZERO)*GRID_Y) f.write(s) f.close() #check Excution addBlocks(test_grid, {source[0]:car_info[source[0]]}) if (test_grid[source[1]][current_time_step+1] != 0): print "death!", source, test_grid[source[1]][source[0]] death += 1 source = (current_time_step+1, 0) if (source[1] >= NUM_GRIDS_Y - EXTRA_GRIDS): cross += 1 #reach goal source = (current_time_step+1, 0) current_time_step += 1 else: print 'Offline Source: '+repr(source) #car_model.printModel() if needUpdateGrid: updateGridMap(grid_map, current_time_step, incremental_car_history, car_model, car_model.enough_info) needUpdateGrid = False #plotGridMap(grid_map) came_from, goal = aStar(grid_map, source) generateAnswer(came_from, goal, output_file) if (goal[1] >= NUM_GRIDS_Y - EXTRA_GRIDS): cross += 1 source = (goal[0]+1,0) current_time_step = source[0] #for time_step in range(0, max_time_steps): # if time_step%500 == 0: # print time_step # car_model.printModel() # plotGridMap(grid_map, time_step, time_step + 4) getScore(cross, death) print 'write to {0} file'.format(output_file)
def process_aliaces(line): print line line = line.strip().split(',') name = line[0].decode('utf-8') alt_name = line[1].decode('utf-8') CarModel.set_alt_name(name, alt_name)
def __init__(self, car_model, N=2000, delta_bn=15, delta_s=0.5, T_prop=0.5): super(Simulation, self).__init__(car_model, N, delta_bn, delta_s, T_prop) self.car = CarModel() self.steer_pub = rp.Publisher('/steering', Steering, queue_size=10)
try: rp.init_node('simulation') rp.Rate(50) iterations = rp.get_param('~iterations') delta_bn = rp.get_param('~delta_bn') delta_s = rp.get_param('~delta_s') T_prop = rp.get_param('~T_prop') L = rp.get_param('~mid_fronwheel_dist') max_steering_angle = rp.get_param('~max_steering_angle') max_lin_vel = rp.get_param('~max_lin_vel') max_ang_vel_wheel = rp.get_param('~max_ang_vel_wheel') max_x_coord = rp.get_param('~max_x_coord') max_y_coord = rp.get_param('~max_y_coord') car = CarModel(max_x_coord, max_y_coord, L, max_steering_angle, max_lin_vel, max_ang_vel_wheel) sim = Simulation(car, iterations, delta_bn, delta_s, T_prop) G = sim.sst_planning() controls, route = sim.get_route(G) i = 0 prev_time = rp.Time.now().to_sec() while not rp.is_shutdown(): sim.publish_route(route, all_points=True) sim.publish_control(controls[i][0]) time = rp.Time.now().to_sec() delta_t = time - prev_time