Skip to content

Local commits done to our fork #680

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

Open
MateoGiova wants to merge 4 commits into
base: humble
Choose a base branch
from
Open

Local commits done to our fork #680

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1019ed7
Add edge detector support
hidmic Jun 25, 2023
23995f5
Expose all IMU sensor modes
hidmic Jul 22, 2023
c96d869
Expose i_stereo_conf_threshold for runtime configuration
hidmic Oct 12, 2023
f5e82a1
Force depth alignment to be centered
hidmic Oct 12, 2023
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
6 changes: 4 additions & 2 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,9 @@ class Detection : public BaseNode {
height));

ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw");
ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager));
ptQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *imageConverter, ptPub, infoManager);
});
}
};
void link(dai::Node::Input in, int /*linkType*/) override {
Expand Down Expand Up @@ -140,4 +142,4 @@ class Detection : public BaseNode {

} // namespace nn
} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
11 changes: 7 additions & 4 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,9 @@ class SpatialDetection : public BaseNode {
height));

ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw");
ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan));
ptQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *ptImageConverter, ptPub, ptInfoMan);
});
}

if(ph->getParam<bool>("i_enable_passthrough_depth")) {
Expand All @@ -98,8 +100,9 @@ class SpatialDetection : public BaseNode {
getROSNode()->get_parameter("stereo.i_height").as_int()));

ptDepthPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough_depth/image_raw");
ptDepthQ->addCallback(
std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptDepthImageConverter, ptDepthPub, ptDepthInfoMan));
ptDepthQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *ptDepthImageConverter, ptDepthPub, ptDepthInfoMan);
});
}
};
void link(dai::Node::Input in, int /*linkType = 0*/) override {
Expand Down Expand Up @@ -173,4 +176,4 @@ class SpatialDetection : public BaseNode {

} // namespace nn
} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
11 changes: 9 additions & 2 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
#include "image_transport/camera_publisher.hpp"
#include "image_transport/image_transport.hpp"
#include "image_transport/publisher.hpp"

