示例#1
0
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()
示例#2
0
 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
示例#4
0
文件: parse.py 项目: kpx13/navaz
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()
示例#5
0
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)
示例#6
0
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)
示例#7
0
文件: parse.py 项目: kpx13/navaz
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)
示例#8
0
 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)
示例#9
0
    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