35#ifdef ROS_DISTRO_IS_ROLLING
36#include <tf2_ros/transform_listener.hpp>
38#include <tf2_ros/transform_listener.h>
42#include <auterion_core_msgs/msg/camera_info.hpp>
43#include <auterion_core_msgs/msg/image8m.hpp>
46#include <opencv2/core/mat.hpp>
47#include <rclcpp/rclcpp.hpp>
48#include <sensor_msgs/msg/camera_info.hpp>
52#include "camera_control.hpp"
58using namespace std::chrono_literals;
61class CameraMonitorImpl;
92 template <
typename ImageT = auterion_core_msgs::msg::Image8m>
94 : width(image.width), height(image.height),
step(image.
step), timestamp(image.stamp) {
95 const int encoding_len =
96 strnlen(
reinterpret_cast<const char*
>(image.encoding.data()), image.encoding.size());
97 const std::string encoding_str(
reinterpret_cast<const char*
>(image.encoding.data()),
99 if (encoding_str ==
"mono8") {
101 }
else if (encoding_str ==
"mono16") {
103 }
else if (encoding_str ==
"uyvy") {
105 }
else if (encoding_str ==
"yuyv") {
107 }
else if (encoding_str ==
"i420") {
111 }
else if (encoding_str ==
"nv12") {
114 }
else if (encoding_str ==
"rgb8") {
115 encoding = Encoding::RGB8;
116 }
else if (encoding_str ==
"rgba8") {
118 }
else if (encoding_str ==
"bgr8") {
119 encoding = Encoding::BGR8;
120 }
else if (encoding_str ==
"bayer_bggr8") {
122 }
else if (encoding_str ==
"bayer_gbrg8") {
124 }
else if (encoding_str ==
"bayer_grbg8") {
126 }
else if (encoding_str ==
"bayer_rggb8") {
131 template <
typename ImageT = auterion_core_msgs::msg::Image8m>
132 void toImage(ImageT& image) {
134 image.height = height;
136 image.stamp = timestamp;
138 case Encoding::Unknown:
139 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"unknown",
140 image.encoding.size());
143 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"mono8",
144 image.encoding.size());
147 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"mono16",
148 image.encoding.size());
151 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"uyvy",
152 image.encoding.size());
155 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"yuyv",
156 image.encoding.size());
159 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"i420",
160 image.encoding.size());
163 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"nv12",
164 image.encoding.size());
167 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"rgb8",
168 image.encoding.size());
171 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"bgr8",
172 image.encoding.size());
175 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"rgba8",
176 image.encoding.size());
179 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"bayer_bggr8",
180 image.encoding.size());
183 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"bayer_gbrg8",
184 image.encoding.size());
187 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"bayer_grbg8",
188 image.encoding.size());
191 strncpy(
reinterpret_cast<char*
>(image.encoding.data()),
"bayer_rggb8",
192 image.encoding.size());
197 size_t planeOffset(
int plane)
const;
198 size_t planeSizeBytes(
int plane)
const;
199 size_t imageSizeBytes()
const;
204 Encoding encoding{Encoding::Unknown};
206 rclcpp::Time timestamp{};
221 float framerate{0.f};
223 sensor_msgs::msg::CameraInfo camera_info{};
234 Image(
ImageHeader header, uint8_t* data) : _header(std::move(header)), _data(data) {}
236 const ImageHeader& header()
const {
return _header; }
238 uint8_t* data() {
return _data; }
239 const uint8_t* data()
const {
return _data; }
241 int width()
const {
return _header.width; }
242 int height()
const {
return _header.height; }
252 uint8_t* _data{
nullptr};
265 std::function<
void()> release_function, uint8_t* buffer =
nullptr,
266 std::function<
void()> discard_function =
nullptr)
267 : _header(std::move(header)),
269 _buffer_size(buffer_size),
271 _release_function(std::move(release_function)),
272 _discard_function(std::move(discard_function)) {}
283 : _header(std::move(other._header)),
285 _buffer_size(other._buffer_size),
286 _buffer(other.buffer()),
287 _release_function(std::move(other._release_function)),
288 _discard_function(std::move(other._discard_function)),
289 _was_released(other._was_released) {
291 other._buffer_size = 0;
292 other._buffer =
nullptr;
293 other._release_function =
nullptr;
294 other._discard_function =
nullptr;
295 other._was_released =
true;
300 if (
this != &other) {
303 _header = std::move(other._header);
305 _buffer_size = other._buffer_size;
306 _buffer = other._buffer;
307 _release_function = std::move(other._release_function);
308 _discard_function = std::move(other._discard_function);
309 _was_released = other._was_released;
311 other._buffer_size = 0;
312 other._buffer =
nullptr;
313 other._release_function =
nullptr;
314 other._discard_function =
nullptr;
315 other._was_released =
true;
320 const ImageHeader& header()
const {
return _header; }
322 int fd()
const {
return _fd; }
324 uint64_t bufferSize()
const {
return _buffer_size; }
326 uint8_t* buffer()
const {
return _buffer; }
332 if (_release_function) {
334 _release_function =
nullptr;
338 _was_released =
true;
341 bool wasReleased()
const {
return _was_released; }
349 if (_discard_function) {
351 _discard_function =
nullptr;
358 uint64_t _buffer_size{0};
359 uint8_t* _buffer{
nullptr};
367 std::function<void()> _release_function{
nullptr};
370 std::function<void()> _discard_function{
nullptr};
372 bool _was_released{
false};
380 enum class SensorType {
388 enum class MountOrientation {
398 enum class MountOrientationDetail {
405 enum class PrimaryPurpose {
414 enum class ImageRotation : uint8_t {
416 Rotation90Clockwise = 1,
418 Rotation270Clockwise = 3,
421 Eigen::Quaterniond rotation{-0.5, 0.5, -0.5, 0.5};
422 Eigen::Vector3d translation{0.0, 0.0, 0.0};
426 explicit CameraDescriptor(
const auterion_core_msgs::msg::CameraInfo& camera_info);
428 operator auterion_core_msgs::msg::CameraInfo()
const;
430 std::string device_driver;
431 std::string device_identifier;
433 std::string unique_hash;
434 std::string unique_name;
436 std::string camera_model;
438 SensorType sensor_type{SensorType::Unknown};
439 MountOrientation mount_orientation{MountOrientation::Unknown};
440 MountOrientationDetail mount_orientation_detail{MountOrientationDetail::Unknown};
441 PrimaryPurpose primary_purpose{PrimaryPurpose::Unknown};
442 ImageRotation image_rotation{ImageRotation::None};
443 ExtrinsicCalibration extrinsic_calibration;
452 enum class Mode { None, Point, Rectangle };
457 Mode mode{Mode::None};
462 float rectangle_top_y{0.F};
463 float rectangle_bottom_x{0.F};
464 float rectangle_bottom_y{0.F};
466 unsigned resolution_y{0};
494 const NotificationCallback& on_camera_removed =
nullptr);
499 std::shared_ptr<CameraMonitorImpl> _impl;
509 Camera(std::shared_ptr<CameraImpl> impl);
519 "waitForCamera with 'camera_descriptor' not used anymore. This will be removed in some "
520 "later release.")]]
static Camera
522 std::optional<auterion::CameraDescriptor> camera_descriptor = std::nullopt);
524 static std::optional<Camera> open(
SDK& sdk,
const std::string& unique_name,
525 const std::chrono::seconds& timeout = 30s);
526 static std::optional<Camera> open(
528 const std::chrono::seconds& timeout = 30s);
529 static std::optional<Camera> openFirst(
SDK& sdk,
const std::chrono::seconds& timeout = 30s);
531 using ImageCallback = std::function<void(
const Image& image)>;
532 using ImageFDCallback = std::function<void(
const std::shared_ptr<ImageFD>& image_fd)>;
533 using CameraInfoAfterSwitchCallback =
534 std::function<void(
const sensor_msgs::msg::CameraInfo::SharedPtr& camera_info)>;
535 using TrackingStatusCallback = std::function<void(
const TrackingStatus& status)>;
537 using CameraStatusCallback = std::function<void(
const CameraStatus& status)>;
561 void subscribeImageFD(
const ImageFDCallback& callback,
float max_rate_hz = 0.0F);
609 ImageReplacementOptions& withPixelFormat(
PixelFormat format) {
614 ImageReplacementOptions& withUseOutputImageSize(
bool use) {
629 void deactivateImageReplacement();
684 "subscribeCameraInfo() is deprecated and will be removed in a future release. The "
685 "CameraInfo is instead included in the image header obtained with subscribeImage(), and "
686 "can also be queried with getCameraInfo().")]]
void
688 std::function<
void(
const sensor_msgs::msg::CameraInfo::SharedPtr)> callback);
766 bool getPoseBodyCamera(Eigen::Quaterniond& q_body_camera, Eigen::Vector3d& t_body_camera)
const;
770 const tf2::TimePoint& time)
const;
791 const tf2::TimePoint& time = tf2::TimePointZero)
const;
826 tf2::TimePoint& time_start, tf2::TimePoint& time_end);
874 std::shared_ptr<CameraImpl> _impl;
Definition camera_control.hpp:17
Monitor for new or removed cameras, of both camera protocols. Existing cameras are reported from the ...
Definition camera.hpp:489
Camera client to subscribe to a camera image stream.
Definition camera.hpp:506
void setImagesPaused(bool paused)
Stop or resume image delivery without giving up the subscription.
std::optional< ImageFD > getReplacementImage(const ImageFD &image)
Get a replacement image buffer.
const CameraDescriptor & descriptor() const
Get the descriptor of the active camera source.
bool cameraInfoValid() const
Determine whether a camera info message contains valid data.
void interestedInReplacement(bool interested=true)
Indicate interest in activating image replacement.
void subscribeServiceStatus(ServiceStatusCallback callback)
Subscribe to the status of the service providing this camera.
void subscribeCameraInfoAfterSwitch(const CameraInfoAfterSwitchCallback &callback)
Subscribe to camera info updates emitted once after each successful stream switch.
bool getCameraInfo(sensor_msgs::msg::CameraInfo &camera_info) const
Get the camera information data for the camera.
bool getPoseBodyCamera(Eigen::Quaterniond &q_body_camera, Eigen::Vector3d &t_body_camera) const
Get the pose of the camera with respect to the body frame.
bool usesImageSession() const
bool getPoseBodyCamera(Eigen::Quaterniond &q_body_camera, Eigen::Vector3d &t_body_camera, const tf2::TimePoint &time) const
Camera pose in body FLU at the requested capture time.
void subscribeImage(const ImageCallback &callback)
void subscribeCameraStatus(CameraStatusCallback callback)
Subscribe to the camera status.
std::optional< ImageFD > getReplacementImage(uint32_t original_frame_id)
Get a replacement image buffer by the original frame's id.
Eigen::Vector3d unprojectImagePoint(const cv::Point &image_point) const
Determine the (relative) camera-frame 3D position of a point in an image.
bool getDeltaRotationWorldCamera(Eigen::Quaterniond &q_delta_world_camera, tf2::TimePoint &time_start, tf2::TimePoint &time_end)
Calculates how much the camera has rotated between two points in time based only on gyro data.
std::optional< cv::Size > getLastImageSize() const
Returns the size of the last received image, if available.
bool getCameraInfoDirect(sensor_msgs::msg::CameraInfo &camera_info) const
get the camera info directly from the camera
static Camera waitForCamera(SDK &sdk, const std::function< bool(const CameraDescriptor &)> &find_callback)
void subscribeTrackingStatus(TrackingStatusCallback callback)
Subscribe to the status of the tracking on this camera.
bool switchTo(const std::function< bool(const CameraDescriptor &)> &find_callback)
Switch active UnixFD input to a discovered camera selected by callback.
bool getPoseWorldCamera(Eigen::Quaterniond &q_enu_camera, Eigen::Vector3d &t_enu_camera, const tf2::TimePoint &time=tf2::TimePointZero) const
Get the pose of the camera with respect to the ENU world frame.
std::chrono::steady_clock::time_point lastReplacementImageOffer() const
When the camera service last offered a replacement image buffer.
void activateImageReplacement(const ImageReplacementOptions &options=ImageReplacementOptions())
Activate image replacement.
Definition camera.hpp:262
Contains image data with a specific encoding.
Definition camera.hpp:232
cv::Mat asOpenCVMat(int plane=-1) const
SDK execution class. All callbacks are called on the same thread.
Definition auterion.hpp:133
Definition camera.hpp:420
Struct describing a camera configuration.
Definition camera.hpp:379
The status the service providing a camera (e.g. video-service) reports for it, about once a second.
Definition camera.hpp:475
uint64_t session_id
changes when the service (re)starts the camera
Definition camera.hpp:476
Definition camera.hpp:479
bool output_enabled
If true, the camera streams images (to a GCS)
Definition camera.hpp:480
Definition camera.hpp:582
bool use_output_image_size
or equal to the subscription image size)
Definition camera.hpp:600
PixelFormat pixel_format
Definition camera.hpp:598
PixelFormat
Definition camera.hpp:583
@ YUYV
packed 4:2:2 (FourCC YUY2)
std::chrono::milliseconds timeout
Definition camera.hpp:592
The status of the tracking on a camera, as the app tracking on it reports it (see TrackingInterface)....
Definition camera.hpp:451
float rectangle_top_x
Mode::Rectangle.
Definition camera.hpp:461
unsigned resolution_x
the displayed resolution, 0 if unknown
Definition camera.hpp:465
bool coasting
the target is obscured, its position predicted
Definition camera.hpp:455
float point_x
Mode::Point.
Definition camera.hpp:458
bool active
a target is tracked (or coasting)
Definition camera.hpp:454
unsigned hfov_deg
the displayed horizontal field of view, 0 if unknown
Definition camera.hpp:467