def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time):
print(f"Exporting camera {camera}")
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple([int(x) for x in util[f'S_rect_{camera_pad}']])
calib.distortion_model = 'plumb_bob'
calib.k = util[f'K_{camera_pad}']
calib.r = util[f'R_rect_{camera_pad}']
calib.d = [float(x) for x in util[f'D_{camera_pad}']]
calib.p = util[f'P_rect_{camera_pad}']
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path, f'image_{camera_pad}')
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.p = util[f'P{camera_pad}']
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg()
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = Time(seconds=dt).to_msg()
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr")
bag.create_topic(camera_info_topic)
camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr")
bag.create_topic(camera_topic)
bag.write((topic_name + topic_ext, serialize_message(image_message),
timestamp_to_nanoseconds_from_epoch(image_message.header.stamp)))
bag.write((topic_name + '/camera_info', serialize_message(calib),
timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera,
camera_frame_id, topic, initial_time):
print("Exporting camera {}".format(camera))
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path,
'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(
lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'),
f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple(
util['S_rect_{}'.format(camera_pad)].tolist())
calib.distortion_model = 'plumb_bob'
calib.K = util['K_{}'.format(camera_pad)]
calib.R = util['R_rect_{}'.format(camera_pad)]
calib.D = util['D_{}'.format(camera_pad)]
calib.P = util['P_rect_{}'.format(camera_pad)]
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path,
'image_{}'.format(camera_pad))
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(),
kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.P = util['P{}'.format(camera_pad)]
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = rospy.Time.from_sec(
float(datetime.strftime(dt, "%s.%f")))
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = rospy.Time.from_sec(dt)
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
bag.write(topic + topic_ext,
image_message,
t=image_message.header.stamp)
bag.write(topic + '/camera_info', calib, t=calib.header.stamp)
def __callback_get_calibration_object(self, _):
if not self.__calibration_object.is_set():
return GetCalibrationCamResponse(False, CameraInfo())
msg_camera_info = CameraInfo()
mtx, dist = self.__calibration_object.get_camera_info()
msg_camera_info.K = list(mtx.flatten())
msg_camera_info.D = list(dist.flatten())
return GetCalibrationCamResponse(True, msg_camera_info)
def __init__(self, pub):
self.depth1_subscriber = message_filters.Subscriber('/camera1/aligned_depth_to_color/image_raw', Image)
self.depth2_subscriber = message_filters.Subscriber('/camera2/aligned_depth_to_color/image_raw', Image)
self.detection_subscriber =message_filters.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes)
self.camera1_param_subscriber = rospy.Subscriber("/camera1/color/camera_info", CameraInfo, self.camera1_parameter_callback)
self.camera2_param_subscriber = rospy.Subscriber("/camera2/color/camera_info", CameraInfo, self.camera2_parameter_callback)
# messageの型を作成
self.pub = pub
self.camera1_parameter = CameraInfo()
self.camera2_parameter = CameraInfo()
ts = message_filters.ApproximateTimeSynchronizer([self.depth1_subscriber, self.depth2_subscriber, self.detection_subscriber], 10, 0.05)
ts.registerCallback(self.bounding_boxes_callback)
def __init__(self):
self.bridge = CvBridge()
self.PhotoCount_rgb = 0
self.PhotoCount_depth = 0
self.PhotoCount_depth_registered = 0
self.PhotoCount_depth_data = 0
self.CaptureTrigger_rgb = False
self.CaptureTrigger_depth = False
self.CaptureTrigger_depth_registered = False
self.CaptureTrigger_depth_data = False
self.Camera_info_rgb_Trigger = False
self.Camera_info_depth_Trigger = False
self.camera_information_rgb = CameraInfo()
self.camera_information_depth = CameraInfo()
def __init__(self):
# Optimization in OpenCV
cv2.useOptimized()
# Initialize cv bridge
self.bridge = CvBridge()
# Subcribers
self.sub_1_ = message_filters.Subscriber("/band/1/image_raw", Image)
self.sub_2_ = message_filters.Subscriber(
"/kinect2/qhd/image_color_rect", Image)
self.sub_1_i = message_filters.Subscriber("/band/camera_info",
CameraInfo)
self.sub_2_i = message_filters.Subscriber("/kinect2/qhd/camera_info",
CameraInfo)
# Publishers
self.pub_1_ = rospy.Publisher("/stereo/right/image_raw",
Image,
queue_size=1)
self.pub_2_ = rospy.Publisher("/stereo/left/image_raw",
Image,
queue_size=1)
self.pub_1_i = rospy.Publisher("/stereo/right/camera_info",
CameraInfo,
queue_size=1)
self.pub_2_i = rospy.Publisher("/stereo/left/camera_info",
CameraInfo,
queue_size=1)
# Synchronizer
ts = message_filters.ApproximateTimeSynchronizer(
[self.sub_1_, self.sub_2_, self.sub_1_i, self.sub_2_i],
10,
0.1,
allow_headerless=True)
ts.registerCallback(self.callback)
# Display info
rospy.loginfo("Synchronizer node (python) started")
# Init camera info
self.M_CAMERA_INFO = CameraInfo()
self.M_CAMERA_INFO.header.seq = 0
self.M_CAMERA_INFO.header.frame_id = self.FRAME_ID
self.M_CAMERA_INFO.width = self.BAND_WIDTH
self.M_CAMERA_INFO.height = self.BAND_HEIGHT
self.M_CAMERA_INFO.distortion_model = self.D_MODEL
self.K_CAMERA_INFO = CameraInfo()
self.K_CAMERA_INFO.header.frame_id = self.FRAME_ID
self.K_CAMERA_INFO.header.seq = 0
self.K_CAMERA_INFO.width = self.BAND_WIDTH
self.K_CAMERA_INFO.height = self.BAND_HEIGHT
self.K_CAMERA_INFO.distortion_model = self.D_MODEL
def run(self):
# left_cam_info = self.yaml_to_camera_info(self.left_yaml_file)
# right_cam_info = self.yaml_to_camera_info(self.right_yaml_file)
left_cam_info = CameraInfo()
left_cam_info.width = 640
left_cam_info.height = 480
left_cam_info.K = [742.858255, 0.000000, 342.003337, 0.000000, 752.915532, 205.211518, 0.000000, 0.000000, 1.000000]
left_cam_info.D = [0.098441, 0.046414, -0.039316, 0.011202, 0.000000]
left_cam_info.R = [0.983638, 0.047847, -0.173686, -0.048987, 0.998797, -0.002280, 0.173368, 0.010751, 0.984798]
left_cam_info.P = [905.027641, 0.000000, 500.770557, 0.000000, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
left_cam_info.distortion_model = 'plumb_bob'
right_cam_info = CameraInfo()
right_cam_info.width = 640
right_cam_info.height = 480
right_cam_info.K = [747.026840, 0.000000, 356.331849, 0.000000, 757.336986, 191.248883, 0.000000, 0.000000, 1.000000]
right_cam_info.D = [0.127580, -0.050428, -0.035857, 0.018986, 0.000000]
right_cam_info.R = [0.987138, 0.047749, -0.152571, -0.046746, 0.998855, 0.010153, 0.152881, -0.002890, 0.988240]
right_cam_info.P = [905.027641, 0.000000, 500.770557, -42.839515, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
right_cam_info.distortion_model = 'plumb_bob'
rate = rospy.Rate(30)
while not rospy.is_shutdown():
ret,frame=self.cam.read()
if not ret:
print('[ERROR]: frame error')
break
expand_frame=cv2.resize(frame,None,fx=2,fy=1)
left_image = expand_frame[0:480,0:640]
right_image = expand_frame[0:480,640:1280]
self.msg_header.frame_id = 'stereo_image'
self.msg_header.stamp = rospy.Time.now()
left_cam_info.header = self.msg_header
right_cam_info.header = self.msg_header
self.left_image_info_pub.publish(left_cam_info)
self.right_image_info_pub.publish(right_cam_info)
# self.pub_image(self.left_image_pub, left_image, self.msg_header )
# self.pub_image(self.right_image_pub, right_image, self.msg_header )
try:
thread.start_new_thread( self.pub_image, (self.left_image_pub, left_image, self.msg_header, ))
thread.start_new_thread( self.pub_image, (self.right_image_pub, right_image, self.msg_header, ))
except:
print("[ERROR]: unable to start thread")
rate.sleep()
def __init__(self): self.bridge = CvBridge() #Direction_Counter self.DirectionCount = 1 # self.CaptureTrigger_rgb = False self.CaptureTrigger_depth = False self.CaptureTrigger_depth_registered =False self.CaptureTrigger_depth_data = False #info_Trigger self.Camera_info_rgb_Trigger = False self.Camera_info_depth_Trigger = False #camerainfo self.camera_information_rgb = CameraInfo() self.camera_information_depth = CameraInfo()
def save_camera_data(bag, kitti, camera, timestamps):
print("Exporting camera {}".format(camera.nr))
camera_info = CameraInfo()
camera_info.header.frame_id = rectified_camera_frame_id
camera_info.K = list(
getattr(kitti.calib, 'K_cam{}'.format(camera.nr)).flat)
camera_info.P = list(
getattr(kitti.calib, 'P_rect_{}0'.format(camera.nr)).flat)
# We do not include the D and R parameters from the calibration data since the images are
# already undistorted and rectified to the camera #0 frame.
camera_info.R = list(np.eye(3).flat)
cv_bridge = CvBridge()
image_paths = getattr(kitti, 'cam{}_files'.format(camera.nr))
for timestamp, image_path in tqdm(list(zip(timestamps, image_paths))):
cv_image = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
camera_info.height, camera_info.width = cv_image.shape[:2]
encoding = 'bgr8' if camera.is_rgb else 'mono8'
image_message = cv_bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = rectified_camera_frame_id
t = to_rostime(timestamp)
image_message.header.stamp = t
camera_info.header.stamp = t
# Follow the naming conventions from
# http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html
image_topic_ext = '/image_rect_color' if camera.is_rgb else '/image_rect'
bag.write(camera.topic_id + image_topic_ext, image_message, t=t)
bag.write(camera.topic_id + '/camera_info', camera_info, t=t)
def callback_step(data):
global cmdx, cmdy, bridge
obs, _, _, _ = env.step([cmdx, cmdy])
rgb = obs['rgb_filled']
depth = obs['depth'].astype(np.float32)
depth[depth > 10] = 10
#depth[depth < 0.45] = np.nan
rgb_image_message = bridge.cv2_to_imgmsg(rgb, encoding='rgb8')
depth_raw_image = (obs["depth"] * 1000).astype(np.uint16)
depth_raw_message = bridge.cv2_to_imgmsg(depth_raw_image, encoding='passthrough')
depth_message = bridge.cv2_to_imgmsg(depth, encoding='passthrough')
now = rospy.Time.now()
rgb_image_message.header.stamp = now
depth_message.header.stamp = now
depth_raw_message.header.stamp = now
rgb_image_message.header.frame_id='camera_depth_optical_frame'
depth_message.header.frame_id='camera_depth_optical_frame'
depth_raw_message.header.frame_id='camera_depth_optical_frame'
image_pub.publish(rgb_image_message)
depth_pub.publish(depth_message)
depth_raw_pub.publish(depth_raw_message)
msg = CameraInfo(height=resolution, width=resolution, distortion_model="plumb_bob", D=[0.0, 0.0, 0.0, 0.0, 0.0],
K=[128, 0.0, 128.5, 0.0, 128, 128.5, 0.0, 0.0, 1.0],
R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
P=[128, 0.0, 128.5, -0.0, 0.0, 128, 128.5, 0.0, 0.0, 0.0, 1.0, 0.0])
msg.header.stamp = now
msg.header.frame_id="camera_depth_optical_frame"
camera_info_pub.publish(msg)
# Publish semantic image if it is available
if has_semantics:
semantics = obs['semantics']
semantic_image_message = bridge.cv2_to_imgmsg(semantics.astype(np.uint8), encoding='rgb8')
semantic_image_message.header.stamp = now
semantic_image_message.header.frame_id='camera_depth_optical_frame'
semantics_pub.publish(semantic_image_message)
# Odometry
odom = env.get_odom()
br.sendTransform((odom[0][0], odom[0][1], 0),
tf.transformations.quaternion_from_euler(0, 0, odom[-1][-1]),
rospy.Time.now(),
'base_footprint',
'odom')
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'odom'
odom_msg.child_frame_id = 'base_footprint'
odom_msg.pose.pose.position.x = odom[0][0]
odom_msg.pose.pose.position.y = odom[0][1]
odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z, \
odom_msg.pose.pose.orientation.w = tf.transformations.quaternion_from_euler(0, 0, odom[-1][-1])
odom_msg.twist.twist.linear.x = (cmdx + cmdy) * 5
odom_msg.twist.twist.angular.z = (cmdy - cmdx) * 25
odom_pub.publish(odom_msg)
def get_camera_info(camera_info, camera):
with open(camera_info, 'r') as stream:
try:
data = yaml.load(stream)
camera_info = CameraInfo()
T=[0,0,0]
camera_info.width = data[camera]['resolution'][0]
camera_info.height = data[camera]['resolution'][1]
if data[camera]['distortion_model'] == "radtan":
camera_info.distortion_model = "plumb_bob"
else:
camera_info.distortion_model = data[camera]['distortion_model']
fx,fy,cx,cy = data[camera]['intrinsics']
camera_info.K[0:3] = [fx, 0, cx]
camera_info.K[3:6] = [0, fy, cy]
camera_info.K[6:9] = [0, 0, 1]
k1,k2,t1,t2 = data[camera]['distortion_coeffs']
camera_info.D = [k1,k2,t1,t2,0]
#if cam0 then it's left camera, so R = identity and T = [0 0 0]
if camera == "cam0":
camera_info.R[0:3] = [1, 0, 0]
camera_info.R[3:6] = [0, 1, 0]
camera_info.R[6:9] = [0, 0, 1]
else:
camera_info.R[0:3] = data[camera]['T_cn_cnm1'][0][:3]
camera_info.R[3:6] = data[camera]['T_cn_cnm1'][1][:3]
camera_info.R[6:9] = data[camera]['T_cn_cnm1'][2][:3]
T[0:3] = [data[camera]['T_cn_cnm1'][0][3], data[camera]['T_cn_cnm1'][1][3], data[camera]['T_cn_cnm1'][2][3]]
except yaml.YAMLError as exc:
print(exc)
return camera_info, T
def info_480p():
data = CameraInfo()
data.height = 480
data.width = 640
data.distortion_model = "plumb_bob"
data.D = [-0.297117, 0.070173, -0.000390, 0.005232, 0.000000]
data.K = [
342.399061, 0.000000, 284.222700, 0.000000, 343.988302, 227.555237,
0.000000, 0.000000, 1.000000
]
data.R = [
1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000,
0.000000, 1.000000
]
data.P = [
247.555710, 0.000000, 290.282918, 0.000000, 0.000000, 281.358612,
220.776009, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000
]
data.binning_x = 0
data.binning_y = 0
data.roi.x_offset = 0
data.roi.y_offset = 0
data.roi.height = 0
data.roi.width = 0
data.roi.do_rectify = False
return data
def load_camera_info(self):
'''Load camera intrinsics'''
filename = (sys.path[0] + "/calibrations/camera_intrinsic/" +
self.robot_name + ".yaml")
if not os.path.isfile(filename):
logger.warn(
"no intrinsic calibration parameters for {}, trying default".
format(self.robot_name))
filename = (sys.path[0] +
"/calibrations/camera_intrinsic/default.yaml")
if not os.path.isfile(filename):
logger.error("can't find default either, something's wrong")
calib_data = yaml_load_file(filename)
# logger.info(yaml_dump(calib_data))
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape(
(3, 3))
cam_info.D = np.array(
calib_data['distortion_coefficients']['data']).reshape((1, 5))
cam_info.R = np.array(
calib_data['rectification_matrix']['data']).reshape((3, 3))
cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape(
(3, 4))
cam_info.distortion_model = calib_data['distortion_model']
logger.info(
"Loaded camera calibration parameters for {} from {}".format(
self.robot_name, os.path.basename(filename)))
return cam_info
def get_camera_info(self, yaml_fname):
"""
Parse a yaml file containing camera calibration data
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# Load data from file
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["intrinsics"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
camera_info_msg.header.frame_id = calib_data["frame_id"]
return camera_info_msg
def __init__(self):
# focal length
self.focalLength = 937.8194580078125
# bridge object to convert cv2 to ros and ros to cv2
self.bridge = CvBridge()
# timer var
self.start = time.time()
# create a camera node
rospy.init_node('node_camera_mission', anonymous=True)
# controllers
self.linear_vel_control = Controller(5, -5, 0.01, 0, 0)
self.angular_vel_control = Controller(5, -5, 0.01, 0, 0)
# odometry topic subscription
rospy.Subscriber('/odometry/filtered', Odometry, self.callback_odometry)
# image publisher object
self.image_pub = rospy.Publisher('camera/mission', Image, queue_size=10)
# cmd_vel publisher object
self.velocity_publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# get camera info
rospy.Subscriber("/camera_work/camera_info", CameraInfo, self.callback_camera_info)
# move to goal
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)
self.msg_move_to_goal = PoseStamped()
self.flag = True
self.camera_info = CameraInfo()
self.start_map = rospy.Publisher("/GetFirstMap/goal", GetFirstMapActionGoal, queue_size=1)
self.start_explore = rospy.Publisher("/Explore/goal", ExploreActionGoal, queue_size = 1)
self.cancel_map = rospy.Publisher("/GetFirstMap/cancel", GoalID, queue_size = 1)
self.cancel_explore = rospy.Publisher("/Explore/cancel", GoalID, queue_size = 1)
time.sleep(1)
self.start_map.publish()
time.sleep(20)
self.cancel_map.publish()
time.sleep(2)
self.start_explore.publish()
def pixel_1_callback(data):
global br
global pub1_raw
global pub1_info
if (data.data != []):
# From compressed to raw
rospy.loginfo("Image received from pixel_1")
img = br.compressed_imgmsg_to_cv2(data, desired_encoding="bgr8")
# time = str(data.header.stamp.secs) + ("00000000" if data.header.stamp.nsecs == 0 else str(data.header.stamp.nsecs))
# cv2.imwrite("/home/soteris-group/phone_test/pixel_1/" + time + ".jpeg", img)
msg = br.cv2_to_imgmsg(img)
msg.header.stamp.secs = data.header.stamp.secs
msg.header.stamp.nsecs = data.header.stamp.nsecs
msg.encoding = "bgr8"
pub1_raw.publish(msg)
# Camera info
pixel_1_info = CameraInfo()
pixel_1_info.P = [
493.7242431641, 0.0000000000, 322.0943908691, 0.0, 0.0000000000,
96.9177246094, 231.7220153809, 0.0, 0.0, 0.0, 1.0, 0.0
]
pub1_info.publish(pixel_1_info)
else:
rospy.loginfo("Something went wrong")
def publishimage(pub, value):
msg = bridge.cv2_to_imgmsg(value.load(), 'mono8')
msg.header = createheader(value)
pub.img.publish(msg)
publishtf(value, 'cam1')
tf = value >> "cam1"
K = value.K
R = tf[0:3, 0:3]
P = K.dot(tf[0:3, :])
name = distmodel
dist = value.distortion(distmodel)
if name == 'radtan':
name = 'plumb_bob'
dist = list(dist)
dist.append(0) # use k3 = 0
elif name == 'fov':
dist = [dist]
msg = CameraInfo(width=res.width,
height=res.height,
distortion_model=name,
D=dist,
K=K.flatten().tolist(),
R=R.flatten().tolist(),
P=P.flatten().tolist())
msg.header = createheader(value)
pub.cam.publish(msg)
pub.exp.publish(Float32(data=value.exposure))
def publish(self, event):
if self.imgmsg is None:
return
now = rospy.Time.now()
# setup ros message and publish
with self.lock:
self.imgmsg.header.stamp = now
self.imgmsg.header.frame_id = self.frame_id
self.pub.publish(self.imgmsg)
if self.publish_info:
info = CameraInfo()
info.header.stamp = now
info.header.frame_id = self.frame_id
info.width = self.imgmsg.width
info.height = self.imgmsg.height
if self.fovx is not None and self.fovy is not None:
fx = self.imgmsg.width / 2.0 / \
np.tan(np.deg2rad(self.fovx / 2.0))
fy = self.imgmsg.height / 2.0 / \
np.tan(np.deg2rad(self.fovy / 2.0))
cx = self.imgmsg.width / 2.0
cy = self.imgmsg.height / 2.0
info.K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1.0])
info.P = np.array([fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0])
info.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
self.pub_info.publish(info)
def rosmsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
"""
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = self._height
msg.width = self._width
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [
self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0
]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0,
0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def _build_camera_info(self):
"""
Private function to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = self.get_msg_header()
camera_info.width = int(self.carla_actor.attributes['image_size_x'])
camera_info.height = int(self.carla_actor.attributes['image_size_y'])
camera_info.distortion_model = 'plumb_bob'
cx = camera_info.width / 2.0
cy = camera_info.height / 2.0
fx = camera_info.width / (
2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0))
fy = fx
if ROS_VERSION == 1:
camera_info.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
camera_info.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
elif ROS_VERSION == 2:
# pylint: disable=assigning-non-slot
camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
self._camera_info = camera_info
def __init__(self): print 'status init' self.depth = PointCloud() self.camera = Image() self.camera_info = CameraInfo() self.map = OccupancyGrid() self.pose = PoseWithCovarianceStamped()
def __init__(self):
self.image = np.zeros([480, 640])
self.depth = np.zeros([480, 640])
self.depth_output = np.zeros([480, 640])
self.camera_info_timestamp = 0
self.camera_info_dump = CameraInfo()
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.depth_aligned_pub = rospy.Publisher("/cutter_node_align_depth",
Image,
queue_size=10)
self.camera_info_pub = rospy.Publisher("/cutter_node_camera_info",
CameraInfo,
queue_size=1)
# cv bridge
self.cv_bridge = CvBridge()
# subscribed Topic
self.image_subscriber = rospy.Subscriber("/grabcut",
Image,
self.callback_image,
queue_size=1)
self.depth_subscriber = rospy.Subscriber("/align_depth",
Image,
self.callback_depth,
queue_size=1)
self.camera_info_subscriber = rospy.Subscriber("/camera_info",
CameraInfo,
self.callback_info,
queue_size=1)
def info_720p():
data = CameraInfo()
data.height = 720
data.width = 1280
data.distortion_model = "plumb_bob"
data.D = [-0.266169, 0.056566, 0.003569, 0.002922, 0.000000]
data.K = [
550.515474, 0.000000, 560.486530, 0.000000, 550.947091, 334.377088,
0.000000, 0.000000, 1.000000
]
data.R = [
1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000,
0.000000, 1.000000
]
data.P = [
395.931458, 0.000000, 570.158643, 0.000000, 0.000000, 474.049377,
329.846866, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000
]
data.binning_x = 0
data.binning_y = 0
data.roi.x_offset = 0
data.roi.y_offset = 0
data.roi.height = 0
data.roi.width = 0
data.roi.do_rectify = False
return data
def __init__(self):
rospy.init_node("pub_camera_info")
self.if_pub_angle = True
self.info_sub = rospy.Subscriber(
"/pepper_robot/naoqi_driver/camera/depth/camera_info", CameraInfo,
self.cmd_callback)
self.image_sub = rospy.Subscriber(
"/pepper_robot/naoqi_driver/camera/depth/image_raw", Image,
self.image_callback)
self.info_pub = rospy.Publisher("/depth/camera_info",
CameraInfo,
queue_size=15)
self.image_pub = rospy.Publisher("/depth/image_raw",
Image,
queue_size=15)
self.Camera_info = CameraInfo()
self.Image = Image()
self.if_pub_info = False
self.if_pub_image = False
while not rospy.is_shutdown():
if self.if_pub_info:
self.info_pub.publish(self.Camera_info)
if self.if_pub_image:
self.image_pub.publish(self.Image)
rospy.sleep(.3)
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info',
SetCameraInfo,
self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(
self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def camera_info(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def yaml_to_CameraInfo(yaml_fname):
"""
Parse a yaml file containing camera calibration data (as produced by
rosrun camera_calibration cameracalibrator.py) into a
sensor_msgs/CameraInfo msg.
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# Load data from file
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
def publishing(pub_image, pub_camera, image, type_of_camera):
if type_of_camera is 1:
image.convert(carla.ColorConverter.Depth)
elif type_of_camera is 2:
image.convert(carla.ColorConverter.CityScapesPalette)
array = np.frombuffer(image.raw_data, dtype=np.uint8)
array = np.reshape(array, (image.height, image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
img = Image()
infomsg = CameraInfo()
img.header.stamp = rospy.Time.now()
img.header.frame_id = 'base'
img.height = infomsg.height = image.height
img.width = infomsg.width = image.width
img.encoding = "rgb8"
img.step = img.width * 3 * 1
st1 = array.tostring()
img.data = st1
cx = infomsg.width / 2.0
cy = infomsg.height / 2.0
fx = fy = infomsg.width / (2.0 * math.tan(image.fov * math.pi / 360.0))
infomsg.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
infomsg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]
infomsg.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
infomsg.D = [0, 0, 0, 0, 0]
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = "plumb_bob"
infomsg.header = img.header
pub_image.publish(img)
pub_camera.publish(infomsg)
def publishloopCallback(self, event):
# fpv_cam的参数
msg = CameraInfo()
msg.K = [277.191356, 0.0, 320.5, 0.0, 277.191356, 240.5, 0.0, 0.0, 1.0]
msg.height = 480
msg.width = 640
self.gridPub_.publish(msg)
def _timer_cb(self, event):
img, depth, cam_info, tf_map_to_camera, _ = self._dataset.get_frame(
self._stamp)
# Use current timestamp for each message
# Camera info
cam_info_msg = CameraInfo()
genpy.message.fill_message_args(cam_info_msg, cam_info)
cam_info_msg.header.stamp = event.current_real
# TF map -> camera
tf_msg = TransformStamped()
genpy.message.fill_message_args(tf_msg, tf_map_to_camera)
tf_msg.header.stamp = event.current_real
bridge = cv_bridge.CvBridge()
# RGB image
imgmsg = bridge.cv2_to_imgmsg(img, encoding='rgb8')
imgmsg.header.frame_id = cam_info_msg.header.frame_id
imgmsg.header.stamp = event.current_real
# Depth image
if depth.dtype == np.uint16:
depth = depth.astype(np.float32)
depth *= 0.001
depth_msg = bridge.cv2_to_imgmsg(depth, encoding='32FC1')
depth_msg.header.frame_id = cam_info_msg.header.frame_id
depth_msg.header.stamp = event.current_real
self.tfb.sendTransformMessage(tf_msg)
self.pub_rgb.publish(imgmsg)
self.pub_rgb_cam_info.publish(cam_info_msg)
self.pub_depth.publish(depth_msg)
self.pub_depth_cam_info.publish(cam_info_msg)
