diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0b5c504 --- /dev/null +++ b/.gitignore @@ -0,0 +1,12 @@ +config/yolo-v3/* +!config/yolo-v3/params.yaml +config/yolo-v3-tiny/* +!config/yolo-v3-tiny/params.yaml +config/yolo-v4/* +!config/yolo-v4/params.yaml +config/yolo-v4-tiny/* +!config/yolo-v4-tiny/params.yaml +config/yolo-v7/* +!config/yolo-v7/params.yaml +config/yolo-v7-tiny/* +!config/yolo-v7-tiny/params.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 93da6cb..a902ad0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.10) project(openrobotics_darknet_ros) +option(DOWNLOAD_YOLO_CONFIG "Download YOLO configuration files" OFF) + # Default to C++14 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -12,7 +14,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) -find_package(darknet_vendor REQUIRED) +find_package(Darknet REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) @@ -25,9 +27,11 @@ target_include_directories(openrobotics_darknet_ros PUBLIC include) target_compile_definitions(openrobotics_darknet_ros PRIVATE "DARKNET_ROS_BUILDING_DLL") ament_target_dependencies(openrobotics_darknet_ros cv_bridge - darknet_vendor sensor_msgs vision_msgs) +target_link_libraries(openrobotics_darknet_ros Darknet::dark) +# Necessary as Darknet currently does not install to ./install/darknet/libdarknet.so +set_target_properties(openrobotics_darknet_ros PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE) add_library(detector_node SHARED src/detector_node.cpp) @@ -70,6 +74,13 @@ install(TARGETS detector_node install(TARGETS detector_node_main DESTINATION lib/${PROJECT_NAME}) +# Download weights for Yolo v3, v4 and v7 +if (DOWNLOAD_YOLO_CONFIG) + include(ConfigDownload.cmake) +endif() + +install(DIRECTORY config/ launch/ DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) install(DIRECTORY include/ DESTINATION include) ament_package() diff --git a/ConfigDownload.cmake b/ConfigDownload.cmake new file mode 100644 index 0000000..0550abc --- /dev/null +++ b/ConfigDownload.cmake @@ -0,0 +1,105 @@ +# Download weights for Yolo v3, v4 and v7 + +# Yolo v3 +# See https://pjreddie.com/darknet/yolo/ +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD +https://github.com/AlexeyAB/darknet/blob/master/cfg/yolov3.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3/yolov3.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://pjreddie.com/media/files/yolov3.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3/yolov3.weights + SHOW_PROGRESS +) +# Yolo v3 tiny +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3-tiny/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/cfg/yolov3-tiny.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3-tiny/yolov3-tiny.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://pjreddie.com/media/files/yolov3-tiny.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v3-tiny/yolov3-tiny.weights + SHOW_PROGRESS +) + +# Yolo v4 +# See https://github.com/AlexeyAB/darknet#pre-trained-models +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD +https://raw.githubusercontent.com/AlexeyAB/darknet/master/cfg/yolov4.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4/yolov4.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4/yolov4.weights + SHOW_PROGRESS +) +# Yolo v4 tiny +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4-tiny/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD + https://raw.githubusercontent.com/AlexeyAB/darknet/master/cfg/yolov4-tiny.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4-tiny/yolov4-tiny.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v4_pre/yolov4-tiny.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v4-tiny/yolov4-tiny.weights + SHOW_PROGRESS +) + +# Yolo v7 +# See https://github.com/AlexeyAB/darknet/issues/8595 +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD + https://raw.githubusercontent.com/AlexeyAB/darknet/master/cfg/yolov7.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7/yolov7.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://github.com/AlexeyAB/darknet/releases/download/yolov4/yolov7.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7/yolov7.weights + SHOW_PROGRESS +) +# Yolo v7 tiny +file(DOWNLOAD + https://raw.githubusercontent.com/pjreddie/darknet/master/data/coco.names + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7-tiny/coco.names + SHOW_PROGRESS +) +file(DOWNLOAD +https://raw.githubusercontent.com/AlexeyAB/darknet/master/cfg/yolov7-tiny.cfg + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7-tiny/yolov7-tiny.cfg + SHOW_PROGRESS +) +file(DOWNLOAD + https://github.com/AlexeyAB/darknet/releases/download/yolov4/yolov7-tiny.weights + ${CMAKE_CURRENT_BINARY_DIR}/config/yolo-v7-tiny/yolov7-tiny.weights + SHOW_PROGRESS +) + +install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/config/ DESTINATION share/${PROJECT_NAME}/config/) diff --git a/README.md b/README.md index 840e2fc..3bce561 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,12 @@ # Open Robotics Darknet ROS -This is a ROS 2 wrapper around [darknet](https://pjreddie.com/darknet), an open source neural network framework. +This is a **ROS 2 wrapper around [darknet](https://pjreddie.com/darknet)**, an open source neural network framework. ![Example image with bounding boxes created using darknet and the yolov3-tiny network](doc/example_darknet_yolov3-tiny.png) ## DetectorNode -This node can run object detectors like [YOLOv3](https://pjreddie.com/darknet/yolo/) on images. +This node can run **object detectors** like [YOLO v3](https://pjreddie.com/darknet/yolo/) or [YOLO v7](https://github.com/WongKinYiu/yolov7) on images and video streams. ### Subscribers @@ -24,35 +24,51 @@ This node can run object detectors like [YOLOv3](https://pjreddie.com/darknet/yo * `detection.threshold` - Minimum probability of a detection to be published * `detection.nms_threshold` - Non-maximal Suppression threshold - controls filtering of overlapping boxes -### Example +## Dependencies -Download `YOLOv3-tiny`. +This package depends on [darknet](https://github.com/AlexeyAB/darknet). If you can't use CUDA but want to use your CPU instead make sure to build it with the flag `-DENABLE_CUDA=OFF` and potentially also disabling multi-threading with `-DCMAKE_DISABLE_FIND_PACKAGE_OpenMP=TRUE`. +### Launching + +Compiling this package with + +```bash +$ colcon build --cmake-args -DDOWNLOAD_YOLO_CONFIG=ON ``` -wget https://raw.githubusercontent.com/pjreddie/darknet/f86901f6177dfc6116360a13cc06ab680e0c86b0/cfg/yolov3-tiny.cfg -wget https://pjreddie.com/media/files/yolov3-tiny.weights -wget https://raw.githubusercontent.com/pjreddie/darknet/c6afc7ff1499fbbe64069e1843d7929bd7ae2eaa/data/coco.names + +will automatically download the pretrained YOLO v3, v4 and v7 configuration files. + +You can then launch the detector node with + +```bash +$ ros2 launch openrobotics_darknet_ros detector_launch.py rgb_image:=/topic ``` -Save the following as `detector_node_params.yaml` +optionally supplying a desired parameter file `detector_parameters:=path/to/detector_node_params.yaml`. + +You can also train YOLO to detect custom objects like described [here](https://github.com/AlexeyAB/darknet#how-to-train-tiny-yolo-to-detect-your-custom-objects) and create the following as `detector_node_params.yaml`: ```yaml /**: ros__parameters: network: - config: "./yolov3-tiny.cfg" - weights: "./yolov3-tiny.weights" - class_names: "./coco.names" + config: "./your-yolo-config.cfg" + weights: "./your-yolo-weights.weights" + class_names: "./your-cocos.names" detection: threshold: 0.25 nms_threshold: 0.45 ``` -Then run the node. +Finally you can run the detector node with +```bash +$ ros2 run openrobotics_darknet_ros detector_node --ros-args --params-file path/to/detector_node_params.yaml ``` -ros2 run openrobotics_darknet_ros detector_node __params:=detector_node_params.yaml + +and publish images on `~/images` to get the node to detect objects. You can also manually remap an external topic to the `~/images` topic with: + +```bash +$ ros2 run openrobotics_darknet_ros detector_node --ros-args --params-file path/to/detector_node_params.yaml -r '~/images:=/your/camera/topic' ``` -The node is now running. -Publish images on `~/images` to get the node to detect objects. diff --git a/config/yolo-v3-tiny/params.yaml b/config/yolo-v3-tiny/params.yaml new file mode 100644 index 0000000..b175dfd --- /dev/null +++ b/config/yolo-v3-tiny/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3-tiny/yolov3-tiny.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3-tiny/yolov3-tiny.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3-tiny/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/config/yolo-v3/params.yaml b/config/yolo-v3/params.yaml new file mode 100644 index 0000000..1589c16 --- /dev/null +++ b/config/yolo-v3/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3/yolov3.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3/yolov3.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v3/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/config/yolo-v4-tiny/params.yaml b/config/yolo-v4-tiny/params.yaml new file mode 100644 index 0000000..86df909 --- /dev/null +++ b/config/yolo-v4-tiny/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4-tiny/yolov4-tiny.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4-tiny/yolov4-tiny.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4-tiny/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/config/yolo-v4/params.yaml b/config/yolo-v4/params.yaml new file mode 100644 index 0000000..8a7b01e --- /dev/null +++ b/config/yolo-v4/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4/yolov4.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4/yolov4.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v4/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/config/yolo-v7-tiny/params.yaml b/config/yolo-v7-tiny/params.yaml new file mode 100644 index 0000000..c831c60 --- /dev/null +++ b/config/yolo-v7-tiny/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7-tiny/yolov7-tiny.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7-tiny/yolov7-tiny.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7-tiny/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/config/yolo-v7/params.yaml b/config/yolo-v7/params.yaml new file mode 100644 index 0000000..f0cd171 --- /dev/null +++ b/config/yolo-v7/params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + network: + config: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7/yolov7.cfg" + weights: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7/yolov7.weights" + class_names: "$(find-pkg-share openrobotics_darknet_ros)/config/yolo-v7/coco.names" + detection: + threshold: 0.25 + nms_threshold: 0.45 diff --git a/include/openrobotics_darknet_ros/detector_node.hpp b/include/openrobotics_darknet_ros/detector_node.hpp index 9fb2cd6..57cb7a3 100644 --- a/include/openrobotics_darknet_ros/detector_node.hpp +++ b/include/openrobotics_darknet_ros/detector_node.hpp @@ -19,6 +19,7 @@ #include #include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "openrobotics_darknet_ros/visibility_node.hpp" @@ -41,6 +42,7 @@ class DetectorNode : public rclcpp::Node private: std::unique_ptr impl_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; } // namespace darknet_ros } // namespace openrobotics diff --git a/launch/detector_launch.py b/launch/detector_launch.py new file mode 100644 index 0000000..d7268c5 --- /dev/null +++ b/launch/detector_launch.py @@ -0,0 +1,52 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile + + +def generate_launch_description(): + detector_parameters_file = LaunchConfiguration('detector_parameters') + rgb_image_topic = LaunchConfiguration('rgb_image') + detections_topic = LaunchConfiguration('detections') + + default_detector_parameter_file = os.path.join( + get_package_share_directory('openrobotics_darknet_ros'), + 'yolo-v7-tiny', + 'params.yaml' + ) + detector_parameters_cmd = DeclareLaunchArgument( + 'detector_parameters', + default_value=default_detector_parameter_file, + description='YOLO object detection configuration file' + ) + rgb_image_cmd = DeclareLaunchArgument( + 'rgb_image', + description='RGB image that should be used for object detection with YOLO' + ) + detections_topic_cmd = DeclareLaunchArgument( + 'detections', + default_value='~/detections', + description='Detections containing the bounding boxes of objects to be considered' + ) + + yolo_detector_node = Node( + package='openrobotics_darknet_ros', + executable='detector_node', + name='detector_node', + remappings=[ + ('~/images', rgb_image_topic), + ('~/detections', detections_topic) + ], + parameters = [ParameterFile(detector_parameters_file, allow_substs=True)] + ) + + ld = LaunchDescription() + ld.add_action(detector_parameters_cmd) + ld.add_action(rgb_image_cmd) + ld.add_action(detections_topic_cmd) + ld.add_action(yolo_detector_node) + return ld diff --git a/package.xml b/package.xml index 8615d45..86734fa 100644 --- a/package.xml +++ b/package.xml @@ -12,7 +12,7 @@ ament_cmake cv_bridge - darknet_vendor + darknet rclcpp rclcpp_components sensor_msgs diff --git a/src/darknet_detections.hpp b/src/darknet_detections.hpp index fc89ff5..40c8a28 100644 --- a/src/darknet_detections.hpp +++ b/src/darknet_detections.hpp @@ -15,7 +15,7 @@ #ifndef DARKNET_DETECTIONS_HPP_ #define DARKNET_DETECTIONS_HPP_ -#include +#include namespace openrobotics { diff --git a/src/darknet_image.hpp b/src/darknet_image.hpp index 0fc6ccb..f65c1d5 100644 --- a/src/darknet_image.hpp +++ b/src/darknet_image.hpp @@ -15,7 +15,7 @@ #ifndef DARKNET_IMAGE_HPP_ #define DARKNET_IMAGE_HPP_ -#include +#include #include #include diff --git a/src/detector_network.cpp b/src/detector_network.cpp index 867e8ff..dd6ef01 100644 --- a/src/detector_network.cpp +++ b/src/detector_network.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -36,7 +36,7 @@ class DetectorNetworkPrivate ~DetectorNetworkPrivate() { if (network_) { - free_network(network_); + free_network_ptr(network_); network_ = nullptr; } } @@ -113,7 +113,7 @@ DetectorNetwork::detect( letterbox_image(orig_image.image_, impl_->network_->w, impl_->network_->h)); // Ask network to make predictions - network_predict(impl_->network_, resized_image.image_.data); + network_predict_ptr(impl_->network_, resized_image.image_.data); // Get predictions from network int num_detections = 0; @@ -121,10 +121,11 @@ DetectorNetwork::detect( const float hier = 0; int * map = nullptr; const int relative = 0; + static const int letter = 1; detection * darknet_detections = get_network_boxes( impl_->network_, image_msg.width, image_msg.height, threshold, hier, map, relative, - &num_detections); + &num_detections, letter); if (num_detections <= 0) { return 0; @@ -151,8 +152,8 @@ DetectorNetwork::detect( if (detection.prob[cls] > 0.0f) { detection_ros.results.emplace_back(); auto & hypothesis = detection_ros.results.back(); - hypothesis.id = impl_->class_names_.at(cls); - hypothesis.score = detection.prob[cls]; + hypothesis.hypothesis.class_id = impl_->class_names_.at(cls); + hypothesis.hypothesis.score = detection.prob[cls]; } } @@ -163,8 +164,8 @@ DetectorNetwork::detect( } // Copy bounding box, darknet uses center of bounding box too - detection_ros.bbox.center.x = detection.bbox.x; - detection_ros.bbox.center.y = detection.bbox.y; + detection_ros.bbox.center.position.x = detection.bbox.x; + detection_ros.bbox.center.position.y = detection.bbox.y; detection_ros.bbox.size_x = detection.bbox.w; detection_ros.bbox.size_y = detection.bbox.h; } diff --git a/src/detector_node.cpp b/src/detector_node.cpp index 1ba195e..4fadd21 100644 --- a/src/detector_node.cpp +++ b/src/detector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -94,30 +94,26 @@ DetectorNode::DetectorNode(rclcpp::NodeOptions options) network_cfg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; network_cfg_desc.read_only = true; network_cfg_desc.name = "network.config"; - const std::string network_config_path = declare_parameter( - network_cfg_desc.name, - rclcpp::ParameterValue(), - network_cfg_desc).get(); + network_cfg_desc.dynamic_typing = true; + const std::string network_config_path = declare_parameter( + network_cfg_desc.name); rcl_interfaces::msg::ParameterDescriptor network_weights_desc; network_weights_desc.description = "Path to file describing network weights"; network_weights_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; network_weights_desc.read_only = true; network_weights_desc.name = "network.weights"; - const std::string network_weights_path = declare_parameter( - network_weights_desc.name, - rclcpp::ParameterValue(), - network_weights_desc).get(); + const std::string network_weights_path = declare_parameter( + network_weights_desc.name); rcl_interfaces::msg::ParameterDescriptor network_class_names_desc; network_class_names_desc.description = "Path to file with class names (one per line)"; network_class_names_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; network_class_names_desc.read_only = true; network_class_names_desc.name = "network.class_names"; - const std::string network_class_names_path = declare_parameter( - network_class_names_desc.name, - rclcpp::ParameterValue(), - network_class_names_desc).get(); + network_class_names_desc.dynamic_typing = true; + const std::string network_class_names_path = declare_parameter( + network_class_names_desc.name); impl_->threshold_desc_.description = "Minimum detection confidence [0.0, 1.0]"; impl_->threshold_desc_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; @@ -136,7 +132,7 @@ DetectorNode::DetectorNode(rclcpp::NodeOptions options) rclcpp::ParameterValue(impl_->nms_threshold_), impl_->nms_threshold_desc_).get(); - set_on_parameters_set_callback( + param_callback_handle_ = add_on_set_parameters_callback( std::bind(&DetectorNodePrivate::on_parameters_change, &*impl_, std::placeholders::_1)); // TODO(sloretz) raise if user tried to initialize node with undeclared parameters