Skip to content

Commit

Permalink
update socket names
Browse files Browse the repository at this point in the history
  • Loading branch information
Serafadam committed Aug 3, 2023
1 parent 18c2d46 commit 56d4533
Show file tree
Hide file tree
Showing 13 changed files with 54 additions and 54 deletions.
6 changes: 3 additions & 3 deletions depthai_examples/src/depth_crop_control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,8 @@ int main() {
}

// Properties
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoRight->setResolution(monoRes);
monoLeft->setResolution(monoRes);

Expand Down Expand Up @@ -137,7 +137,7 @@ int main() {

dai::rosBridge::ImageConverter depthConverter(cameraName + "_right_camera_optical_frame", true);
// TODO(sachin): Modify the calibration based on crop from service
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);

dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> depthPublish(depthQueue,
pnh,
Expand Down
10 changes: 5 additions & 5 deletions depthai_examples/src/rgb_stereo_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,9 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con

// MonoCamera
monoLeft->setResolution(monoResolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoRight->setResolution(monoResolution);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);

// StereoDepth
stereo->initialConfig.setConfidenceThreshold(confidence);
Expand All @@ -60,7 +60,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
stereo->setLeftRightCheck(lrcheck);
stereo->setExtendedDisparity(extended);
stereo->setSubpixel(subpixel);
// stereo->setDepthAlign(dai::CameraBoardSocket::RGB);
// stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
// // Link plugins CAM -> STEREO -> XLINK
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
Expand All @@ -70,7 +70,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
auto colorCam = pipeline.create<dai::node::ColorCamera>();
auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
xlinkOut->setStreamName("video");
colorCam->setBoardSocket(dai::CameraBoardSocket::RGB);
colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);

colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
colorCam->setInterleaved(true);
Expand Down Expand Up @@ -123,7 +123,7 @@ int main(int argc, char** argv) {
std::string color_uri = camera_param_uri + "/" + "color.yaml";

dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true);
auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 1280, 720);
auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 1280, 720);

dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> depthPublish(stereoQueue,
pnh,
Expand Down
32 changes: 16 additions & 16 deletions depthai_examples/src/stereo_inertial_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,10 +97,10 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,

// MonoCamera
monoLeft->setResolution(monoResolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoLeft->setFps(stereo_fps);
monoRight->setResolution(monoResolution);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
monoRight->setFps(stereo_fps);

// StereoDepth
Expand All @@ -110,7 +110,7 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
stereo->setLeftRightCheck(lrcheck);
stereo->setExtendedDisparity(extended);
stereo->setSubpixel(subpixel);
if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::RGB);
if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);

// Imu
imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
Expand All @@ -123,7 +123,7 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
auto camRgb = pipeline.create<dai::node::ColorCamera>();
auto xoutRgb = pipeline.create<dai::node::XLinkOut>();
xoutRgb->setStreamName("rgb");
camRgb->setBoardSocket(dai::CameraBoardSocket::RGB);
camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A);
camRgb->setFps(stereo_fps);
dai::node::ColorCamera::Properties::SensorResolution rgbResolution;

Expand Down Expand Up @@ -494,10 +494,10 @@ int main(int argc, char** argv) {
rgbConverter.setUpdateRosBaseTimeOnToRosMsg();
}
if(enableDepth) {
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);

auto depthCameraInfo =
depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height) : rightCameraInfo;
depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo;

