Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Compressed image publishers #580

Merged
merged 3 commits into from
Aug 22, 2024
Merged

Compressed image publishers #580

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8f49b8d
add compressed image publishers
Serafadam Aug 20, 2024
d81463e
minor cleanup
Serafadam Aug 21, 2024
28b70a3
fix publishing bugs in release mode, fix parameter bugs
Serafadam Aug 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions depthai_bridge/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(composition_interfaces REQUIRED)
find_package(ffmpeg_image_transport_msgs REQUIRED)

set(dependencies
camera_info_manager
Expand All @@ -56,6 +57,7 @@ set(dependencies
tf2_geometry_msgs
tf2
composition_interfaces
ffmpeg_image_transport_msgs
)

include_directories(
Expand Down
59 changes: 38 additions & 21 deletions depthai_bridge/include/depthai_bridge/ImageConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,12 @@
#include "depthai-shared/common/CameraBoardSocket.hpp"
#include "depthai-shared/common/Point2f.hpp"
#include "depthai/device/CalibrationHandler.hpp"
#include "depthai/pipeline/datatype/EncodedFrame.hpp"
#include "depthai/pipeline/datatype/ImgFrame.hpp"
#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
#include "rclcpp/time.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"

Expand All @@ -22,7 +25,10 @@ namespace ros {

namespace StdMsgs = std_msgs::msg;
namespace ImageMsgs = sensor_msgs::msg;
namespace FFMPEGMsgs = ffmpeg_image_transport_msgs::msg;
using ImagePtr = ImageMsgs::Image::SharedPtr;
using FFMPEGImagePtr = FFMPEGMsgs::FFMPEGPacket::SharedPtr;
using CompImagePtr = ImageMsgs::CompressedImage::SharedPtr;

using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;

Expand All @@ -47,7 +53,7 @@ class ImageConverter {
* @param update: bool whether to automatically update the ROS base time on message conversion
*/
void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
_updateRosBaseTimeOnToRosMsg = update;
updateRosBaseTimeOnToRosMsg = update;
}

/**
Expand Down Expand Up @@ -80,10 +86,20 @@ class ImageConverter {
*/
void setAlphaScaling(double alphaScalingFactor = 0.0);

/**
* @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264.
* @param encoding: The encoding to be used.
*/
void setFFMPEGEncoding(const std::string& encoding);

void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info = sensor_msgs::msg::CameraInfo());
ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);

FFMPEGMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);

ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData);

void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);

