def __init__(self, data_dir):
super(ImageFeeder, self).__init__()
self._data_dir = data_dir
filename = rospy.get_param('intrinsic', 'realsense_intrinsic.json')
with open(data_dir + '/' + filename) as f:
try:
intrinsic = json.loads(f.read())
except Exception as e:
rospy.logerr(str(e))
return
Kt = intrinsic['intrinsic_matrix']
K = [Kt[0], Kt[3], Kt[6], Kt[1], Kt[4], Kt[7], Kt[2], Kt[5], Kt[8]]
self._cinfo = smsg.CameraInfo()
self._cinfo.header.frame_id = rospy.get_param('frame', 'map')
self._cinfo.height = intrinsic['height']
self._cinfo.width = intrinsic['width']
self._cinfo.distortion_model = 'plumb_bob'
self._cinfo.D = [0, 0, 0, 0, 0]
self._cinfo.K = K
self._cinfo.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
self._cinfo.P = K[0:3] + [0] + K[3:6] + [0] + K[6:9] + [0]
self._cinfo.binning_x = 0
self._cinfo.binning_y = 0
self._cinfo_pub = rospy.Publisher('~camera_info',
smsg.CameraInfo,
queue_size=1)
self._image_pub = rospy.Publisher('~image', smsg.Image, queue_size=1)
self._depth_pub = rospy.Publisher('~depth', smsg.Image, queue_size=1)
self._dfilter = DepthFilterClient('depth_filter')
self._dfilter.window_radius = 2
def __init__(
self,
param={
"low_H": 110,
"high_H": 120,
"low_S": 0,
"high_S": 254,
"low_V": 220,
"high_V": 230,
},
):
self.DIGITS_LOOKUP = {
(0, 0, 1, 1, 0, 1, 0): -1,
(0, 0, 0, 1, 0, 0, 0): "-",
(1, 1, 1, 0, 1, 1, 1): 0,
(0, 0, 1, 0, 0, 1, 0): 1,
(1, 0, 1, 1, 1, 0, 1): 2,
(1, 0, 1, 1, 0, 1, 1): 3,
(0, 1, 1, 1, 0, 1, 0): 4,
(1, 1, 0, 1, 0, 1, 1): 5,
(1, 1, 0, 1, 1, 1, 1): 6,
(1, 0, 1, 0, 0, 1, 0): 7,
(1, 1, 1, 1, 1, 1, 1): 8,
(1, 1, 1, 1, 0, 1, 1): 9,
}
self.heading_data = mrs_msgs.Float64Stamped()
self.nav = Position_Info()
self.camera_setting = sensor.CameraInfo()
self.camera_frame = sensor.Image()
self.sub_Camera = rospy.Subscriber(
"/uav1/bluefox_optflow/camera_info",
sensor.CameraInfo,
self.setting_callback,
)
self.sub = rospy.Subscriber("/uav1/bluefox_optflow/image_raw",
sensor.Image, self.camera_callback)
self.sub_hdg = self.sub = rospy.Subscriber(
"/uav1/control_manager/heading",
mrs_msgs.Float64Stamped,
self.heading_callback,
)
while not self.camera_frame.data:
pass
while self.camera_setting.K[0] <= 0:
pass
self.fx = self.camera_setting.K[0]
self.fy = self.camera_setting.K[5]
self.height = self.camera_setting.height
self.width = self.camera_setting.width
self.hfov = 2 * np.arctan2(self.width, 2 * self.fx)
self.vfov = 2 * np.arctan2(self.height, 2 * self.fy)
self.parameters = param
self.kernel = np.ones((5, 5), np.uint8)
self.bridge = CvBridge()
self.img = ""
def __init__(self, data_dir):
super(ImageFeeder, self).__init__()
self._data_dir = data_dir
self._nposes = rospy.get_param('~nposes', 1)
self._timeout = rospy.get_param('~timeout', 30)
# Load camera intrinsics
filename = rospy.get_param('~intrinsic', 'realsense_intrinsic.json')
with open(self._data_dir + '/' + filename) as f:
try:
intrinsic = json.loads(f.read())
except Exception as e:
rospy.logerr('(Feeder) %s', str(e))
Kt = intrinsic['intrinsic_matrix']
K = [Kt[0], Kt[3], Kt[6], Kt[1], Kt[4], Kt[7], Kt[2], Kt[5], Kt[8]]
self._cinfo = smsg.CameraInfo()
self._cinfo.header.frame_id = rospy.get_param('~camera_frame', 'map')
self._cinfo.height = intrinsic['height']
self._cinfo.width = intrinsic['width']
self._cinfo.distortion_model = 'plumb_bob'
self._cinfo.D = [0, 0, 0, 0, 0]
self._cinfo.K = K
self._cinfo.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
self._cinfo.P = K[0:3] + [0] + K[3:6] + [0] + K[6:9] + [0]
self._cinfo.binning_x = 0
self._cinfo.binning_y = 0
self._cinfo_pub = rospy.Publisher('~camera_info',
smsg.CameraInfo, queue_size=1)
self._image_pub = rospy.Publisher('~image', smsg.Image, queue_size=1)
self._depth_pub = rospy.Publisher('~depth', smsg.Image, queue_size=1)
self._dfilter = DepthFilterClient('depth_filter')
self._localizer = LocalizationClient('localization')
self._spawner = ModelSpawnerClient('model_spawner')