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