Merge pull request leggedrobotics#287 from leggedrobotics/feature/nod…

…eletize

Feature/nodeletize

darknet_ros/CMakeLists.txt

@@ -58,6 +58,7 @@ find_package(catkin REQUIRED
     actionlib
     darknet_ros_msgs
     image_transport
+    nodelet
 )
 
 # Enable OPENCV in darknet
@@ -77,6 +78,7 @@ catkin_package(
     std_msgs
     darknet_ros_msgs
     image_transport
+    nodelet
   DEPENDS
     Boost
 )
@@ -158,6 +160,10 @@ if (CUDA_FOUND)
     src/yolo_object_detector_node.cpp
   )
 
+  cuda_add_library(${PROJECT_NAME}_nodelet
+    src/yolo_object_detector_nodelet.cpp
+  )
+
 else()
 
   add_library(${PROJECT_NAME}_lib
@@ -168,6 +174,10 @@ else()
     src/yolo_object_detector_node.cpp
   )
 
+  add_library(${PROJECT_NAME}_nodelet
+    src/yolo_object_detector_nodelet.cpp
+  )
+
 endif()
 
 target_link_libraries(${PROJECT_NAME}_lib
@@ -184,6 +194,10 @@ target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_lib
 )
 
+target_link_libraries(${PROJECT_NAME}_nodelet
+  ${PROJECT_NAME}_lib
+)
+
 add_dependencies(${PROJECT_NAME}_lib
   darknet_ros_msgs_generate_messages_cpp
 )

darknet_ros/launch/darknet_ros_nodelet.launch

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<launch>
+  <!-- Console launch prefix -->
+  <arg name="launch_prefix" default=""/>
+
+  <!-- Config and weights folder. -->
+  <arg name="yolo_weights_path"          default="$(find darknet_ros)/yolo_network_config/weights"/>
+  <arg name="yolo_config_path"           default="$(find darknet_ros)/yolo_network_config/cfg"/>
+
+  <!-- ROS parameter files -->
+  <arg name="ros_param_file"             default="$(find darknet_ros)/config/ros.yaml"/>
+  <arg name="network_param_file"         default="$(find darknet_ros)/config/yolov2-tiny.yaml"/>
+
+  <!-- Load parameters -->
+  <rosparam command="load" ns="darknet_ros_nodelet" file="$(arg ros_param_file)"/>
+  <rosparam command="load" ns="darknet_ros_nodelet" file="$(arg network_param_file)"/>
+
+  <!-- Darknet and ros wrapper nodelet manager -->
+  <node name="darknet_ros_nodelet_manager" type="nodelet" pkg="nodelet" args="manager" output="screen"/>
+
+  <!-- Start darknet and ros wrapper -->
+  <node pkg="nodelet" type="nodelet" name="darknet_ros_nodelet" output="screen" launch-prefix="$(arg launch_prefix)" args="load darknet_ros_nodelet darknet_ros_nodelet_manager">
+    <param name="weights_path"          value="$(arg yolo_weights_path)" />
+    <param name="config_path"           value="$(arg yolo_config_path)" />
+  </node>
+
+
+ <!--<node name="republish" type="republish" pkg="image_transport" output="screen"      args="compressed in:=/front_camera/image_raw raw out:=/camera/image_raw" /> -->
+</launch>

darknet_ros/nodelet_plugins.xml

@@ -0,0 +1,8 @@
+<library path="lib/libdarknet_ros_nodelet">
+
+  <!-- Darknet Ros Plugin -->
+  <class name="darknet_ros_nodelet" type="DarknetRosNodelet" base_class_type="nodelet::Nodelet">
+    <description>Darknet Ros Nodelet.</description>
+  </class>
+
+</library>

darknet_ros/package.xml

@@ -24,8 +24,14 @@
   <depend>message_generation</depend>
   <depend>darknet_ros_msgs</depend>
   <depend>actionlib</depend>
+  <depend>nodelet</depend>
 
   <!-- Test dependencies -->
   <test_depend>rostest</test_depend>
   <test_depend>wget</test_depend>
+
+  <!-- Export nodelet plugins -->
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>  
 </package>

darknet_ros/src/yolo_object_detector_node.cpp

@@ -1,5 +1,5 @@
 /*
- * yolo_obstacle_detector_node.cpp
+ * yolo_object_detector_node.cpp
  *
  *  Created on: Dec 19, 2016
  *      Author: Marko Bjelonic

darknet_ros/src/yolo_object_detector_nodelet.cpp

@@ -0,0 +1,29 @@
+/*
+ *   Author: Timon Homberger
+ *   Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <darknet_ros/YoloObjectDetector.hpp>
+
+class DarknetRosNodelet : public nodelet::Nodelet {
+ public:
+  DarknetRosNodelet() = default;
+  ~DarknetRosNodelet() {
+    if (darknetRos_) delete darknetRos_;
+  }
+
+ private:
+  virtual void onInit() {
+    ros::NodeHandle NodeHandle("~");
+    NodeHandle = getPrivateNodeHandle();
+    darknetRos_ = new darknet_ros::YoloObjectDetector(NodeHandle);
+  }
+
+  darknet_ros::YoloObjectDetector* darknetRos_;
+};
+
+// Declare as a Plug-in
+PLUGINLIB_EXPORT_CLASS(DarknetRosNodelet, nodelet::Nodelet);