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 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 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 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 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 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 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 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__(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 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
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()