namespace dai {
class Pipeline;
Expand All @@ -13,6 +14,7 @@ class DataInputQueue;
class ADatatype;
namespace node {
class MonoCamera;
class EdgeDetector;
class XLinkIn;
class XLinkOut;
class VideoEncoder;
Expand Down Expand Up @@ -56,16 +58,21 @@ class Mono : public BaseNode {
private:
std::unique_ptr<dai::ros::ImageConverter> imageConverter;
image_transport::CameraPublisher monoPub;
image_transport::Publisher edgesPub;
std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager;
std::shared_ptr<dai::node::MonoCamera> monoCamNode;
std::shared_ptr<dai::node::EdgeDetector> edgeDetectorNode;
std::shared_ptr<dai::node::VideoEncoder> videoEnc;
std::shared_ptr<dai::node::VideoEncoder> edgesVideoEnc;
std::unique_ptr<param_handlers::SensorParamHandler> ph;
std::shared_ptr<dai::DataOutputQueue> monoQ;
std::shared_ptr<dai::DataOutputQueue> edgesQ;
std::shared_ptr<dai::DataInputQueue> controlQ;
std::shared_ptr<dai::node::XLinkOut> xoutMono;
std::shared_ptr<dai::node::XLinkOut> xoutEdges;
std::shared_ptr<dai::node::XLinkIn> xinControl;
std::string monoQName, controlQName;
std::string monoQName, edgesQName, controlQName;
};

} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
14 changes: 9 additions & 5 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include "depthai_ros_driver/dai_nodes/base_node.hpp"
#include "image_transport/camera_publisher.hpp"
#include "image_transport/image_transport.hpp"
#include "image_transport/publisher.hpp"
#include "sensor_msgs/msg/camera_info.hpp"

namespace dai {
Expand All @@ -14,6 +15,7 @@ enum class CameraBoardSocket;
class ADatatype;
namespace node {
class ColorCamera;
class EdgeDetector;
class XLinkIn;
class XLinkOut;
class VideoEncoder;
Expand Down Expand Up @@ -61,16 +63,18 @@ class RGB : public BaseNode {
private:
std::unique_ptr<dai::ros::ImageConverter> imageConverter;
image_transport::CameraPublisher rgbPub, previewPub;
image_transport::Publisher edgesPub;
std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager, previewInfoManager;
std::shared_ptr<dai::node::ColorCamera> colorCamNode;
std::shared_ptr<dai::node::VideoEncoder> videoEnc;
std::shared_ptr<dai::node::EdgeDetector> edgeDetectorNode;
std::shared_ptr<dai::node::VideoEncoder> videoEnc, edgesVideoEnc;
std::unique_ptr<param_handlers::SensorParamHandler> ph;
std::shared_ptr<dai::DataOutputQueue> colorQ, previewQ;
std::shared_ptr<dai::DataOutputQueue> colorQ, previewQ, edgesQ;
std::shared_ptr<dai::DataInputQueue> controlQ;
std::shared_ptr<dai::node::XLinkOut> xoutColor, xoutPreview;
std::shared_ptr<dai::node::XLinkOut> xoutColor, xoutPreview, xoutEdges;
std::shared_ptr<dai::node::XLinkIn> xinControl;
std::string ispQName, previewQName, controlQName;
std::string ispQName, previewQName, controlQName, edgesQName;
};

} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
15 changes: 14 additions & 1 deletion depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include "depthai-shared/properties/VideoEncoderProperties.hpp"
#include "depthai/pipeline/datatype/ADatatype.hpp"
#include "image_transport/camera_publisher.hpp"
#include "image_transport/publisher.hpp"
#include "sensor_msgs/msg/camera_info.hpp"

namespace dai {
Expand Down Expand Up @@ -44,12 +45,24 @@ struct ImageSensor {
};
extern std::vector<ImageSensor> availableSensors;

void imgCB(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
dai::ros::ImageConverter& converter,
image_transport::Publisher& pub);

void imgCB(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
dai::ros::ImageConverter& converter,
image_transport::CameraPublisher& pub,
std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager);

void compressedImgCB(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
dai::ros::ImageConverter& converter,
image_transport::Publisher& pub,
std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager,
dai::RawImgFrame::Type dataType);

void compressedImgCB(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
dai::ros::ImageConverter& converter,
Expand All @@ -68,4 +81,4 @@ std::shared_ptr<dai::node::VideoEncoder> createEncoder(std::shared_ptr<dai::Pipe
dai::VideoEncoderProperties::Profile profile = dai::VideoEncoderProperties::Profile::MJPEG);
} // namespace sensor_helpers
} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
9 changes: 7 additions & 2 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@
namespace dai {
class Pipeline;
class Device;
class DataInputQueue;
class DataOutputQueue;
class ADatatype;
class StereoDepthConfig;
namespace node {
class StereoDepth;
class XLinkOut;
Expand Down Expand Up @@ -76,11 +78,14 @@ class Stereo : public BaseNode {
std::unique_ptr<SensorWrapper> left;
std::unique_ptr<SensorWrapper> right;
std::unique_ptr<param_handlers::StereoParamHandler> ph;
std::shared_ptr<dai::DataInputQueue> configQ;
std::shared_ptr<dai::DataOutputQueue> stereoQ, leftRectQ, rightRectQ;
std::shared_ptr<dai::node::XLinkIn> xinConfig;
std::shared_ptr<dai::node::XLinkOut> xoutStereo, xoutLeftRect, xoutRightRect;
std::string stereoQName, leftRectQName, rightRectQName;
std::string stereoQName, leftRectQName, rightRectQName, configQName;
StereoSensorInfo leftSensInfo, rightSensInfo;
std::shared_ptr<dai::StereoDepthConfig> config;
};

} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
7 changes: 6 additions & 1 deletion depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include "depthai_ros_driver/param_handlers/base_param_handler.hpp"

namespace dai {
enum class IMUSensor;
namespace node {
class IMU;
}
Expand All @@ -30,10 +31,14 @@ class ImuParamHandler : public BaseParamHandler {
~ImuParamHandler();
void declareParams(std::shared_ptr<dai::node::IMU> imu, const std::string& imuType);
dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;
std::unordered_map<std::string, dai::IMUSensor> imuAccelerometerModeMap;
std::unordered_map<std::string, dai::IMUSensor> imuGyroscopeModeMap;
std::unordered_map<std::string, dai::IMUSensor> imuMagnetometerModeMap;
std::unordered_map<std::string, dai::IMUSensor> imuRotationModeMap;
std::unordered_map<std::string, dai::ros::ImuSyncMethod> imuSyncMethodMap;
std::unordered_map<std::string, imu::ImuMsgType> imuMessagetTypeMap;
imu::ImuMsgType getMsgType();
dai::ros::ImuSyncMethod getSyncMethod();
};
} // namespace param_handlers
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
4 changes: 3 additions & 1 deletion depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ namespace dai {
namespace node {
class MonoCamera;
class ColorCamera;
class EdgeDetector;
} // namespace node
} // namespace dai

Expand All @@ -29,6 +30,7 @@ class SensorParamHandler : public BaseParamHandler {
explicit SensorParamHandler(rclcpp::Node* node, const std::string& name);
~SensorParamHandler();
void declareCommonParams();
void declareParams(std::shared_ptr<dai::node::EdgeDetector> edgeDetector);
void declareParams(std::shared_ptr<dai::node::MonoCamera> monoCam,
dai::CameraBoardSocket socket,
dai_nodes::sensor_helpers::ImageSensor sensor,
Expand All @@ -45,4 +47,4 @@ class SensorParamHandler : public BaseParamHandler {
std::unordered_map<std::string, dai::CameraControl::FrameSyncMode> fSyncModeMap;
};
} // namespace param_handlers
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
2 changes: 1 addition & 1 deletion depthai_ros_driver/include/depthai_ros_driver/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,4 +24,4 @@ T getValFromMap(const std::string& name, const std::unordered_map<std::string, T
}
std::string getUpperCaseStr(const std::string& string);
} // namespace utils
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
6 changes: 4 additions & 2 deletions depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,9 @@ void Segmentation::setupQueues(std::shared_ptr<dai::Device> device) {
imageManip->initialConfig.getResizeWidth()));

ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw");
ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager));
ptQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *imageConverter, ptPub, infoManager);
});
}
}

