Program Listing for File SpatialDetectionConverter.hpp
↰ Return to documentation for file (/tmp/ws/src/depthai-ros/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp
)
#pragma once
#include <deque>
#include <memory>
#include <string>
#include "depthai/pipeline/datatype/SpatialImgDetections.hpp"
#include "depthai_ros_msgs/msg/spatial_detection_array.hpp"
#include "rclcpp/time.hpp"
#include "vision_msgs/msg/detection3_d_array.hpp"
namespace dai {
namespace ros {
namespace SpatialMessages = depthai_ros_msgs::msg;
using SpatialDetectionArrayPtr = SpatialMessages::SpatialDetectionArray::SharedPtr;
class SpatialDetectionConverter {
public:
SpatialDetectionConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false);
~SpatialDetectionConverter();
void updateRosBaseTime();
void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
_updateRosBaseTimeOnToRosMsg = update;
}
void toRosMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<SpatialMessages::SpatialDetectionArray>& opDetectionMsg);
void toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<vision_msgs::msg::Detection3DArray>& opDetectionMsg);
SpatialDetectionArrayPtr toRosMsgPtr(std::shared_ptr<dai::SpatialImgDetections> inNetData);
private:
int _width, _height;
const std::string _frameName;
bool _normalized;
std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
rclcpp::Time _rosBaseTime;
bool _getBaseDeviceTimestamp;
// For handling ROS time shifts and debugging
int64_t _totalNsChange{0};
// Whether to update the ROS base time on each message conversion
bool _updateRosBaseTimeOnToRosMsg{false};
};
} // namespace ros
namespace rosBridge = ros;
} // namespace dai