forked from luxonis/depthai-ros
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
34 changed files
with
536 additions
and
186 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,141 @@ | ||
|
|
||
| #include <cstdio> | ||
| #include <iostream> | ||
|
|
||
| #include "camera_info_manager/camera_info_manager.hpp" | ||
| #include "depthai_bridge/BridgePublisher.hpp" | ||
| #include "depthai_bridge/ImageConverter.hpp" | ||
| #include "depthai_bridge/ImgDetectionConverter.hpp" | ||
| #include "rclcpp/executors.hpp" | ||
| #include "rclcpp/node.hpp" | ||
| #include "sensor_msgs/msg/image.hpp" | ||
| #include "vision_msgs/msg/detection2_d_array.hpp" | ||
|
|
||
| // Inludes common necessary includes for development using depthai library | ||
| #include "depthai/device/DataQueue.hpp" | ||
| #include "depthai/device/Device.hpp" | ||
| #include "depthai/pipeline/Pipeline.hpp" | ||
| #include "depthai/pipeline/node/ColorCamera.hpp" | ||
| #include "depthai/pipeline/node/DetectionNetwork.hpp" | ||
| #include "depthai/pipeline/node/XLinkOut.hpp" | ||
|
|
||
| const std::vector<std::string> label_map = { | ||
| "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", | ||
| "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", | ||
| "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", | ||
| "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", | ||
| "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", | ||
| "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", | ||
| "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", | ||
| "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", | ||
| "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; | ||
|
|
||
| dai::Pipeline createPipeline(bool syncNN, std::string nnPath) { | ||
| dai::Pipeline pipeline; | ||
| auto colorCam = pipeline.create<dai::node::ColorCamera>(); | ||
| auto detectionNetwork = pipeline.create<dai::node::YoloDetectionNetwork>(); | ||
|
|
||
| // create xlink connections | ||
| auto xoutRgb = pipeline.create<dai::node::XLinkOut>(); | ||
| auto xoutNN = pipeline.create<dai::node::XLinkOut>(); | ||
|
|
||
| xoutRgb->setStreamName("preview"); | ||
| xoutNN->setStreamName("detections"); | ||
|
|
||
| // Properties | ||
| colorCam->setPreviewSize(416, 416); | ||
| colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); | ||
| colorCam->setInterleaved(false); | ||
| colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); | ||
| colorCam->setFps(40); | ||
|
|
||
| // Network specific settings | ||
| detectionNetwork->setConfidenceThreshold(0.5f); | ||
| detectionNetwork->setNumClasses(80); | ||
| detectionNetwork->setCoordinateSize(4); | ||
| detectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); | ||
| detectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); | ||
| detectionNetwork->setIouThreshold(0.5f); | ||
| detectionNetwork->setBlobPath(nnPath); | ||
| detectionNetwork->setNumInferenceThreads(2); | ||
| detectionNetwork->input.setBlocking(false); | ||
|
|
||
| // Linking | ||
| colorCam->preview.link(detectionNetwork->input); | ||
| if(syncNN) | ||
| detectionNetwork->passthrough.link(xoutRgb->input); | ||
| else | ||
| colorCam->preview.link(xoutRgb->input); | ||
|
|
||
| detectionNetwork->out.link(xoutNN->input); | ||
| return pipeline; | ||
| } | ||
|
|
||
| int main(int argc, char** argv) { | ||
| rclcpp::init(argc, argv); | ||
| auto node = rclcpp::Node::make_shared("yolov4_node"); | ||
|
|
||
| std::string tfPrefix, resourceBaseFolder, nnPath; | ||
| std::string camera_param_uri; | ||
| std::string nnName(BLOB_NAME); // Set your blob name for the model here | ||
| bool syncNN; | ||
| std::string monoResolution = "400p"; | ||
|
|
||
| node->declare_parameter("tf_prefix", "oak"); | ||
| node->declare_parameter("camera_param_uri", camera_param_uri); | ||
| node->declare_parameter("sync_nn", true); | ||
| node->declare_parameter("nnName", ""); | ||
| node->declare_parameter("resourceBaseFolder", ""); | ||
|
|
||
| node->get_parameter("tf_prefix", tfPrefix); | ||
| node->get_parameter("camera_param_uri", camera_param_uri); | ||
| node->get_parameter("sync_nn", syncNN); | ||
| node->get_parameter("resourceBaseFolder", resourceBaseFolder); | ||
|
|
||
| if(resourceBaseFolder.empty()) { | ||
| throw std::runtime_error("Send the path to the resouce folder containing NNBlob in \'resourceBaseFolder\' "); | ||
| } | ||
|
|
||
| std::string nnParam; | ||
| node->get_parameter("nnName", nnParam); | ||
| if(nnParam != "x") { | ||
| node->get_parameter("nnName", nnName); | ||
| } | ||
|
|
||
| nnPath = resourceBaseFolder + "/" + nnName; | ||
| dai::Pipeline pipeline = createPipeline(syncNN, nnPath); | ||
| dai::Device device(pipeline); | ||
|
|
||
| auto colorQueue = device.getOutputQueue("preview", 30, false); | ||
| auto detectionQueue = device.getOutputQueue("detections", 30, false); | ||
| auto calibrationHandler = device.readCalibration(); | ||
|
|
||
| dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); | ||
| auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, -1, -1); | ||
| dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(colorQueue, | ||
| node, | ||
| std::string("color/image"), | ||
| std::bind(&dai::rosBridge::ImageConverter::toRosMsg, | ||
| &rgbConverter, // since the converter has the same frame name | ||
| // and image type is also same we can reuse it | ||
| std::placeholders::_1, | ||
| std::placeholders::_2), | ||
| 30, | ||
| rgbCameraInfo, | ||
| "color"); | ||
|
|
||
| dai::rosBridge::ImgDetectionConverter detConverter(tfPrefix + "_rgb_camera_optical_frame", 416, 416, false); | ||
| dai::rosBridge::BridgePublisher<vision_msgs::msg::Detection2DArray, dai::ImgDetections> detectionPublish( | ||
| detectionQueue, | ||
| node, | ||
| std::string("color/yolov4_detections"), | ||
| std::bind(&dai::rosBridge::ImgDetectionConverter::toRosMsg, &detConverter, std::placeholders::_1, std::placeholders::_2), | ||
| 30); | ||
|
|
||
| detectionPublish.addPublisherCallback(); | ||
| rgbPublish.addPublisherCallback(); // addPublisherCallback works only when the dataqueue is non blocking. | ||
|
|
||
| rclcpp::spin(node); | ||
|
|
||
| return 0; | ||
| } |
Oops, something went wrong.