1+ #include < ros/ros.h>
2+
13#include < boost/make_shared.hpp>
24#include < depthai_bridge/DisparityConverter.hpp>
3- # include < ros/ros.h >
5+
46#include " sensor_msgs/image_encodings.h"
57
68// FIXME(Sachin): Do I need to convert the encodings that are available in dai
1416// it planar
1517namespace dai ::rosBridge {
1618
17- /*
19+ /*
1820std::unordered_map<dai::RawImgFrame::Type, std::string> DisparityConverter::encodingEnumMap = {
1921 {dai::RawImgFrame::Type::YUV422i , "yuv422" },
2022 {dai::RawImgFrame::Type::RGBA8888 , "rgba8" },
@@ -28,87 +30,75 @@ std::unordered_map<dai::RawImgFrame::Type, std::string> DisparityConverter::enco
2830
2931std::unordered_map<dai::RawImgFrame::Type, std::string> DisparityConverter::planarEncodingEnumMap = {
3032 {dai::RawImgFrame::Type::BGR888p, "3_1_bgr8"}, // 3_1_bgr8 represents 3 planes/channels and 1 byte per pixel in BGR format
31- {dai::RawImgFrame::Type::NV12 , "nv12" }
33+ {dai::RawImgFrame::Type::NV12 , "nv12" }
3234
33- };
35+ };
3436*/
3537
36- DisparityConverter::DisparityConverter (const std::string frameName, float focalLength, float baseline, float minDepth, float maxDepth)
37- : _frameName(frameName), _focalLength(focalLength), _baseline(baseline / 100.0 ), _minDepth(minDepth / 100.0 ), _maxDepth(maxDepth / 100.0 ){}
38+ DisparityConverter::DisparityConverter (const std::string frameName, float focalLength, float baseline, float minDepth, float maxDepth)
39+ : _frameName(frameName), _focalLength(focalLength), _baseline(baseline / 100.0 ), _minDepth(minDepth / 100.0 ), _maxDepth(maxDepth / 100.0 ) {}
3840
39- void DisparityConverter::toRosMsg (std::shared_ptr<dai::ImgFrame> inData,
40- stereo_msgs::DisparityImage &outDispImageMsg) {
41+ void DisparityConverter::toRosMsg (std::shared_ptr<dai::ImgFrame> inData, stereo_msgs::DisparityImage& outDispImageMsg) {
42+ auto tstamp = inData->getTimestamp ();
43+ int32_t sec = std::chrono::duration_cast<std::chrono::seconds>(tstamp.time_since_epoch ()).count ();
44+ int32_t nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(tstamp.time_since_epoch ()).count () % 1000000000UL ;
4145
46+ outDispImageMsg.header .seq = inData->getSequenceNum ();
47+ outDispImageMsg.header .stamp = ros::Time (sec, nsec);
48+ outDispImageMsg.header .frame_id = _frameName;
49+ outDispImageMsg.f = _focalLength;
50+ outDispImageMsg.T = _baseline; // TODO(Sachin): Change this units
51+ outDispImageMsg.min_disparity = _focalLength * _baseline / _maxDepth;
52+ outDispImageMsg.max_disparity = _focalLength * _baseline / _minDepth;
4253
43- auto tstamp = inData->getTimestamp ();
44- int32_t sec = std::chrono::duration_cast<std::chrono::seconds>(
45- tstamp.time_since_epoch ())
46- .count ();
47- int32_t nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(
48- tstamp.time_since_epoch ())
49- .count () %
50- 1000000000UL ;
51-
52- outDispImageMsg.header .seq = inData->getSequenceNum ();
53- outDispImageMsg.header .stamp = ros::Time (sec, nsec);
54- outDispImageMsg.header .frame_id = _frameName;
55- outDispImageMsg.f = _focalLength;
56- outDispImageMsg.T = _baseline; // TODO(Sachin): Change this units
57- outDispImageMsg.min_disparity = _focalLength * _baseline / _maxDepth;
58- outDispImageMsg.max_disparity = _focalLength * _baseline / _minDepth;
59-
60- sensor_msgs::Image& outImageMsg = outDispImageMsg.image ;
54+ sensor_msgs::Image& outImageMsg = outDispImageMsg.image ;
6155
6256 // copying the data to ros msg
6357 // outDispImageMsg.header = imgHeader;
6458 // std::string temp_str(encodingEnumMap[inData->getType()]);
6559 outImageMsg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
6660 outImageMsg.header = outDispImageMsg.header ;
67- if (inData->getType () == dai::RawImgFrame::Type::RAW8){
68- outDispImageMsg.delta_d = 1 ;
69- size_t size = inData->getData ().size () * sizeof (float );
70- outImageMsg.data .resize (size);
71- outImageMsg.height = inData->getHeight ();
72- outImageMsg.width = inData->getWidth ();
73- outImageMsg.step = size / inData->getHeight ();
74- outImageMsg.is_bigendian = true ;
75-
76- std::vector<float > convertedData (inData->getData ().begin (),inData->getData ().end ());
77- unsigned char *imageMsgDataPtr =
78- reinterpret_cast <unsigned char *>(outImageMsg.data .data ());
79-
80- unsigned char *daiImgData =
81- reinterpret_cast <unsigned char *>(convertedData.data ());
82-
83- // TODO(Sachin): Try using assign since it is a vector
84- // img->data.assign(packet.data->cbegin(), packet.data->cend());
85- memcpy (imageMsgDataPtr, daiImgData, size);
86-
87- }else {
88- outDispImageMsg.delta_d = 1 / 32 ;
89- size_t size = inData->getHeight () * inData->getWidth () * sizeof (float );
90- outImageMsg.data .resize (size);
91- outImageMsg.height = inData->getHeight ();
92- outImageMsg.width = inData->getWidth ();
93- outImageMsg.step = size / inData->getHeight ();
94- outImageMsg.is_bigendian = true ;
95- unsigned char *daiImgData =
96- reinterpret_cast <unsigned char *>(inData->getData ().data ());
97-
98- std::vector<int16_t > raw16Data (inData->getHeight () * inData->getWidth ());
99- unsigned char *raw16DataPtr = reinterpret_cast <unsigned char *>(raw16Data.data ());
100- memcpy (raw16DataPtr, daiImgData, inData->getData ().size ());
101- std::vector<float > convertedData;
102- std::transform (raw16Data.begin (), raw16Data.end (), std::back_inserter (convertedData),
103- [](int16_t disp) -> std::size_t { return static_cast <float >(disp) / 32.0 ; });
104-
105- unsigned char *imageMsgDataPtr = reinterpret_cast <unsigned char *>(outImageMsg.data .data ());
106- unsigned char *convertedDataPtr = reinterpret_cast <unsigned char *>(convertedData.data ());
107- memcpy (imageMsgDataPtr, convertedDataPtr, size);
108-
61+ if (inData->getType () == dai::RawImgFrame::Type::RAW8) {
62+ outDispImageMsg.delta_d = 1 ;
63+ size_t size = inData->getData ().size () * sizeof (float );
64+ outImageMsg.data .resize (size);
65+ outImageMsg.height = inData->getHeight ();
66+ outImageMsg.width = inData->getWidth ();
67+ outImageMsg.step = size / inData->getHeight ();
68+ outImageMsg.is_bigendian = true ;
69+
70+ std::vector<float > convertedData (inData->getData ().begin (), inData->getData ().end ());
71+ unsigned char * imageMsgDataPtr = reinterpret_cast <unsigned char *>(outImageMsg.data .data ());
72+
73+ unsigned char * daiImgData = reinterpret_cast <unsigned char *>(convertedData.data ());
74+
75+ // TODO(Sachin): Try using assign since it is a vector
76+ // img->data.assign(packet.data->cbegin(), packet.data->cend());
77+ memcpy (imageMsgDataPtr, daiImgData, size);
78+
79+ } else {
80+ outDispImageMsg.delta_d = 1 / 32 ;
81+ size_t size = inData->getHeight () * inData->getWidth () * sizeof (float );
82+ outImageMsg.data .resize (size);
83+ outImageMsg.height = inData->getHeight ();
84+ outImageMsg.width = inData->getWidth ();
85+ outImageMsg.step = size / inData->getHeight ();
86+ outImageMsg.is_bigendian = true ;
87+ unsigned char * daiImgData = reinterpret_cast <unsigned char *>(inData->getData ().data ());
88+
89+ std::vector<int16_t > raw16Data (inData->getHeight () * inData->getWidth ());
90+ unsigned char * raw16DataPtr = reinterpret_cast <unsigned char *>(raw16Data.data ());
91+ memcpy (raw16DataPtr, daiImgData, inData->getData ().size ());
92+ std::vector<float > convertedData;
93+ std::transform (
94+ raw16Data.begin (), raw16Data.end (), std::back_inserter (convertedData), [](int16_t disp) -> std::size_t { return static_cast <float >(disp) / 32.0 ; });
95+
96+ unsigned char * imageMsgDataPtr = reinterpret_cast <unsigned char *>(outImageMsg.data .data ());
97+ unsigned char * convertedDataPtr = reinterpret_cast <unsigned char *>(convertedData.data ());
98+ memcpy (imageMsgDataPtr, convertedDataPtr, size);
10999 }
110-
111- return ;
100+
101+ return ;
112102}
113103
114104/* void DisparityConverter::toDaiMsg(const stereo_msgs::DisparityImage &inMsg,
@@ -158,11 +148,10 @@ void DisparityConverter::toRosMsg(std::shared_ptr<dai::ImgFrame> inData,
158148 outData.setType(revEncodingIter->first);
159149} */
160150
161- stereo_msgs::DisparityImagePtr
162- DisparityConverter::toRosMsgPtr (std::shared_ptr<dai::ImgFrame> inData) {
163- stereo_msgs::DisparityImagePtr ptr = boost::make_shared<stereo_msgs::DisparityImage>();
164- toRosMsg (inData, *ptr);
165- return ptr;
151+ stereo_msgs::DisparityImagePtr DisparityConverter::toRosMsgPtr (std::shared_ptr<dai::ImgFrame> inData) {
152+ stereo_msgs::DisparityImagePtr ptr = boost::make_shared<stereo_msgs::DisparityImage>();
153+ toRosMsg (inData, *ptr);
154+ return ptr;
166155}
167156
168- } // namespace dai::rosBridge
157+ } // namespace dai::rosBridge
0 commit comments