Expand Down Expand Up @@ -132,4 +134,4 @@ void Segmentation::updateParams(const std::vector<rclcpp::Parameter>& params) {
}
} // namespace nn
} // namespace dai_nodes
} // namespace depthai_ros_driver
} // namespace depthai_ros_driver
56 changes: 48 additions & 8 deletions depthai_ros_driver/src/dai_nodes/sensors/mono.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "depthai/device/DataQueue.hpp"
#include "depthai/device/Device.hpp"
#include "depthai/pipeline/Pipeline.hpp"
#include "depthai/pipeline/node/EdgeDetector.hpp"
#include "depthai/pipeline/node/MonoCamera.hpp"
#include "depthai/pipeline/node/VideoEncoder.hpp"
#include "depthai/pipeline/node/XLinkIn.hpp"
Expand All @@ -12,6 +13,7 @@
#include "depthai_ros_driver/param_handlers/sensor_param_handler.hpp"
#include "image_transport/camera_publisher.hpp"
#include "image_transport/image_transport.hpp"
#include "image_transport/publisher.hpp"
#include "rclcpp/node.hpp"

namespace depthai_ros_driver {
Expand All @@ -28,16 +30,27 @@ Mono::Mono(const std::string& daiNodeName,
monoCamNode = pipeline->create<dai::node::MonoCamera>();
ph = std::make_unique<param_handlers::SensorParamHandler>(node, daiNodeName);
ph->declareParams(monoCamNode, socket, sensor, publish);
if(ph->getParam<bool>("i_enable_edge_detection")) {
edgeDetectorNode = pipeline->create<dai::node::EdgeDetector>();
ph->declareParams(edgeDetectorNode);
}
setXinXout(pipeline);
RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str());
}
Mono::~Mono() = default;
void Mono::setNames() {
monoQName = getName() + "_mono";
edgesQName = getName() + "_edges";
controlQName = getName() + "_control";
}

