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