def cmd_publish(self, event):
"""
1. publish control cmds
2. save imgs and corresponding behaviors
"""
current_steer, current_speed = self.joy_to_cmd(self.last_speed)
self.control.steering_angle = current_steer
self.control.expected_speed = current_speed
self.last_speed = current_speed
#control.handbrake_on = brake
# control vehicle via speed & steer
ret = usimpy.UsimSetVehicleControlsBySA(self.id, self.control)
# get vehicle states
ret = usimpy.UsimGetVehicleState(self.id, self.states)
# get collision
ret = usimpy.UsimGetCollisionInformation(self.id, self.collision)
if self.data_flag:
# get RBG image
ret = usimpy.UsimGetOneCameraResponse(self.id, 0, self.image)
# save img
img_path = os.path.join(self.data_path, "img")
if not os.path.exists(img_path):
os.makedirs(img_path)
img = np.array(self.image.image[0:480 * 320 * 3])
img = img.reshape((320, 480, 3))
img_PIL = Image.fromarray(np.uint8(img))
img_PIL.save(os.path.join(img_path, ('%d' % self.count) + '.png'),
'png')
# save label
with open(os.path.join(self.data_path + '/label.txt'), 'a+') as f:
f.write(
'%d %d %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n'
% (max(self.start_id, self.count - 1), self.count,
self.states.pose.position.x_val,
self.states.pose.position.y_val,
self.states.pose.position.z_val,
self.states.pose.rotation.x_val,
self.states.pose.rotation.y_val,
self.states.pose.rotation.z_val,
self.states.steering_angle, self.states.forward_speed,
current_steer, current_speed))
print("count: %d, current speed: %.4f, steer: %.4f" %
(self.count, current_speed, current_steer))
self.count += 1
else:
print("current speed: %.4f, steer: %.4f" %
(current_speed, current_steer))
collision = usimpy.UsimCollision()
## image
image = usimpy.UsimCameraResponse()
count = 0
time1 = 0
time2 = 0
root_dir = './dataset/'
img_dir = root_dir + 'imgs/'
mkdir(img_dir)
while(1):
# control vehicle via speed & steer
ret = usimpy.UsimSetVehicleControlsBySA(id, control)
# get vehicle post & action
ret = usimpy.UsimGetVehicleState(id, action)
# get collision
ret =usimpy.UsimGetCollisionInformation(id, collision)
# get RGB image
ret = usimpy.UsimGetOneCameraResponse(id, 0, image)
# save image
img = np.array(image.image[0:480*320*3])
img = img.reshape((320, 480, 3))
img_PIL = Image.fromarray(np.uint8(img))
img_PIL.save(img_dir+str(action.time_stamp)+".png", 'png')
# save pose & action
with open(root_dir + 'labels.txt', 'a+') as f:
f.write('%d %d %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n' % (count, action.time_stamp, action.pose.position.x_val, \
action.pose.position.y_val, action.pose.position.z_val, action.pose.rotation.x_val, action.pose.rotation.y_val, \
action.pose.rotation.z_val, action.steering_angle, action.forward_speed))
# auxiliary info
def cmd_publish(self, event):
"""
1. publish control cmds
2. save imgs and corresponding behaviors and historical states
"""
cmd_steer, cmd_speed = self.joy_to_cmd()
self.control.steering_angle = cmd_steer
self.control.expected_speed = cmd_speed
#self.last_speed = cmd_speed
#control.handbrake_on = brake
# control vehicle via speed & steer
ret = usimpy.UsimSetVehicleControlsBySA(self.id, self.control)
# get vehicle states
ret = usimpy.UsimGetVehicleState(self.id, self.states)
# get collision
ret = usimpy.UsimGetCollisionInformation(self.id, self.collision)
if self.data_flag:
# get RBG image
ret = usimpy.UsimGetOneCameraResponse(self.id, 0, self.image)
# save img
img_path = os.path.join(self.data_path, "img")
if not os.path.exists(img_path):
os.makedirs(img_path)
img = np.array(self.image.image[0:480 * 320 * 3])
img = img.reshape((320, 480, 3))
img_PIL = Image.fromarray(np.uint8(img))
img_PIL.save(os.path.join(img_path, ('%d' % self.count) + '.png'),
'png')
# save to csv
indexes = [
max(self.start_id, self.count - i)
for i in reversed(range(self.num_frames))
]
pose = [
self.states.pose.position.x_val,
self.states.pose.position.y_val,
self.states.pose.position.z_val,
self.states.pose.rotation.x_val,
self.states.pose.rotation.y_val,
self.states.pose.rotation.z_val
]
if self.Q_steer.qsize() == 0:
for i in range(self.num_frames):
self.Q_steer.put(self.states.steering_angle)
self.Q_speed.put(self.states.forward_speed)
else:
self.Q_steer.put(self.states.steering_angle)
self.Q_speed.put(self.states.forward_speed)
steers = [self.Q_steer.get() for i in range(self.num_frames)]
speeds = [self.Q_speed.get() for i in range(self.num_frames)]
# put back (except buttom)
for i in range(1, self.num_frames):
self.Q_steer.put(steers[i])
self.Q_speed.put(speeds[i])
cmds = [cmd_steer, cmd_speed]
# save label
with open(os.path.join(self.data_path + '/label.csv'), 'a+') as f:
ff = csv.writer(f)
ff.writerow(indexes + pose + steers + speeds + cmds)
print("count: %d, current speed: %.4f, steer: %.4f" %
(self.count, cmd_speed, cmd_steer))
self.count += 1
else:
print("current speed: %.4f, steer: %.4f" % (cmd_speed, cmd_steer))