def test_outlet_detector(self): rospy.sleep(1.0) p = PointStamped() p.point.x = 1 p.point.x = 0 p.point.x = 0 p.header.frame_id = "base_footprint" p.header.stamp = rospy.get_rostime() pose = self.detect_outlet(p) if len(sys.argv) < 2: self.fail("No ground truth present") if sys.argv[1] == 'fail': self.assertTrue(pose == None) return filename = sys.argv[1] print sys.argv if os.path.exists(filename) and os.path.isfile(filename): if pose == None: #fail test self.assertTrue(False) file = open(filename, "r") gt_pose = PoseStamped() gt_pose.deserialize(file.read()) file.close() self.check_poses(pose, gt_pose) else: if len(sys.argv) >= 3 and sys.argv[2] == 'save': file = open(filename, "w") pose.serialize(file) file.close() # failing test self.assertTrue(False)
def get_recorded_dataset(directories): if isinstance(directories, str): directories = [directories] dataset = [] suffixes = {'sound': '.wav', 'meta': '.txt', 'mic_pose': '.msg', 'speaker_pose': '.msg', 'self_pose': '.msg', 'other_pose': '.msg', 'self_scan': '.msg', 'other_scan': '.msg'} for directory in directories: d_full = os.path.abspath(directory) _dataset = {} for f in os.listdir(d_full): f_full = os.path.join(d_full, f) for prefix, suffix in suffixes.items(): if f.startswith(prefix+'_') and f.endswith(suffix): id = f[len(prefix)+1:-len(suffix)] if id in _dataset: entry = _dataset[id] else: entry = {} _dataset[id] = entry if prefix == 'sound': entry['recorded_sound_file'] = f_full elif prefix == 'meta': with open(f_full, 'r') as _f: entry.update(yaml.load(_f.read())) elif prefix.endswith('pose'): ps = PoseStamped() with open(f_full, 'rb') as _f: ps.deserialize(_f.read()) entry[prefix] = ps elif prefix.endswith('scan'): ls = LaserScan() with open(f_full, 'rb') as _f: ls.deserialize(_f.read()) entry[prefix] = ls print 'Found {} entries in directory "{}"'.format(len(_dataset), d_full) for k, v in _dataset.iteritems(): v['id'] = os.path.basename(directory)+'/'+k dataset.append(v) return dataset
rate, sound = wavfile.read(os.path.join(data_dir, 'sound_'+data_num+'.wav')) sound = sound/32768. channels = sound.shape[1] mapmsg = OccupancyGrid() with open(os.path.join(data_dir, 'map.msg'), 'rb') as f: mapmsg.deserialize(f.read()) lscanmsg = LaserScan() with open(os.path.join(data_dir, 'self_scan_'+data_num+'.msg'), 'rb') as f: lscanmsg.deserialize(f.read()) rscanmsg = LaserScan() with open(os.path.join(data_dir, 'other_scan_'+data_num+'.msg'), 'rb') as f: rscanmsg.deserialize(f.read()) lposemsg = PoseStamped() with open(os.path.join(data_dir, 'self_pose_'+data_num+'.msg'), 'rb') as f: lposemsg.deserialize(f.read()) rposemsg = PoseStamped() with open(os.path.join(data_dir, 'other_pose_'+data_num+'.msg'), 'rb') as f: rposemsg.deserialize(f.read()) with open(os.path.join(data_dir, 'meta_'+data_num+'.txt'), 'rb') as f: metadata = yaml.load(f.read()) _rate, _sound = wavfile.read(metadata['sound_file']) if len(_sound.shape) == 1: _sound = np.expand_dims(_sound, 1) _sound = _sound/32768. matcher = SoundMatcher() begin, end, confidence = matcher.match(sound, _sound) text = '"{}" ({:.2f})'.format(metadata['text'], confidence) loc = SoundSourceLocalizer(channels)
class Agent(object): def __init__(self, controller, a_id, namespace, address, rx_port): self.calibration_path = Path() self.controller = controller self.a_id = a_id self.namespace = namespace self.local_map_name = '%s_map' % (namespace, ) self.local_map_transform = TransformStamped() self.local_map_transform_calculated = False self.heartbeat_time = None self.command_id = 1 self.mavros_state = MavrosState() self.local_pose = PoseStamped() self.global_position = NavSatFix() self.rx_port = rx_port self._create_socket(address) self.recv_q = Queue.Queue() self.recv_thread = threading.Thread(target=self._recv_thread) self.recv_thread.start() self.publishers = {} self.subscribers = {} self._register_publisher() self._register_subscribers() self._create_calibration_path() self.calibration_poses = [] self.command_thread = None self.last_command = (1, True ) # last_command_id, last_command_completed rospy.loginfo("%s: Initialized", self.namespace) def _get_command_id(self): return self.command_id def _increment_and_get_command_id(self): self.command_id += 1 return self._get_command_id() def _create_socket(self, address): self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.socket.bind(("0.0.0.0", self.rx_port)) rospy.loginfo('connecting to %s, bound to %s' % (address, self.rx_port)) self.socket.connect(tuple(address)) def _broadcast_transforms(self): if self.local_map_transform_calculated: self.controller.tf_broadcaster.sendTransform( self.local_map_transform) def _recv_thread(self): while not rospy.is_shutdown(): data = self.socket.recv(1024) reply = messages_pb2.Reply() try: reply.ParseFromString(data) self.recv_q.put(reply) except Exception as e: rospy.logerr(str(e) + ' data: ' + str(self.data)) return def _process_reply(self, reply): self.last_command = (reply.heartbeat_data.last_command_id, reply.heartbeat_data.last_command_completed) rospy.loginfo(self.last_command) self.mavros_state.deserialize(reply.heartbeat_data.mavros_state) self.local_pose.deserialize(reply.heartbeat_data.local_pose) self.global_position.deserialize(reply.heartbeat_data.gps_nav_sat) self.publishers['mavros_state'].publish(self.mavros_state) self.publishers['local_pose'].publish(self.local_pose) self.publishers['global_position'].publish(self.global_position) if self.local_map_transform_calculated: transformed_pose = self.local_pose_to_global_pose(self.local_pose) self.publishers['global_pose'].publish(transformed_pose) def _send_heartbeat(self): now = rospy.get_rostime() if self.heartbeat_time is not None and now - self.heartbeat_time < rospy.Duration( 1): return request = messages_pb2.Request() request.timestamp = int(now.to_sec()) request.command = messages_pb2.Command.HEARTBEAT self.socket.send(request.SerializeToString()) self.heartbeat_time = now def _register_publisher(self): """ commander_state_topic = '/pegasus_ros/%s/state/commander' % (self.namespace,) rospy.loginfo('Publisher to commander_state_topic: %s' % (commander_state_topic,)) self.publishers['commander_state'] = rospy.Publisher(commander_state_topic, UInt8, queue_size=1000) """ mavros_state_topic = '/pegasus/%s/state/mavros' % (self.namespace, ) rospy.loginfo('Publisher to mavros_state_topic: %s' % (mavros_state_topic, )) self.publishers['mavros_state'] = rospy.Publisher(mavros_state_topic, MavrosState, queue_size=1000) local_pose_topic = '/pegasus/%s/local_position/pose' % ( self.namespace, ) rospy.loginfo('Publisher to local_pose_topic: %s' % (local_pose_topic, )) self.publishers['local_pose'] = rospy.Publisher(local_pose_topic, PoseStamped, queue_size=1000) global_position_topic = '/pegasus/%s/global_position/global' % ( self.namespace, ) rospy.loginfo('Publisher to global_position_topic: %s' % (global_position_topic, )) self.publishers['global_position'] = rospy.Publisher( global_position_topic, NavSatFix, queue_size=1000) global_pose_topic = '/pegasus/%s/global_position/pose' % ( self.namespace, ) rospy.loginfo('Publisher to global_pose_topic: %s' % (global_pose_topic, )) self.publishers['global_pose'] = rospy.Publisher(global_pose_topic, PoseStamped, queue_size=1000) calibration_path_topic = '/pegasus/%s/path/calibration' % ( self.namespace, ) rospy.loginfo('Publisher to calibration path: %s' % (calibration_path_topic, )) self.publishers['calibration_path'] = rospy.Publisher( calibration_path_topic, Path, latch=True, queue_size=1000) def _register_subscribers(self): path_topic = '/pegasus/%s/path' % (self.namespace, ) rospy.loginfo('Subscriber to path topic: %s' % (path_topic, )) self.subscribers['path'] = rospy.Subscriber(path_topic, Path, self._recv_path) def _recv_path(self, path): self.path = path def _create_calibration_path(self): side_length = self.controller.params['grid_size'] z_height = self.controller.params['z_height'] box_points = ((0., 0.), (side_length, 0.), (side_length, side_length), (0., side_length), (0., 0.)) # box_points = ((0, 0), (sideLength, 0)) num_points = len(box_points) v1 = np.array(box_points) v2 = np.zeros((num_points, 2)) v2[1:] = v1[0:-1] v1sv2 = v1 - v2 direction = np.arctan2(v1sv2[:, 1], v1sv2[:, 0]) for k in range(num_points): yaw = direction[k] q = quaternion_from_euler(0, 0, yaw) pose = PoseStamped() pose.header.frame_id = 'map' pose.pose.position.x = box_points[k][0] pose.pose.position.y = box_points[k][1] pose.pose.position.z = z_height + (self.a_id * 2 ) # 2 meters distance pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.calibration_path.header.frame_id = 'map' self.calibration_path.poses.append(pose) self.publishers['calibration_path'].publish(self.calibration_path) def _run_command_thread(self, command_id, command, param): request = None if command == messages_pb2.Command.SET_OFFBOARD: request = _set_offboard() elif command == messages_pb2.Command.SET_ARM: request = _set_arm() elif command == messages_pb2.Command.SET_RETURN_TO_HOME: request = _set_return_to_home() elif command == messages_pb2.Command.GOTO: request = _goto(param) else: return request.id = command_id request.timestamp = int(rospy.get_rostime().to_sec()) # Busy wait for completion/retry rate = rospy.Rate(20) retry_counter = 0 while not rospy.is_shutdown(): if retry_counter == 0: self.socket.send(request.SerializeToString()) retry_counter = (retry_counter + 1) % 100 # 5 seconds? if self.last_command[1] == True and self.last_command[ 0] == command_id: return rate.sleep() def run_command(self, command, param=()): command_id = self._increment_and_get_command_id() self.command_thread = threading.Thread(target=self._run_command_thread, args=(command_id, command, param)) self.command_thread.start() def wait_for_command(self): if self.command_thread is not None: self.command_thread.join() def capture_calibration_pose(self): utm_pose = utm.fromLatLong(self.global_position.latitude, self.global_position.longitude, self.global_position.altitude) pose = ((self.local_pose.pose.position.x, self.local_pose.pose.position.y), (utm_pose.toPoint().x, utm_pose.toPoint().y)) rospy.loginfo(pose) self.calibration_poses.append(pose) def generate_transforms(self): size = len(self.calibration_poses) local_points = np.empty((size, 2)) global_points = np.empty((size, 2)) for i, correspondence_pts in enumerate(self.calibration_poses): local_points[i] = correspondence_pts[0] global_points[i] = correspondence_pts[1] map_points = np.subtract(global_points, self.controller.map_origin_point) rospy.loginfo(local_points) rospy.loginfo(map_points) h, mask = cv2.findHomography(local_points, map_points, method=cv2.RANSAC) homography = np.eye(4) homography[0:2, 0:2] = h[0:2, 0:2] homography[0:2, 3] = h[0:2, 2] homography[3, 0:2] = h[2, 0:2] homography[3, 3] = h[2, 2] transform_q = quaternion_from_matrix(homography) transform_t = translation_from_matrix(homography) self.local_map_transform.header.stamp = rospy.Time.now() self.local_map_transform.header.frame_id = self.controller.global_map_name self.local_map_transform.child_frame_id = self.local_map_name self.local_map_transform.transform.rotation.x = transform_q[0] self.local_map_transform.transform.rotation.y = transform_q[1] self.local_map_transform.transform.rotation.z = transform_q[2] self.local_map_transform.transform.rotation.w = transform_q[3] self.local_map_transform.transform.translation.x = transform_t[0] self.local_map_transform.transform.translation.y = transform_t[1] self.local_map_transform.transform.translation.z = 0 rospy.loginfo(transform_q) rospy.loginfo(transform_t) self.local_map_transform_calculated = True def global_pose_to_local_pose(self, global_pose): trans = self.controller.tf_buffer.lookup_transform( self.local_map_name, # target self.controller.global_map_name, # source rospy.Time(), rospy.Duration(10.0)) transformed_pose = tf2_geometry_msgs.do_transform_pose( global_pose, trans) transformed_pose.header.frame_id = self.local_map_name return transformed_pose def local_pose_to_global_pose(self, local_pose): trans = self.controller.tf_buffer.lookup_transform( self.controller.global_map_name, # target self.local_map_name, # source rospy.Time(), rospy.Duration(10.0)) transformed_pose = tf2_geometry_msgs.do_transform_pose( local_pose, trans) transformed_pose.header.frame_id = self.controller.global_map_name return transformed_pose def spin(self): self._send_heartbeat() self._broadcast_transforms() try: reply = self.recv_q.get_nowait() self._process_reply(reply) except Queue.Empty: pass
elif role == 'SAVE': sound = buf.stop() msgs = {} msgs['self_scan'] = rospy.wait_for_message('/hsrb/base_scan', LaserScan) msgs['other_scan'] = rospy.wait_for_message(prefix + '/hsrb/base_scan', LaserScan) mic_tf = tf_buffer.lookup_transform('map', 'head_rgbd_sensor_link', rospy.Time(0)) msgs['mic_pose'] = tf2pose(mic_tf) base_tf = tf_buffer.lookup_transform('map', 'base_range_sensor_link', rospy.Time(0)) msgs['self_pose'] = tf2pose(base_tf) speaker_pose = PoseStamped() speaker_pose.deserialize(data.pop('speaker_pose')) msgs['speaker_pose'] = speaker_pose other_pose = PoseStamped() other_pose.deserialize(data.pop('base_pose')) msgs['other_pose'] = other_pose filename = os.path.join( save_dir, 'sound_{:04d}_{:04d}.wav'.format(iteration, phase)) wavfile.write(filename, sample_rate, sound) for k, v in msgs.iteritems(): filename = os.path.join( save_dir, '{}_{:04d}_{:04d}.msg'.format(k, iteration, phase)) with open(filename, 'wb') as f: v.serialize(f) filename = os.path.join( save_dir, 'meta_{:04d}_{:04}.txt'.format(iteration, phase))