Merge branch 'master' into feature/51_publish_heartbeat

reinzor · reinzor · commit d19705b30a3b · 2018-11-03T15:13:38.000+01:00
diff --git a/.gitignore b/.gitignore
@@ -2,6 +2,7 @@
 __pycache__/
 *.py[cod]
 *$py.class
+CMakeLists.txt.user
 
 # C extensions
 *.so
diff --git a/README.md b/README.md
@@ -15,6 +15,7 @@ Package | Build status Xenial Kinetic x64 | Description
 [openface_ros](https://github.com/tue-robotics/image_recognition/tree/master/openface_ros) | [![Build Status](http://build.ros.org/job/Ksrc_uX__openface_ros__ubuntu_xenial__source/1//badge/icon)](http://build.ros.org/job/Ksrc_uX__openface_ros__ubuntu_xenial__source/1/) | ROS wrapper for Openface (https://github.com/cmusatyalab/openface) to detect and recognize faces in images.
 [skybiometry_ros](https://github.com/tue-robotics/image_recognition/tree/master/skybiometry_ros) | [![Build Status](http://build.ros.org/job/Ksrc_uX__skybiometry_ros__ubuntu_xenial__source/1//badge/icon)](http://build.ros.org/job/Ksrc_uX__skybiometry_ros_ubuntu_xenial__source/1/) | ROS wrapper for Skybiometry (https://skybiometry.com/) for getting face properties of a detected face, e.g. age estimation, gender estimation etc.
 [openpose_ros](https://github.com/tue-robotics/image_recognition/tree/master/openpose_ros) | [![Build Status](http://build.ros.org/job/Ksrc_uX__openpose_ros__ubuntu_xenial__source/1//badge/icon)](http://build.ros.org/job/Ksrc_uX__openpose_ros_ubuntu_xenial__source/1/) | ROS wrapper for Openpose (https://github.com/CMU-Perceptual-Computing-Lab/) for getting poses of 2D images.
+[jetson_inference_ros](https://github.com/tue-robotics/image_recognition/tree/master/jetson_inference_ros) | [![Build Status](http://build.ros.org/job/Ksrc_uX__jetson_inference_ros__ubuntu_xenial__source/1//badge/icon)](http://build.ros.org/job/Ksrc_uX__jetson_inference_ros_ubuntu_xenial__source/1/) | ROS wrapper for Jetson Inference (https://github.com/dusty-nv/jetson-inference) for running inference using TensorRT.
 [keras_ros](https://github.com/tue-robotics/image_recognition/tree/master/keras_ros) | [![Build Status](http://build.ros.org/job/Ksrc_uX__keras_ros__ubuntu_xenial__source/1//badge/icon)](http://build.ros.org/job/Ksrc_uX__keras_ros_ubuntu_xenial__source/1/) | ROS wrappers for Keras neural nets.
 
 ## Travis CI Build Status
diff --git a/image_recognition_util/package.xml b/image_recognition_util/package.xml
@@ -12,7 +12,8 @@
 
   <exec_depend>python-numpy</exec_depend>
   <exec_depend>python-opencv</exec_depend>
+  <exec_depend>rospy</exec_depend>
 
   <test_depend>rostest</test_depend>
 
-</package>
+</package>
diff --git a/image_recognition_util/scripts/publish_image_with_recognitions_overlay b/image_recognition_util/scripts/publish_image_with_recognitions_overlay
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+import message_filters
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from image_recognition_msgs.msg import Recognitions
+from image_recognition_util import image_writer
+
+
+class OverlayPublisher(object):
+
+    def __init__(self):
+        self._bridge = CvBridge()
+
+        # Topics
+        image_sub = message_filters.Subscriber('image', Image, queue_size=1)
+        recognitions_sub = message_filters.Subscriber('recognitions', Recognitions, queue_size=1)
+
+        self._ts = message_filters.TimeSynchronizer([image_sub, recognitions_sub], queue_size=100)
+        # self._ts = message_filters.ApproximateTimeSynchronizer([image_sub, recognitions_sub],
+        #                                                        queue_size=10, slop=1.0)
+        self._ts.registerCallback(self.callback)
+
+	self._result_image_publisher = rospy.Publisher("image_recognitions_overlay", Image, queue_size=10)
+
+    def callback(self, image_msg, recognitions_msg):
+        rospy.loginfo('got image cb')
+
+        cv_image = self._bridge.imgmsg_to_cv2(image_msg, "bgr8")
+	annotated_image = image_writer.get_annotated_cv_image(cv_image, recognitions_msg.recognitions)
+	self._result_image_publisher.publish(self._bridge.cv2_to_imgmsg(annotated_image, "bgr8"))
+
+
+if __name__ == '__main__':
+    rospy.init_node('publish_image_with_recognitions_overlay')
+    OverlayPublisher()
+    rospy.spin()
diff --git a/jetson_inference_ros/CMakeLists.txt b/jetson_inference_ros/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jetson_inference_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  image_recognition_msgs
+  image_transport
+  roscpp
+  sensor_msgs
+)
+
+catkin_package()
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+find_package(jetson-inference)
+if(jetson-inference_FOUND) # If we are on Jetson, use the actual impl, otherwise a mock
+    find_package(CUDA REQUIRED)
+    find_package(Qt4 REQUIRED)
+    include(${QT_USE_FILE})
+    add_definitions(${QT_DEFINITIONS})
+    include_directories(${CUDA_INCLUDE_DIRS})
+    add_library(detect_net src/detect_net.cpp)
+    target_link_libraries(detect_net ${catkin_LIBRARIES} jetson-inference)
+else()
+  message(WARNING "Jetson-inference not found, using a mock instead ...")
+  message(WARNING "The library can be installed using scripts/install_jetson_inference.bash")
+  add_library(detect_net src/detect_net_mock.cpp)
+  target_link_libraries(detect_net ${catkin_LIBRARIES})
+endif()
+add_dependencies(detect_net ${catkin_EXPORTED_TARGETS})
+
+add_executable(detect_net_ros src/detect_net_ros.cpp)
+target_link_libraries(detect_net_ros detect_net)
diff --git a/jetson_inference_ros/README.md b/jetson_inference_ros/README.md
@@ -0,0 +1,22 @@
+jetson_inference_ros
+====================================
+
+ROS Wrapper for Jetson inference https://github.com/dusty-nv/jetson-inference
+
+## Description
+Provides a service and topic interface for jetson inference. For now only the detect nets
+
+### Some illustrations (pednet, bottlenet, facenet)
+
+![Pedestrians](doc/pedestrians.png)
+![Bottles](doc/bottles.png)
+![Faces](doc/faces.png)
+
+## Installation on Jetson TX2
+
+Run the install jetson-inference script
+
+    rosrun jetson_inference_ros install_jetson_inference.bash 
+
+If the jetson-inference cannot be found using CMake, it will compile a mock.
+
diff --git a/jetson_inference_ros/doc/bottles.png b/jetson_inference_ros/doc/bottles.png
diff --git a/jetson_inference_ros/doc/faces.png b/jetson_inference_ros/doc/faces.png
diff --git a/jetson_inference_ros/doc/pedestrians.png b/jetson_inference_ros/doc/pedestrians.png
diff --git a/jetson_inference_ros/launch/static_kinect.launch b/jetson_inference_ros/launch/static_kinect.launch
@@ -0,0 +1,17 @@
+<launch>
+
+  <arg name="networks_path" default="/home/ubuntu/jetson-inference/data/networks" />
+  <arg name="prototxt_path" default="ped-100/deploy.prototxt" />
+  <arg name="model_path" default="ped-100/snapshot_iter_70800.caffemodel" />
+
+  <group ns="camera/rgb">
+    <node pkg="jetson_inference_ros" type="detect_net_ros" name="detect_net" output="screen">
+      <param name="prototxt_path" value="$(arg networks_path)/$(arg prototxt_path)" />
+      <param name="model_path" value="$(arg networks_path)/$(arg model_path)" />
+      <remap from="image" to="image_rect_color" />
+    </node>
+    <node pkg="image_recognition_util" type="publish_image_with_recognitions_overlay" name="overlay">
+      <remap from="image" to="image_rect_color" />
+    </node>
+  </group>
+</launch>
diff --git a/jetson_inference_ros/launch/static_kinect_coco_bottle.launch b/jetson_inference_ros/launch/static_kinect_coco_bottle.launch
@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find jetson_inference_ros)/launch/static_kinect.launch">
+    <arg name="prototxt_path" value="DetectNet-COCO-Bottle/deploy.prototxt" />
+    <arg name="model_path" value="DetectNet-COCO-Bottle/snapshot_iter_59700.caffemodel" />
+  </include> 
+</launch>
diff --git a/jetson_inference_ros/launch/static_kinect_faces.launch b/jetson_inference_ros/launch/static_kinect_faces.launch
@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find jetson_inference_ros)/launch/static_kinect.launch">
+    <arg name="prototxt_path" value="facenet-120/deploy.prototxt" />
+    <arg name="model_path" value="facenet-120/snapshot_iter_24000.caffemodel" />
+  </include> 
+</launch>
diff --git a/jetson_inference_ros/launch/static_kinect_multi_pedestrians.launch b/jetson_inference_ros/launch/static_kinect_multi_pedestrians.launch
@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find jetson_inference_ros)/launch/static_kinect.launch">
+    <arg name="prototxt_path" value="multiped-500/deploy.prototxt" />
+    <arg name="model_path" value="multiped-500/snapshot_iter_178000.caffemodel" />
+  </include> 
+</launch>
diff --git a/jetson_inference_ros/launch/static_kinect_pedestrians.launch b/jetson_inference_ros/launch/static_kinect_pedestrians.launch
@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find jetson_inference_ros)/launch/static_kinect.launch">
+    <arg name="prototxt_path" value="ped-100/deploy.prototxt" />
+    <arg name="model_path" value="ped-100/snapshot_iter_70800.caffemodel" />
+  </include> 
+</launch>
diff --git a/jetson_inference_ros/package.xml b/jetson_inference_ros/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>jetson_inference_ros</name>
+  <version>0.0.0</version>
+  <description>The jetson_inference_ros package</description>
+
+  <maintainer email="reinzor@gmail.com">Rein Appeldoorn</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_recognition_msgs</depend>
+  <depend>image_recognition_util</depend>
+  <depend>image_transport</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+</package>
diff --git a/jetson_inference_ros/scripts/install_jetson_inference.bash b/jetson_inference_ros/scripts/install_jetson_inference.bash
@@ -0,0 +1,41 @@
+#!/bin/bash
+
+BUILD_ROOT=$PWD
+
+echo "[Pre-build]  dependency installer script running..."
+echo "[Pre-build]  build root directory:       $BUILD_ROOT"
+
+function build_jetson-inference {
+    # enter the jetson-inference directory
+    cd ~/jetson-inference
+
+    # create and enter a build directory
+    mkdir -p build
+    cd build
+
+    # build and install the jetson-inference library
+    cmake ../ -DCMAKE_BUILD_TYPE=Release
+    make
+    sudo make install
+}
+
+if [ ! -d ~/jetson-inference ]
+then
+    # clone the jetson-inference repository
+    git clone https://github.com/dusty-nv/jetson-inference ~/jetson-inference
+    build_jetson-inference
+elif cd ~/jetson-inference &&
+    git checkout master -q &&
+    git fetch origin master -q &&
+    [ `git rev-list HEAD...origin/master --count` != 0 ] &&
+    git merge origin/master #If something needed to be updated
+then
+    echo "[Pre-build]  needs update"
+
+    build_jetson-inference
+
+    echo "[Pre-build]  has been updated!"
+else
+    # Show up-to-date message
+    echo "[Pre-build]  up to date"
+fi
diff --git a/jetson_inference_ros/src/detect_net.cpp b/jetson_inference_ros/src/detect_net.cpp
@@ -0,0 +1,106 @@
+#include "detect_net.h"
+
+#include <jetson-inference/detectNet.h>
+
+#include <jetson-inference/cudaMappedMemory.h>
+#include <jetson-inference/loadImage.h>
+#include <jetson-inference/cudaFont.h>
+
+detectNet* g_net = NULL;
+float4* g_gpu_data = NULL;
+float* g_bounding_box_CPU = NULL;
+float* g_bounding_box_CUDA = NULL;
+float* g_confidence_CPU = NULL;
+float* g_confidence_CUDA = NULL;
+uint32_t g_max_boxes;
+uint32_t g_classes;
+uint32_t g_image_width;
+uint32_t g_image_height;
+size_t   g_image_size;
+
+DetectNet::DetectNet(std::string prototxt_path, std::string model_path, int mean_pixel, float threshold)
+{
+  // make sure files exist (and we can read them)
+  if( access(prototxt_path.c_str(), R_OK) )
+  {
+    throw std::runtime_error("Unable to read prototxt file: " + prototxt_path);
+  }
+  if( access(model_path.c_str(), R_OK) )
+  {
+    throw std::runtime_error("Unable to read model path file: " + model_path);
+  }
+
+  // create imageNet
+  g_net = detectNet::Create(prototxt_path.c_str(), model_path.c_str(), mean_pixel, threshold);
+
+  if( !g_net )
+  {
+    throw std::runtime_error("Failed to initialize detectNet");
+  }
+
+  /*
+   * allocate memory for output bounding boxes and class confidence
+   */
+  g_max_boxes = g_net->GetMaxBoundingBoxes();
+  g_classes  = g_net->GetNumClasses();
+
+  if( !cudaAllocMapped((void**)&g_bounding_box_CPU, (void**)&g_bounding_box_CUDA, g_max_boxes * sizeof(float4)) ||
+      !cudaAllocMapped((void**)&g_confidence_CPU, (void**)&g_confidence_CUDA, g_max_boxes * g_classes * sizeof(float)) )
+  {
+    throw std::runtime_error("Failed to alloc output memory");
+  }
+}
+
+DetectNet::~DetectNet()
+{
+  if(g_gpu_data)
+    CUDA(cudaFree(g_gpu_data));
+  delete g_net;
+}
+
+std::vector<image_recognition_msgs::Recognition> DetectNet::processImage(const cv::Mat &cv_im)
+{
+  // allocate GPU data if necessary
+  if (!g_gpu_data)
+  {
+    CUDA(cudaMalloc(&g_gpu_data, cv_im.rows * cv_im.cols * sizeof(float4)));
+  } 
+  else if (g_image_height != cv_im.rows || g_image_width != cv_im.cols)
+  {
+    CUDA(cudaFree(g_gpu_data));
+    CUDA(cudaMalloc(&g_gpu_data, cv_im.rows*cv_im.cols * sizeof(float4)));
+  }
+
+  g_image_height = cv_im.rows;
+  g_image_width = cv_im.cols;
+  g_image_size = cv_im.rows * cv_im.cols * sizeof(float4);
+  float4* cpu_data = (float4*)(cv_im.data);
+
+  // copy to device
+  CUDA(cudaMemcpy(g_gpu_data, cpu_data, g_image_size, cudaMemcpyHostToDevice));
+
+  int number_of_bounding_boxes = g_max_boxes;
+
+  std::vector<image_recognition_msgs::Recognition> recognitions;
+  if (g_net->Detect((float*)g_gpu_data, g_image_width, g_image_height, g_bounding_box_CPU, &number_of_bounding_boxes, g_confidence_CPU)) 
+  {
+    recognitions.resize(number_of_bounding_boxes);
+
+    for(int n = 0; n < number_of_bounding_boxes; n++)
+    {
+      // confidence optional pointer to float2 array filled with a (confidence, class) pair for each bounding box (numBoxes)
+      const float confidence = g_confidence_CPU[n*2];
+      const int label = g_confidence_CPU[n*2+1];
+      float* bounding_box = g_bounding_box_CPU + (n * 4);
+      recognitions[n].roi.x_offset = bounding_box[0];
+      recognitions[n].roi.y_offset = bounding_box[1];
+      recognitions[n].roi.width = bounding_box[2] - bounding_box[0];
+      recognitions[n].roi.height = bounding_box[3] - bounding_box[1];
+      recognitions[n].categorical_distribution.probabilities.resize(1);
+      recognitions[n].categorical_distribution.probabilities[0].label = std::to_string(label);
+      recognitions[n].categorical_distribution.probabilities[0].probability = confidence;
+    }
+  }
+
+  return recognitions;
+}
diff --git a/jetson_inference_ros/src/detect_net.h b/jetson_inference_ros/src/detect_net.h
@@ -0,0 +1,20 @@
+#include <opencv/cv.h>
+#include <vector>
+
+#include <image_recognition_msgs/Recognitions.h>
+
+class DetectNet
+{
+
+public:
+  DetectNet(std::string prototxt_path, std::string model_path, int  mean_pixel, float threshold);
+
+  ~DetectNet();
+
+  std::vector<image_recognition_msgs::Recognition> processImage(const cv::Mat& cv_im);
+
+private:
+  int mean_pixel_;
+  float threshold_;
+
+};
diff --git a/jetson_inference_ros/src/detect_net_mock.cpp b/jetson_inference_ros/src/detect_net_mock.cpp
@@ -0,0 +1,14 @@
+#include "detect_net.h"
+
+DetectNet::DetectNet(std::string prototxt_path, std::string model_path, int mean_pixel, float threshold)
+{
+}
+
+DetectNet::~DetectNet()
+{
+}
+
+std::vector<image_recognition_msgs::Recognition> DetectNet::processImage(const cv::Mat &cv_im)
+{
+  return std::vector<image_recognition_msgs::Recognition>();
+}
diff --git a/jetson_inference_ros/src/detect_net_ros.cpp b/jetson_inference_ros/src/detect_net_ros.cpp
diff --git a/tensorflow_ros/src/tensorflow_ros/object_recognizer.py b/tensorflow_ros/src/tensorflow_ros/object_recognizer.py
diff --git a/tensorflow_ros/src/tensorflow_ros/tf_retrain.py b/tensorflow_ros/src/tensorflow_ros/tf_retrain.py