auto depthconverter = depth_aligned ? rgbConverter : rightconverter;
dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> depthPublish(
Expand All @@ -515,7 +515,7 @@ int main(int argc, char** argv) {
depthPublish.addPublisherCallback();

if(depth_aligned) {
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height);
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height);
auto imgQueue = device->getOutputQueue("rgb", 30, false);
dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> rgbPublish(
imgQueue,
Expand All @@ -530,7 +530,7 @@ int main(int argc, char** argv) {
if(enableSpatialDetection) {
auto previewQueue = device->getOutputQueue("preview", 30, false);
auto detectionQueue = device->getOutputQueue("detections", 30, false);
auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight);
auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight);

dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> previewPublish(
previewQueue,
Expand All @@ -555,8 +555,8 @@ int main(int argc, char** argv) {

ros::spin();
} else {
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);

auto leftQueue = device->getOutputQueue("left", 30, false);
auto rightQueue = device->getOutputQueue("right", 30, false);
Expand Down Expand Up @@ -584,9 +584,9 @@ int main(int argc, char** argv) {
std::string tfSuffix = depth_aligned ? "_rgb_camera_optical_frame" : "_right_camera_optical_frame";
dai::rosBridge::DisparityConverter dispConverter(tfPrefix + tfSuffix, 880, 7.5, 20, 2000); // TODO(sachin): undo hardcoding of baseline

auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
auto disparityCameraInfo =
depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height) : rightCameraInfo;
depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo;
auto depthconverter = depth_aligned ? rgbConverter : rightconverter;
dai::rosBridge::BridgePublisher<stereo_msgs::DisparityImage, dai::ImgFrame> dispPublish(
stereoQueue,
Expand All @@ -599,7 +599,7 @@ int main(int argc, char** argv) {
dispPublish.addPublisherCallback();

if(depth_aligned) {
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height);
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height);
auto imgQueue = device->getOutputQueue("rgb", 30, false);
dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> rgbPublish(
Expand All @@ -615,7 +615,7 @@ int main(int argc, char** argv) {
if(enableSpatialDetection) {
auto previewQueue = device->getOutputQueue("preview", 30, false);
auto detectionQueue = device->getOutputQueue("detections", 30, false);
auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight);
auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight);

dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> previewPublish(
previewQueue,
Expand All @@ -640,8 +640,8 @@ int main(int argc, char** argv) {

ros::spin();
} else {
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);

auto leftQueue = device->getOutputQueue("left", 30, false);
auto rightQueue = device->getOutputQueue("right", 30, false);
Expand Down
8 changes: 4 additions & 4 deletions depthai_examples/src/stereo_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class StereoNodelet : public nodelet::Nodelet {
}

leftConverter = std::make_unique<dai::rosBridge::ImageConverter>(tfPrefix + "_left_camera_optical_frame", true);
auto leftCameraInfo = leftConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight);
auto leftCameraInfo = leftConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight);

leftPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
leftQueue,
Expand All @@ -108,7 +108,7 @@ class StereoNodelet : public nodelet::Nodelet {
leftPublish->addPublisherCallback();

rightConverter = std::make_unique<dai::rosBridge::ImageConverter>(tfPrefix + "_right_camera_optical_frame", true);
auto rightCameraInfo = rightConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
auto rightCameraInfo = rightConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);

rightPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
rightQueue,
Expand Down Expand Up @@ -187,9 +187,9 @@ class StereoNodelet : public nodelet::Nodelet {

// MonoCamera
monoLeft->setResolution(monoResolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoRight->setResolution(monoResolution);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);

// int maxDisp = 96;
// if (extended) maxDisp *= 2;
Expand Down
8 changes: 4 additions & 4 deletions depthai_examples/src/stereo_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ std::tuple<dai::Pipeline, int, int> createPipeline(

// MonoCamera
monoLeft->setResolution(monoResolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoRight->setResolution(monoResolution);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);

// StereoDepth
stereo->initialConfig.setConfidenceThreshold(confidence);
Expand Down Expand Up @@ -154,7 +154,7 @@ int main(int argc, char** argv) {
}

dai::rosBridge::ImageConverter converter(tfPrefix + "_left_camera_optical_frame", true);
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight);
auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight);
dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> leftPublish(
leftQueue,
pnh,
Expand All @@ -167,7 +167,7 @@ int main(int argc, char** argv) {
leftPublish.addPublisherCallback();

dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);

dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> rightPublish(
rightQueue,
Expand Down
10 changes: 5 additions & 5 deletions depthai_examples/src/yolov4_spatial_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,16 +69,16 @@ dai::Pipeline createPipeline(bool syncNN, bool subpixel, std::string nnPath, int
}

monoLeft->setResolution(monoResolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
monoRight->setResolution(monoResolution);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);

/// setting node configs
stereo->initialConfig.setConfidenceThreshold(confidence);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh);
stereo->setSubpixel(subpixel);
stereo->setDepthAlign(dai::CameraBoardSocket::RGB);
stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);

spatialDetectionNetwork->setBlobPath(nnPath);
spatialDetectionNetwork->setConfidenceThreshold(0.5f);
Expand Down Expand Up @@ -181,7 +181,7 @@ int main(int argc, char** argv) {
}

dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 416, 416);
auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 416, 416);
rgbPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
colorQueue,
pnh,
Expand All @@ -204,7 +204,7 @@ int main(int argc, char** argv) {
30);

dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true);
auto rightCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
auto rightCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
depthPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
depthQueue,
pnh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class Detection : public BaseNode {
imageConverter = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false);
imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(ros::NodeHandle(getROSNode(), getName()), "/" + getName());
infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::RGB, width, height));
infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::CAM_A, width, height));

ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1);
ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,19 +67,19 @@ class SpatialDetection : public BaseNode {
ptImageConverter = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false);
ptImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
ptInfoMan = std::make_shared<camera_info_manager::CameraInfoManager>(ros::NodeHandle(getROSNode(), getName()), "/" + getName());
ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::RGB, width, height));
ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::CAM_A, width, height));

ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1);
ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan));
}

if(ph->getParam<bool>("i_enable_passthrough_depth")) {
dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB;
dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A;
bool align;
getROSNode().getParam("stereo_i_align_depth", align);
if(!align) {
tfPrefix = getTFPrefix("right");
socket = dai::CameraBoardSocket::RIGHT;
socket = dai::CameraBoardSocket::CAM_C;
};
ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam<int>("i_max_q_size"), false);
ptDepthImageConverter = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ class Stereo : public BaseNode {
ros::NodeHandle node,
std::shared_ptr<dai::Pipeline> pipeline,
std::shared_ptr<dai::Device> device,
StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::LEFT},
StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::RIGHT});
StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::CAM_B},
StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::CAM_C});
~Stereo();
void updateParams(parametersConfig& config) override;
void setupQueues(std::shared_ptr<dai::Device> device) override;
Expand Down
Loading

0 comments on commit 56d4533

Please sign in to comment.