def callback(data):
#rospy.loginfo(data)
rownum=data.layout.dim[0].size
columnnum=data.layout.dim[1].size
dataset=data.data
subarrayset=[]
#count=0
for i in range(0,rownum-1):
temp=[]
for k in range(0,columnnum-1):
temp.append(dataset[rownum*i+k])
(x,y,z)=findsubarray(temp)
#rospy.loginfo((x,y,z))
for j in range(x,y):
subarrayset.append(dataset[rownum*i+j])
#count=count+y-x+1
#rospy.loginfo(count)
#rospy.loginfo(subarrayset)
(begin,end,summary)=findsubarray(subarrayset)
rospy.loginfo((begin,end,summary))
outputdata=[begin,end]
pub=rospy.Publisher('/hw1/subarray', UInt32MultiArray, queue_size=10)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension('height', 1, 2 * 1),
MultiArrayDimension('width', 2, 2)]
pub.publish(layout, outputdata)
def gen_layout(shape):
layout = MultiArrayLayout()
layout.dim = [
gen_dim('dim{}'.format(i), size, 1) for i, size in enumerate(shape)
]
layout.data_offset = 0
return layout
def callback(msg,prevMarkers):
detectedMarkers = msg.markers
now = rospy.get_time() #the current time in seconds
for detectedMarker in detectedMarkers:
measuredTime = detectedMarker.header.stamp.secs
markerID = detectedMarker.id
prevMarkers[markerID] = measuredTime
detected_markers = []
for marker in prevMarkers.keys():
if abs(prevMarkers[marker]-now) > 5: #if the measurement has been stale for 5 seconds
del prevMarkers[marker]
else:
detected_markers.append(marker)
array1 = MultiArrayDimension()
array1.label = 'numMarkers'
array1.size = len(detected_markers)
array1.size = len(detected_markers)
layout = MultiArrayLayout()
layout.dim = [array1,]
layout.data_offset = 0
msg = Int16MultiArray()
msg.layout = layout
msg.data = detected_markers
pub.publish(msg)
def callback(msg, prevMarkers):
detectedMarkers = msg.markers
# The current time in seconds
now = rospy.get_time()
for detectedMarker in detectedMarkers:
measuredTime = detectedMarker.header.stamp.secs
markerID = detectedMarker.id
prevMarkers[markerID] = measuredTime
detected_markers = list()
for marker in prevMarkers.keys():
if abs(prevMarkers[marker] - now) > 5:
del prevMarkers[marker]
else:
detected_markers.append(marker)
array = MultiArrayDimension()
array.label = 'numMarkers'
array.size = len(detected_markers)
array.size = len(detected_markers)
layout = MultiArrayLayout()
layout.dim = [
array,
]
layout.data_offset = 0
msg = Int16MultiArray()
msg.layout = layout
msg.data = detected_markers
pub.publish(msg)
def publish(self):
# compose the multiarray message
jointVelocities = Float32MultiArray()
myLayout = MultiArrayLayout()
myMultiArrayDimension = MultiArrayDimension()
myMultiArrayDimension.label = "joint_velocities"
myMultiArrayDimension.size = 1
myMultiArrayDimension.stride = self.numOfWheels
myLayout.dim = [myMultiArrayDimension]
myLayout.data_offset = 0
jointVelocities.layout = myLayout
if rospy.get_time() - self.lastTwistTime < self.timeout:
self.right = 1.0 * self.linearVelocity + self.angularVelocity * self.baseWidth / 2
self.left = 1.0 * self.linearVelocity - self.angularVelocity * self.baseWidth / 2
rospy.loginfo("wheels velocities vl, vr [{0:.3f},{1:.3f}]".format(self.left, self.right))
if self.numOfWheels == 2:
# first item is left and second is right
jointVelocities.data = [self.left, self.right]
elif self.numOfWheels == 4:
jointVelocities.data = [self.left, self.right, self.left, self.right]
else:
if self.numOfWheels == 2:
jointVelocities.data = [0.0, 0.0]
# first item is left and second is right
elif self.numOfWheels == 4:
jointVelocities.data = [0.0, 0.0, 0.0, 0.0]
self.joint_velocities_Pub.publish(jointVelocities)
def write_ms25(ms25, i, bag):
utime = ms25[i, 0]
mag_x = ms25[i, 1]
mag_y = ms25[i, 2]
mag_z = ms25[i, 3]
accel_x = ms25[i, 4]
accel_y = ms25[i, 5]
accel_z = ms25[i, 6]
rot_r = ms25[i, 7]
rot_p = ms25[i, 8]
rot_h = ms25[i, 9]
timestamp = rospy.Time.from_sec(utime/1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "xyz"
layout.dim[0].size = 3
layout.dim[0].stride = 1
mag = Float64MultiArray()
mag.data = [mag_x, mag_y, mag_z]
mag.layout = layout
accel = Float64MultiArray()
accel.data = [accel_x, accel_y, accel_z]
accel.layout = layout
layout_rph = MultiArrayLayout()
layout_rph.dim = [MultiArrayDimension()]
layout_rph.dim[0].label = "rph"
layout_rph.dim[0].size = 3
layout_rph.dim[0].stride = 1
rot = Float64MultiArray()
rot.data = [rot_r, rot_p, rot_h]
rot.layout = layout_rph
bag.write('mag', mag, t=timestamp)
bag.write('accel', accel, t=timestamp)
bag.write('rot', rot, t=timestamp)
def main(args):
if len(sys.argv) < 2:
print 'Please specify hokuyo_4m file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
# hokuyo_4m always has 726 hits
num_hits = 726
f_bin = open(sys.argv[1], "r")
bag = rosbag.Bag(sys.argv[2], 'w')
try:
while True:
# check for eof
data = f_bin.read(8)
if data == '':
break
# Read timestamp
utime = struct.unpack('<Q', data)[0]
r = np.zeros(num_hits)
for i in range(num_hits):
s = struct.unpack('<H', f_bin.read(2))[0]
r[i] = convert(s)
# Now write out to rosbag
timestamp = rospy.Time.from_sec(utime / 1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "r"
layout.dim[0].size = num_hits
layout.dim[0].stride = 1
hits = Float64MultiArray()
hits.data = r
hits.layout = layout
bag.write('hokuyo_4m_packet', hits, t=timestamp)
except Exception:
print 'End of File'
finally:
f_bin.close()
bag.close()
return 0
def sendCommand(self, ts_rostime):
msg = Float64MultiArray()
dim0 = MultiArrayDimension()
dim0.label = 'ts_rostime, numOfsamples, dt'
dim0.size = 3
layout_var = MultiArrayLayout()
layout_var.dim = [dim0]
msg.layout = layout_var
msg.data = [ts_rostime, self.numOfSamples, self.dt]
self.sampleParticalTrajectory_pub.publish(msg)
def publish(vals):
command = Float64MultiArray()
layout = MultiArrayLayout()
dimension = MultiArrayDimension()
dimension.label = "keyboard_control"
dimension.size = 4
dimension.stride = 4
layout.data_offset = 0
layout.dim = [dimension]
command.layout = layout
command.data = vals
control_pub.publish(command)
def init_multdata(self):
msg = MultiArrayLayout()
msg.data_offset = 0
msg.dim = [MultiArrayDimension()]
msg.dim[0].label= "Quat"
msg.dim[0].size = 4
msg.dim[0].stride = 4
return msg
def init_multdata(self):
msg = MultiArrayLayout()
msg.data_offset = 0
msg.dim = [MultiArrayDimension()]
msg.dim[0].label = "state_estimation"
msg.dim[0].size = 15
msg.dim[0].stride = 15
return msg
def init_multdata_NEW(self, data):
msg = MultiArrayLayout()
msg.data_offset = 0
msg.dim = [MultiArrayDimension()]
msg.dim[0].label = "state_estimation"
msg.dim[0].size = data.shape[0]
msg.dim[0].stride = data.shape[0]
return msg
def create_depth_msg(depth_img):
command = Float64MultiArray()
layout = MultiArrayLayout()
dimension = MultiArrayDimension()
dimension.label = "depth_img"
dimension.size = len(depth_img)
dimension.stride = len(depth_img)
layout.data_offset = 0
layout.dim = [dimension]
command.layout = layout
command.data = depth_img
return command
def flatten(self):
#tran_array = np.split(np.array(self.listener.data),240)
for i in self.listener.image_data:
tempa,tempb = self.max_array(i)
while tempa <= tempb:
self.flatten_array.append(i[tempa])
tempa += 1
finala,finalb = self.max_array(self.flatten_array)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension('height', 1, 2*1), MultiArrayDimension('width', 2, 2)]
index_data = [finala, finalb]
self.maxsub_pub.publish(layout,index_data)
print("in the show")
rospy.loginfo("calculating the work")
print(index_data)
rospy.spin()
def write_hokuyo_4m_packet(hok_4m, utime, bag):
# hokuyo_4m always has 726 hits
num_hits = 726
timestamp = microseconds_to_ros_timestamp(utime)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "r"
layout.dim[0].size = num_hits
layout.dim[0].stride = 1
hits = Float64MultiArray()
hits.data = hok_4m
hits.layout = layout
bag.write('/hokuyo_4m_packet', hits, t=timestamp)
def write_vel(vel_data, utime, bag):
timestamp = rospy.Time.from_sec(utime / 1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension(), MultiArrayDimension()]
layout.dim[0].label = "hits"
layout.dim[0].size = num_hits
layout.dim[0].stride = 5
layout.dim[1].label = "xyzil"
layout.dim[1].size = 5
layout.dim[1].stride = 1
vel = Float64MultiArray()
vel.data = vel_data
vel.layout = layout
bag.write('velodyne_packet', vel, t=timestamp)
def write_hokuyo_4m_packet(hok_4m, utime, bag):
# hokuyo_4m always has 726 hits
num_hits = 726
timestamp = rospy.Time.from_sec(utime/1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "r"
layout.dim[0].size = num_hits
layout.dim[0].stride = 1
hits = Float64MultiArray()
hits.data = hok_4m
hits.layout = layout
bag.write('hokuyo_4m_packet', hits, t=timestamp)
def PublishtoPurePursuit(self, waypoints):
pub_points = Float32MultiArray()
wp_len = len(waypoints)
dim0 = MultiArrayDimension()
dim1 = MultiArrayDimension()
dim0.label = "height"
dim0.size = wp_len/3
dim1.label = "width"
dim1.size = 3
dimensions = MultiArrayLayout()
dimensions.dim = [dim0, dim1]
pub_points.layout = dimensions
pub_points.data = waypoints
self.pursuit_pub.publish(pub_points)
def main(args):
if len(sys.argv) < 2:
print 'Please specify ms25_euler file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
ms25_euler = np.loadtxt(sys.argv[1], delimiter=",")
utimes = ms25_euler[:, 0]
rs = ms25_euler[:, 1]
ps = ms25_euler[:, 2]
hs = ms25_euler[:, 3]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime / 1e6)
layout_rph = MultiArrayLayout()
layout_rph.dim = [MultiArrayDimension()]
layout_rph.dim[0].label = "rph"
layout_rph.dim[0].size = 3
layout_rph.dim[0].stride = 1
euler = Float64MultiArray()
euler.data = [rs[i], ps[i], hs[i]]
euler.layout = layout_rph
bag.write('ms25_euler', euler, t=timestamp)
finally:
bag.close()
return 0
def write_vel(vel_data, utime, num_hits, bag):
timestamp = microseconds_to_ros_timestamp(utime)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension(), MultiArrayDimension()]
layout.dim[0].label = "hits"
layout.dim[0].size = num_hits
layout.dim[0].stride = 5
layout.dim[1].label = "xyzil"
layout.dim[1].size = 5
layout.dim[1].stride = 1
vel = Float64MultiArray()
vel.data = vel_data
vel.layout = layout
pc2_msg = xyz_array_to_pointcloud2(np.array(vel_data), timestamp,
'velodyne')
bag.write('/velodyne_packet', pc2_msg, t=timestamp)
def write_ms25_euler(ms25_euler, i, bag):
utime = ms25_euler[i, 0]
r = ms25_euler[i, 1]
p = ms25_euler[i, 2]
h = ms25_euler[i, 3]
timestamp = rospy.Time.from_sec(utime/1e6)
layout_rph = MultiArrayLayout()
layout_rph.dim = [MultiArrayDimension()]
layout_rph.dim[0].label = "rph"
layout_rph.dim[0].size = 3
layout_rph.dim[0].stride = 1
euler = Float64MultiArray()
euler.data = [r, p, h]
euler.layout = layout_rph
bag.write('ms25_euler', euler, t=timestamp)
def __init__(self, arg, rot):
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('joint_velocities',
Float32MultiArray,
queue_size=5)
rospy.init_node('joint_velocities', anonymous=True)
self.rate = rospy.Rate(20)
self.wheels = arg
if rot:
r = -1
else:
r = 1
# compose the multiarray message
self.jointVelocities = Float32MultiArray()
myLayout = MultiArrayLayout()
myMultiArrayDimension = MultiArrayDimension()
myMultiArrayDimension.label = "joint_velocities"
myMultiArrayDimension.size = 1
myMultiArrayDimension.stride = numOfWheels
myLayout.dim = [myMultiArrayDimension]
myLayout.data_offset = 0
self.jointVelocities.layout = myLayout
while not rospy.is_shutdown():
# first item is left and second is right
if self.wheels == 2:
self.jointVelocities.data = [1.0, 1.0 * r]
elif self.wheels == 4:
self.jointVelocities.data = [1.0, 1.0, 1.0 * r, 1.0 * r]
self.pub.publish(self.jointVelocities)
self.rate.sleep()
def callback(self, data):
ang = data.angles
f0 = self.fk(data.angles).points.points[4]
Jaco = [0 for j in range(15)]
for i in range(5):
temp = list(ang.angles)
temp[i] = temp[i] + self.delta
f1 = self.fk(JointAngles(temp)).points.points[4]
Jaco[i] = (f1.x - f0.x) / self.delta
Jaco[i + 5] = (f1.y - f0.y) / self.delta
Jaco[i + 10] = (f1.z - f0.z) / self.delta
# These dimensions should stay the same
layout = MultiArrayLayout()
layout.dim = [
MultiArrayDimension('height', 3, 15),
MultiArrayDimension('width', 5, 5)
]
jac = Float64MultiArray(
layout, np.array(Jaco)) # FILL IN: Replace zeros with your result
return jac
def build_atg(self, obs_save_dir=None):
time_elapsed = time.time()
action = None
first_node_found = False
reward_s_a = {}
self.reward_a = 1e-8*np.ones(NUM_ACTIONS)
while not rospy.is_shutdown():
self.rate.sleep()
ros_image = self.get_current_observation().img
print('Done observing!!')
cv_image = self.bridge.imgmsg_to_cv2(ros_image)
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
if NEED_CROP:
cv_image = cv_image[self.c_coord_tl[1]:self.c_coord_br[1], self.c_coord_tl[0]:self.c_coord_br[0]]
if COLLECT_DATA:
if self.observation_count > NUM_DATA_POINTS:
break
if obs_save_dir is not None:
obs_save_dir_full = self.data_folder_path + obs_save_dir + '/dataset/'
if not os.path.exists(obs_save_dir_full):
os.makedirs(obs_save_dir_full)
writing_path = 'obs_' + str(self.observation_count) + '.jpg'
write_loc = os.path.join(obs_save_dir_full, writing_path)
cv2.imwrite(write_loc , cv_image)
action = random_action()
self.action_pub.publish(ACTION_PARAMETER_SPACE[action])
print('Taking action %d'%(action))
self.rate.sleep()
print('Done taking action')
self.observation_count +=1
else:
image = to_var(cv_to_tensor(cv_image, image_size=IMAGE_SIZE))
#recon_loss, recon_loss_norm = get_reconstruction_loss_with_all_ae(image,
# self.autoencoder_mixture,
# loss_fn = torch.nn.functional.mse_loss)
#c_aspect_node = current_aspect_node(recon_loss, self.aspect_count)
recon_ll = get_recon_likelihood_with_all_ae(image, self.autoencoder_mixture)
c_aspect_node_blief = belief_from_recon_ll(recon_ll)
c_aspect_node = current_aspect_node_ll(recon_ll)
print('recon_loss: ', recon_ll)
#plt.bar(np.arange(len(c_aspect_node_blief)), c_aspect_node_blief)
#plt.figure()
#imshow(make_grid(image.cpu().data), True)
if obs_save_dir is not None:
obs_save_dir_full = self.data_folder_path + obs_save_dir + '/' + str(c_aspect_node) + '/'
if not os.path.exists(obs_save_dir_full):
os.makedirs(obs_save_dir_full)
writing_path = 'obs_' + str(self.observation_count) + '.jpg'
write_loc = os.path.join(obs_save_dir_full, writing_path)
cv2.imwrite(write_loc , cv_image)
obs_save_dir_full = self.data_folder_path + obs_save_dir + '/dataset/'
if not os.path.exists(obs_save_dir_full):
os.makedirs(obs_save_dir_full)
writing_path = 'obs_' + str(self.observation_count) + '.jpg'
write_loc = os.path.join(obs_save_dir_full, writing_path)
cv2.imwrite(write_loc , cv_image)
atg_mat_fma = Float64MultiArray()
if np.max(recon_ll) > RECONSTRUCTION_TOLERANCE_LL:
print('[Observation: %d was matched with Aspect node: %d]'%(self.observation_count, c_aspect_node))
#c_aspect_node_blief = belief_from_recon_loss(recon_loss)
self.time_last_aspect_discoverd += 1
if self.time_last_aspect_discoverd > CONVERGENCE_THRESHOLD:
break
else:
self.time_last_aspect_discoverd = 0
print('Observation: %d not matched hence Creating aspect_%d'%(self.observation_count, self.aspect_count))
print()
self.aspect_images.append(tensor_to_ros(image.cpu().data, self.bridge))
self.autoencoder_mixture[self.aspect_count] = {}
self.autoencoder_mixture[self.aspect_count]['autoencoder'] = init_autoencoder()
gen_images = generate_random_versions_of_image(image.cpu().squeeze(0), random_transformer, n_versions=N_VERSIONS)
if VIZ_GEN_IMAGES:
for i in range(len(gen_images)):
imshow(make_grid(gen_images[i]), True)
if USE_ASPECT_IMAGE:
ds = AutoEncoderDataset(gen_images, aspect_image=image)
else:
ds = AutoEncoderDataset(gen_images, aspect_image=None)
optimizer = optim.Adam(self.autoencoder_mixture[self.aspect_count]['autoencoder'].parameters(), lr=1e-3)
criterion = nn.BCELoss()
data_loader = DataLoader(ds, batch_size=4, shuffle=True)
train_autoencoder(self.autoencoder_mixture[self.aspect_count]['autoencoder'],
optimizer,
criterion,
data_loader,
number_of_epochs=NUMBER_OF_EPOCHS,
name='aspect_autoencoder_' + str(self.aspect_count), verbose=VERBOSE)
recon_images = self.autoencoder_mixture[self.aspect_count]['autoencoder'](to_var(gen_images)).cpu().data
self.autoencoder_mixture[self.aspect_count]['stat'] = stat_of_mse_loss(recon_images, gen_images)
#test_image = to_var(gen_images[0].unsqueeze(0))
#test_image_recon = self.autoencoder_mixture[self.aspect_count]['autoencoder'](test_image)
#recon_error = torch.nn.functional.mse_loss(test_image_recon, test_image)
#self.autoencoder_mixture[self.aspect_count]['recon_error'] = recon_error.data.sum()
self.autoencoder_mixture[self.aspect_count]['image'] = image
#recon_loss, recon_loss_norm = get_reconstruction_loss_with_all_ae(image,
# self.autoencoder_mixture,
# loss_fn = torch.nn.functional.mse_loss)
#c_aspect_node_blief = belief_from_recon_loss(recon_loss)
recon_ll = get_recon_likelihood_with_all_ae(image, self.autoencoder_mixture)
c_aspect_node_blief = belief_from_recon_ll(recon_ll)
#rand_img = random_image(self.data_folder_path + obs_save_dir + '/aspect_nodes/')
#if rand_img is not None:
# rand_img_recon = self.autoencoder_mixture[self.aspect_count]['autoencoder'](rand_img)
#imshow(make_grid(rand_img.cpu().data), True)
# fig=plt.figure(figsize=(18, 16), dpi= 100, facecolor='w', edgecolor='k')
# viz = torch.stack([test_image, test_image_recon, rand_img, rand_img_recon]).squeeze(1).cpu().data
# if VIZ_GEN_IMAGES:
# imshow(make_grid(viz), True)
if obs_save_dir is not None:
obs_save_dir_full = self.data_folder_path + obs_save_dir + '/aspect_nodes/'
if not os.path.exists(obs_save_dir_full):
os.makedirs(obs_save_dir_full)
writing_path = 'aspect_' + str(self.aspect_count) + '.jpg'
write_loc = os.path.join(obs_save_dir_full, writing_path)
cv2.imwrite(write_loc , cv_image)
self.aspect_count += 1
if action is not None:
#print(len(self.prev_aspect_node_belief), len(c_aspect_node_blief))
#print('ATG BEFORE: ', self.atg, c_aspect_node_blief)
for s in range(len(self.prev_aspect_node_belief)):
for s_prime in range(len(c_aspect_node_blief)):
atg_entry_key = (s, action, s_prime)
self.atg_mat = atg_dict_to_mat(self.atg, len(c_aspect_node_blief), NUM_ACTIONS)
if first_node_found:
H_t = entropy_from_belief(self.atg_mat[s, action, :])
if atg_entry_key in self.atg.keys():
self.atg[atg_entry_key] += self.prev_aspect_node_belief[s] * c_aspect_node_blief[s_prime]
else:
reward_s_a[(s, action)] = {}
reward_s_a[(s, action)]['queue'] = [1.]
reward_s_a[(s, action)]['mean'] = None
reward_s_a_mat = np.ones((len(self.prev_aspect_node_belief), NUM_ACTIONS))
self.atg[atg_entry_key] = self.prev_aspect_node_belief[s] * c_aspect_node_blief[s_prime]
self.atg_mat = atg_dict_to_mat(self.atg, len(c_aspect_node_blief), NUM_ACTIONS)
if first_node_found:
H_t_1 = entropy_from_belief(self.atg_mat[s, action, :])
delta_H = H_t_1 - H_t
queue, running_mean = update_reward(reward_s_a[(s, action)]['queue'], delta_H)
reward_s_a[(s, action)]['queue'] = queue
reward_s_a[(s, action)]['mean'] = running_mean
reward_s_a_mat = reward_dict_to_mat(reward_s_a, len(self.prev_aspect_node_belief))
#print(s, action, running_mean)
#print(delta_H)
#print('ATG AFTER: ', self.atg)
if not first_node_found:
first_node_found = True
print('Time since new aspect node discoverd: ', self.time_last_aspect_discoverd ,
' Number of Aspect nodes: ', self.aspect_count,
'Time (secs)', time.time() - time_elapsed)
self.reward_a = np.zeros(len(ACTION_PARAMETER_SPACE))
for act in range(len(ACTION_PARAMETER_SPACE)):
#print('reward: ', reward_s_a_mat[:, act])
self.reward_a[act] = (self.prev_aspect_node_belief*abs(reward_s_a_mat[:, act])).sum()
#self.atg_mat = atg_dict_to_mat(self.atg, self.aspect_count, len(ACTION_PARAMETER_SPACE))
#print(self.atg_mat)
atg_mat_layout = MultiArrayLayout()
dim1 = MultiArrayDimension()
dim2 = MultiArrayDimension()
dim3 = MultiArrayDimension()
dim1.label = 's'
dim1.size = self.atg_mat.shape[0]
dim2.label = 'a'
dim2.size = self.atg_mat.shape[1]
dim3.label = 's_prime'
dim3.size = self.atg_mat.shape[2]
dims = [dim1, dim2, dim3]
atg_mat_layout.dim = dims
atg_mat_layout.data_offset = 0
atg_mat_fma.layout = atg_mat_layout
atg_mat_fma.data = self.atg_mat.reshape(1, -1).tolist()[0]
aspect_nodes = AspectNodes()
aspect_nodes.data = self.aspect_images
self.aspect_nodes_pub.publish(aspect_nodes)
self.atg_pub.publish(atg_mat_fma)
self.observation_count += 1
self.prev_aspect_node = c_aspect_node
self.prev_aspect_node_belief = c_aspect_node_blief
if ACTION_SELECTION_MODE =='RANDOM':
action = random_action()
elif ACTION_SELECTION_MODE == 'IM':
action = im_action(self.reward_a)
else:
print('UNKOWN ACTION SELECTION!!')
self.action_pub.publish(ACTION_PARAMETER_SPACE[action])
print('Taking action %d'%(action))
self.rate.sleep()
print('Done taking action')
if not COLLECT_DATA:
print('ATG took %.2f secs to discover %d Aspect Nodes and converge'%(time.time()-time_elapsed, self.aspect_count))
result = {}
result['autoencoder_mixture'] = self.autoencoder_mixture
result['atg'] = self.atg_mat
result['running_time'] = time.time()-time_elapsed
result['n_aspect_nodes'] = self.aspect_count
return result
def pf_callback(self, pose_msg):
car_x, car_y, angle_z = self.ObtainCarState(pose_msg)
car_x = car_x + car_length * math.cos(angle_z)
car_y = car_y + car_length * math.sin(angle_z)
car_x, car_y, x_index, y_index = self.localize(car_x, car_y)
self.PublishCarPoint(car_x, car_y)
car_point = np.asarray([car_x, car_y])
tree = []
root_node = Node(x_index, y_index, None, True)
tree.append(root_node)
goal_points = self.Waypoints_Master
#print(goal_points)
goal_magnitudes = np.asarray(
[LA.norm(goal_point - car_point) for goal_point in goal_points])
goal_index = self.FindGoalIndex(goal_magnitudes, self.goal_lookahead)
goal_x, goal_y = self.FindGoalPoint(goal_index, goal_magnitudes,
goal_points, self.goal_lookahead)
goal_found = False
while goal_found == False:
#rospy.sleep(0.1)
sample_point = self.sample(goal_x, goal_y)
nearest_node = self.nearest(tree, sample_point)
new_node = self.steer_modified(nearest_node, sample_point, tree,
x_index, y_index)
tree_node = Node(new_node[0], new_node[1], nearest_node, False)
collides = self.check_collision(new_node, nearest_node, tree)
if collides == False:
tree.append(tree_node)
goal_found = self.is_goal(tree_node, goal_x, goal_y)
self.PublishTreeArray(tree)
unaligned_waypoints = self.find_path(tree, tree_node)
#print(unaligned_waypoints)
waypoints = []
for i in unaligned_waypoints:
wp_x = self.occ_grid[i[0]][i[1]][1]
wp_y = self.occ_grid[i[0]][i[1]][2]
waypoints.append(wp_x)
waypoints.append(wp_y)
#print(waypoints)
pub_points = Float32MultiArray()
wp_len = len(waypoints)
dim0 = MultiArrayDimension()
dim1 = MultiArrayDimension()
dim0.label = "height"
dim0.size = wp_len / 2
dim1.label = "width"
dim1.size = 2
dimensions = MultiArrayLayout()
dimensions.dim = [dim0, dim1]
pub_points.layout = dimensions
pub_points.data = waypoints
self.pursuit_pub.publish(pub_points)
def main(args):
if len(sys.argv) < 2:
print 'Please specify ms25 file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
ms25 = np.loadtxt(sys.argv[1], delimiter=",")
utimes = ms25[:, 0]
mag_xs = ms25[:, 1]
mag_ys = ms25[:, 2]
mag_zs = ms25[:, 3]
accel_xs = ms25[:, 4]
accel_ys = ms25[:, 5]
accel_zs = ms25[:, 6]
rot_rs = ms25[:, 7]
rot_ps = ms25[:, 8]
rot_hs = ms25[:, 9]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime / 1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "xyz"
layout.dim[0].size = 3
layout.dim[0].stride = 1
mag = Float64MultiArray()
mag.data = [mag_xs[i], mag_ys[i], mag_zs[i]]
mag.layout = layout
accel = Float64MultiArray()
accel.data = [accel_xs[i], accel_ys[i], accel_zs[i]]
accel.layout = layout
layout_rph = MultiArrayLayout()
layout_rph.dim = [MultiArrayDimension()]
layout_rph.dim[0].label = "rph"
layout_rph.dim[0].size = 3
layout_rph.dim[0].stride = 1
rot = Float64MultiArray()
rot.data = [rot_rs[i], rot_ps[i], rot_hs[i]]
rot.layout = layout_rph
bag.write('mag', mag, t=timestamp)
bag.write('accel', accel, t=timestamp)
bag.write('rot', rot, t=timestamp)
finally:
bag.close()
return 0
def main(args):
if len(sys.argv) < 2:
print 'Please specify velodyne hits file and output rosbag file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
f_bin = open(sys.argv[1], "r")
bag = rosbag.Bag(sys.argv[2], 'w')
print 'coverting...'
print 'please wait...'
try:
while True:
magic = f_bin.read(8)
if magic == '': # eof
break
if not verify_magic(magic):
print "Could not verify magic"
num_hits = struct.unpack('<I', f_bin.read(4))[0]
utime = struct.unpack('<Q', f_bin.read(8))[0]
f_bin.read(4) # padding
# Read all hits
data = []
for i in range(num_hits):
x = struct.unpack('<H', f_bin.read(2))[0]
y = struct.unpack('<H', f_bin.read(2))[0]
z = struct.unpack('<H', f_bin.read(2))[0]
i = struct.unpack('B', f_bin.read(1))[0]
l = struct.unpack('B', f_bin.read(1))[0]
x, y, z = convert(x, y, z)
data += [x, y, z, float(i), float(l)]
# Now write out to rosbag
timestamp = rospy.Time.from_sec(utime/1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension(), MultiArrayDimension()]
layout.dim[0].label = "hits"
layout.dim[0].size = num_hits
layout.dim[0].stride = 5
layout.dim[1].label = "xyzil"
layout.dim[1].size = 5
layout.dim[1].stride = 1
vel = Float64MultiArray()
vel.data = data
vel.layout = layout
bag.write('velodyne_packet', vel, t=timestamp)
except Exception:
print 'End of File'
finally:
f_bin.close()
bag.close()
print 'done.'
return 0
scan.range_max = MAX_LASER_RANGE
scan.angle_increment = angle_step
scan.time_increment = .0
scan.ranges = simulated_laser
scan_pub.publish(scan)
#### publish the costmap
dim1 = MultiArrayDimension()
dim1.label = "height"
dim1.size = GRID_HEIGHT
dim1.stride = GRID_HEIGHT * GRID_WIDTH
dim2 = MultiArrayDimension()
dim2.label = "width"
dim2.size = GRID_WIDTH
dim2.stride = GRID_WIDTH
dimentions = [dim1, dim2]
layout = MultiArrayLayout()
layout.dim = dimentions
layout.data_offset = 0
array = Float64MultiArray()
array.layout = layout
array.data = local_costmap.flatten().tolist()
costmap_pub.publish(array)
seq_num += 1
#print simulated_laser
rate.sleep()
# rospy.spin()