/** TODO(sachin): Add support for ros msg to cv mat since we have some
Expand All @@ -99,36 +115,37 @@ class ImageConverter {
Point2f bottomRightPixelId = Point2f());

private:
void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
static std::unordered_map<dai::RawImgFrame::Type, std::string> encodingEnumMap;
static std::unordered_map<dai::RawImgFrame::Type, std::string> planarEncodingEnumMap;

// dai::RawImgFrame::Type _srcType;
bool _daiInterleaved;
bool daiInterleaved;
// bool c
const std::string _frameName = "";
void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
const std::string frameName = "";
std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime;

rclcpp::Time _rosBaseTime;
bool _getBaseDeviceTimestamp;
rclcpp::Time rosBaseTime;
bool getBaseDeviceTimestamp;
// For handling ROS time shifts and debugging
int64_t _totalNsChange{0};
int64_t totalNsChange{0};
// Whether to update the ROS base time on each message conversion
bool _updateRosBaseTimeOnToRosMsg{false};
dai::RawImgFrame::Type _srcType;
bool _fromBitstream = false;
bool _convertDispToDepth = false;
bool _addExpOffset = false;
bool _alphaScalingEnabled = false;
dai::CameraExposureOffset _expOffset;
bool _reverseStereoSocketOrder = false;
double _baseline;
double _alphaScalingFactor = 0.0;
bool updateRosBaseTimeOnToRosMsg{false};
dai::RawImgFrame::Type srcType;
bool fromBitstream = false;
bool dispToDepth = false;
bool addExpOffset = false;
bool alphaScalingEnabled = false;
dai::CameraExposureOffset expOffset;
bool reversedStereoSocketOrder = false;
double baseline;
double alphaScalingFactor = 0.0;
int camHeight = -1;
int camWidth = -1;
std::string ffmpegEncoding = "libx264";
};

} // namespace ros

namespace rosBridge = ros;

} // namespace dai
1 change: 1 addition & 0 deletions depthai_bridge/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
<depend>std_msgs</depend>
<depend>stereo_msgs</depend>
<depend>vision_msgs</depend>
<depend>ffmpeg_image_transport_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
Expand Down
146 changes: 114 additions & 32 deletions depthai_bridge/src/ImageConverter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@

#include "depthai_bridge/ImageConverter.hpp"

#include <sensor_msgs/msg/detail/compressed_image__struct.hpp>

#include "depthai-shared/datatype/RawEncodedFrame.hpp"
#include "depthai/pipeline/datatype/EncodedFrame.hpp"
#include "depthai_bridge/depthaiUtility.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
Expand All @@ -26,71 +30,75 @@ std::unordered_map<dai::RawImgFrame::Type, std::string> ImageConverter::planarEn
{dai::RawImgFrame::Type::YUV420p, "rgb8"}};

ImageConverter::ImageConverter(bool interleaved, bool getBaseDeviceTimestamp)
: _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
_rosBaseTime = rclcpp::Clock().now();
: daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
rosBaseTime = rclcpp::Clock().now();
}

ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp)
: _frameName(frameName), _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
_rosBaseTime = rclcpp::Clock().now();
: frameName(frameName), daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
rosBaseTime = rclcpp::Clock().now();
}

ImageConverter::~ImageConverter() = default;

void ImageConverter::updateRosBaseTime() {
updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange);
updateBaseTime(steadyBaseTime, rosBaseTime, totalNsChange);
}

void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) {
_fromBitstream = true;
_srcType = srcType;
fromBitstream = true;
this->srcType = srcType;
}

void ImageConverter::convertDispToDepth(double baseline) {
_convertDispToDepth = true;
_baseline = baseline;
dispToDepth = true;
this->baseline = baseline;
}

void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) {
_expOffset = offset;
_addExpOffset = true;
expOffset = offset;
addExpOffset = true;
}

void ImageConverter::reverseStereoSocketOrder() {
_reverseStereoSocketOrder = true;
reversedStereoSocketOrder = true;
}

void ImageConverter::setAlphaScaling(double alphaScalingFactor) {
_alphaScalingEnabled = true;
_alphaScalingFactor = alphaScalingFactor;
alphaScalingEnabled = true;
this->alphaScalingFactor = alphaScalingFactor;
}

void ImageConverter::setFFMPEGEncoding(const std::string& encoding) {
ffmpegEncoding = encoding;
}

ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info) {
if(_updateRosBaseTimeOnToRosMsg) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
if(_addExpOffset)
tstamp = inData->getTimestampDevice(_expOffset);
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = inData->getTimestampDevice(expOffset);
else
tstamp = inData->getTimestampDevice();
else if(_addExpOffset)
tstamp = inData->getTimestamp(_expOffset);
else if(addExpOffset)
tstamp = inData->getTimestamp(expOffset);
else
tstamp = inData->getTimestamp();
ImageMsgs::Image outImageMsg;
StdMsgs::Header header;
header.frame_id = _frameName;
header.frame_id = frameName;

header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);

if(_fromBitstream) {
if(fromBitstream) {
std::string encoding;
int decodeFlags;
int channels;
cv::Mat output;
switch(_srcType) {
switch(srcType) {
case dai::RawImgFrame::Type::BGR888i: {
encoding = sensor_msgs::image_encodings::BGR8;
decodeFlags = cv::IMREAD_COLOR;
Expand All @@ -110,16 +118,16 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i
break;
}
default: {
std::cout << _frameName << static_cast<int>(_srcType) << std::endl;
std::cout << frameName << static_cast<int>(srcType) << std::endl;
throw(std::runtime_error("Converted type not supported!"));
}
}

output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags);

// converting disparity
if(_convertDispToDepth) {
auto factor = std::abs(_baseline * 10) * info.p[0];
if(dispToDepth) {
auto factor = std::abs(baseline * 10) * info.p[0];
cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1);
depthOut.forEach<uint16_t>([&output, &factor](uint16_t& pixel, const int* position) -> void {
auto disp = output.at<uint8_t>(position);
Expand Down Expand Up @@ -209,7 +217,79 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i
}
return outImageMsg;
}
std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> getOffsetTimestamp(
std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> ts,
CameraExposureOffset offset,
std::chrono::microseconds expTime) {
switch(offset) {
case CameraExposureOffset::START:
return ts - expTime;
case CameraExposureOffset::MIDDLE:
return ts - expTime / 2;
case CameraExposureOffset::END:
default:
return ts;
}
}

ImageMsgs::CompressedImage ImageConverter::toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestampDevice();
else if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestamp();

ImageMsgs::CompressedImage outImageMsg;
StdMsgs::Header header;
header.frame_id = frameName;
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);

outImageMsg.header = header;
outImageMsg.format = "jpeg";
outImageMsg.data = inData->getData();
return outImageMsg;
}

FFMPEGMsgs::FFMPEGPacket ImageConverter::toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestampDevice();
else if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestamp();

FFMPEGMsgs::FFMPEGPacket outFrameMsg;
StdMsgs::Header header;
header.frame_id = frameName;
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);
outFrameMsg.header = header;
auto ft = inData->getFrameType();

outFrameMsg.width = camWidth;
outFrameMsg.height = camHeight;
outFrameMsg.encoding = ffmpegEncoding;
outFrameMsg.pts = header.stamp.sec * 1000000000 + header.stamp.nanosec; // in nanoseconds
outFrameMsg.flags = (int)(ft == RawEncodedFrame::FrameType::I);
outFrameMsg.is_bigendian = false;
outFrameMsg.data = inData->getData();

return outFrameMsg;
}
void ImageConverter::toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs) {
auto outImageMsg = toRosMsgRawPtr(inData);
outImageMsgs.push_back(outImageMsg);
Expand All @@ -225,7 +305,7 @@ ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData) {

void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) {
std::unordered_map<dai::RawImgFrame::Type, std::string>::iterator revEncodingIter;
if(_daiInterleaved) {
if(daiInterleaved) {
revEncodingIter = std::find_if(encodingEnumMap.begin(), encodingEnumMap.end(), [&](const std::pair<dai::RawImgFrame::Type, std::string>& pair) {
return pair.second == inMsg.encoding;
});
Expand Down Expand Up @@ -352,6 +432,8 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
cameraData.height = static_cast<uint32_t>(height);
}

camWidth = cameraData.width;
camHeight = cameraData.height;
camIntrinsics = calibHandler.getCameraIntrinsics(cameraId, cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);

flatIntrinsics.resize(9);
Expand Down Expand Up @@ -382,7 +464,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
std::vector<std::vector<float>> stereoIntrinsics = calibHandler.getCameraIntrinsics(
calibHandler.getStereoRightCameraId(), cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);

if(_alphaScalingEnabled) {
if(alphaScalingEnabled) {
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F);
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
Expand All @@ -391,7 +473,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
}
cv::Mat distCoefficients(distCoeffs);

cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), _alphaScalingFactor);
cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), alphaScalingFactor);
// Copying the contents of newCameraMatrix to stereoIntrinsics
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
Expand All @@ -411,7 +493,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
dai::CameraBoardSocket stereoSocketFirst = calibHandler.getStereoLeftCameraId();
dai::CameraBoardSocket stereoSocketSecond = calibHandler.getStereoRightCameraId();
double factor = 1.0;
if(_reverseStereoSocketOrder) {
if(reversedStereoSocketOrder) {
stereoSocketFirst = calibHandler.getStereoRightCameraId();
stereoSocketSecond = calibHandler.getStereoLeftCameraId();
factor = -1.0;
Expand Down Expand Up @@ -440,4 +522,4 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
return cameraData;
}
} // namespace ros
} // namespace dai
} // namespace dai
Loading
Loading