void Mono::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
if(ph->getParam<bool>("i_enable_edge_detection")) {
monoCamNode->out.link(edgeDetectorNode->inputImage);
edgeDetectorNode->setMaxOutputFrameSize(
monoCamNode->getResolutionWidth() *
monoCamNode->getResolutionHeight());
}
if(ph->getParam<bool>("i_publish_topic")) {
xoutMono = pipeline->create<dai::node::XLinkOut>();
xoutMono->setStreamName(monoQName);
Expand All @@ -48,6 +61,17 @@ void Mono::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
} else {
monoCamNode->out.link(xoutMono->input);
}
if(ph->getParam<bool>("i_enable_edge_detection")) {
xoutEdges = pipeline->create<dai::node::XLinkOut>();
xoutEdges->setStreamName(edgesQName);
if(ph->getParam<bool>("i_low_bandwidth")) {
edgesVideoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam<int>("i_low_bandwidth_quality"));
edgeDetectorNode->outputImage.link(edgesVideoEnc->input);
edgesVideoEnc->bitstream.link(xoutEdges->input);
} else {
edgeDetectorNode->outputImage.link(xoutEdges->input);
}
}
}
xinControl = pipeline->create<dai::node::XLinkIn>();
xinControl->setStreamName(controlQName);
Expand All @@ -61,6 +85,10 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
imageConverter =
std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
monoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw");
if(ph->getParam<bool>("i_enable_edge_detection")) {
edgesQ = device->getOutputQueue(edgesQName, ph->getParam<int>("i_max_q_size"), false);
edgesPub = image_transport::create_publisher(getROSNode(), "~/" + getName() + "/image_edges");
}
infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(
getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName());
if(ph->getParam<std::string>("i_calibration_file").empty()) {
Expand All @@ -74,15 +102,24 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
infoManager->loadCameraInfo(ph->getParam<std::string>("i_calibration_file"));
}
if(ph->getParam<bool>("i_low_bandwidth")) {
monoQ->addCallback(std::bind(sensor_helpers::compressedImgCB,
std::placeholders::_1,
std::placeholders::_2,
*imageConverter,
monoPub,
infoManager,
dai::RawImgFrame::Type::GRAY8));
monoQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::compressedImgCB(name, data, *imageConverter, monoPub, infoManager, dai::RawImgFrame::Type::GRAY8);
});

if(ph->getParam<bool>("i_enable_edge_detection")) {
edgesQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::compressedImgCB(name, data, *imageConverter, edgesPub, infoManager, dai::RawImgFrame::Type::GRAY8);
});
}
} else {
monoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, monoPub, infoManager));
monoQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *imageConverter, monoPub, infoManager);
});
if(ph->getParam<bool>("i_enable_edge_detection")) {
edgesQ->addCallback([this](const std::string& name, const std::shared_ptr<dai::ADatatype>& data) {
sensor_helpers::imgCB(name, data, *imageConverter, edgesPub);
});
}
}
}
controlQ = device->getInputQueue(controlQName);
Expand All @@ -91,6 +128,9 @@ void Mono::closeQueues() {
if(ph->getParam<bool>("i_publish_topic")) {
monoQ->close();
}
if(ph->getParam<bool>("i_enable_edge_detection")) {
edgesQ->close();
}
controlQ->close();
}

Expand Down
Loading