diff --git a/data/images/bicycle.bmp b/data/images/bicycle.bmp new file mode 100644 index 0000000..ef3b1d5 Binary files /dev/null and b/data/images/bicycle.bmp differ diff --git a/data/images/bicycle.jpeg b/data/images/bicycle.jpeg new file mode 100644 index 0000000..2ead001 Binary files /dev/null and b/data/images/bicycle.jpeg differ diff --git a/data/images/bicycle.png b/data/images/bicycle.png new file mode 100644 index 0000000..4386086 Binary files /dev/null and b/data/images/bicycle.png differ diff --git a/movidius_ncs_example/CMakeLists.txt b/movidius_ncs_example/CMakeLists.txt index 7d850b1..a282c07 100644 --- a/movidius_ncs_example/CMakeLists.txt +++ b/movidius_ncs_example/CMakeLists.txt @@ -1,11 +1,11 @@ # Copyright (c) 2017 Intel Corporation -# +# # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -42,13 +42,15 @@ add_executable(${PROJECT_NAME}_image_classification src/image_classification.cpp add_executable(${PROJECT_NAME}_image_detection src/image_detection.cpp) add_executable(${PROJECT_NAME}_stream_classification src/stream_classification.cpp) add_executable(${PROJECT_NAME}_stream_detection src/stream_detection.cpp) +add_executable(${PROJECT_NAME}_single_device_classification src/single_device_classification.cpp) +add_executable(${PROJECT_NAME}_multiple_devices_classification src/multiple_devices_classification.cpp) add_dependencies(${PROJECT_NAME}_image_classification - ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_image_detection - ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_stream_classification @@ -59,6 +61,14 @@ add_dependencies(${PROJECT_NAME}_stream_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_single_device_classification + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) + +add_dependencies(${PROJECT_NAME}_multiple_devices_classification + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${PROJECT_NAME}_image_classification ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} @@ -78,6 +88,17 @@ target_link_libraries(${PROJECT_NAME}_stream_detection ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) + +target_link_libraries(${PROJECT_NAME}_single_device_classification + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_multiple_devices_classification + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + # Flags if(UNIX OR APPLE) # Linker flags. @@ -119,7 +140,8 @@ if(UNIX OR APPLE) endif() install(TARGETS ${PROJECT_NAME}_image_classification ${PROJECT_NAME}_image_detection - ${PROJECT_NAME}_stream_classification ${PROJECT_NAME}_stream_detection + ${PROJECT_NAME}_stream_classification ${PROJECT_NAME}_stream_detection ${PROJECT_NAME}_single_device_classification + ${PROJECT_NAME}_multiple_devices_classification ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/movidius_ncs_example/src/image_classification.cpp b/movidius_ncs_example/src/image_classification.cpp index 8b0070c..b0df16e 100644 --- a/movidius_ncs_example/src/image_classification.cpp +++ b/movidius_ncs_example/src/image_classification.cpp @@ -16,9 +16,48 @@ #include #include + #include #include +#include +#include + +#include +#include + +#define LINESPACING 30 + +std::vector getImagePath(std::string image_dir) +{ + if (image_dir.back() != '/') + { + image_dir += "/"; + } + + std::vector files; + + DIR* dir; + struct dirent* ptr; + + if ((dir = opendir(image_dir.c_str())) == NULL) + { + perror("Open Dir error..."); + exit(1); + } + + while ((ptr = readdir(dir)) != NULL) + { + if (strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0) + continue; + else if (ptr->d_type == 8) + files.push_back(image_dir + ptr->d_name); + } + closedir(dir); + + return files; +} + int main(int argc, char** argv) { ros::init(argc, argv, "movidius_ncs_example"); @@ -29,17 +68,16 @@ int main(int argc, char** argv) return -1; } + std::vector images_path = getImagePath(*(argv + 1)); + ros::NodeHandle n; ros::ServiceClient client; client = n.serviceClient("/movidius_ncs_image/classify_object"); object_msgs::ClassifyObject srv; - cv_bridge::CvImage cv_image; - sensor_msgs::Image ros_image; - cv_image.image = cv::imread(argv[1]); - cv_image.encoding = "bgr8"; - cv_image.toImageMsg(ros_image); - srv.request.image = ros_image; + srv.request.images_path = images_path; + + boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time(); if (!client.call(srv)) { @@ -47,13 +85,35 @@ int main(int argc, char** argv) return 1; } - for (unsigned int i = 0; i < srv.response.objects.objects_vector.size(); i++) + boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration msdiff = end - start; + + for (unsigned int i = 0; i < srv.response.objects.size(); i++) { - ROS_INFO("%d: object: %s\nprobability: %lf%%", i, - srv.response.objects.objects_vector[i].object_name.c_str(), - srv.response.objects.objects_vector[i].probability * 100); + cv_bridge::CvImage cv_image; + cv_image.image = cv::imread(images_path[i]); + cv_image.encoding = "bgr8"; + int cnt = 0; + + ROS_INFO("Classification result for image No.%u:", i + 1); + for (unsigned int j = 0; j < srv.response.objects[i].objects_vector.size(); j++) + { + std::stringstream ss; + ss << srv.response.objects[i].objects_vector[j].object_name << ": " + << srv.response.objects[i].objects_vector[j].probability * 100 << "%"; + + ROS_INFO("%d: object: %s\nprobability: %lf%%", j, srv.response.objects[i].objects_vector[j].object_name.c_str(), + srv.response.objects[i].objects_vector[j].probability * 100); + + cv::putText(cv_image.image, ss.str(), cvPoint(LINESPACING, LINESPACING * (++cnt)), cv::FONT_HERSHEY_SIMPLEX, 0.5, + cv::Scalar(0, 255, 0), 1); + } + + cv::imshow("image_classification", cv_image.image); + cv::waitKey(0); } - ROS_INFO("inference time: %fms", srv.response.objects.inference_time_ms); + ROS_INFO("inference %lu images during %ld ms", srv.response.objects.size(), msdiff.total_milliseconds()); + return 0; } diff --git a/movidius_ncs_example/src/image_detection.cpp b/movidius_ncs_example/src/image_detection.cpp index 4ca2990..5648a81 100644 --- a/movidius_ncs_example/src/image_detection.cpp +++ b/movidius_ncs_example/src/image_detection.cpp @@ -13,6 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ + #include #include @@ -21,6 +22,42 @@ #include +#include +#include + +#include +#include + +std::vector getImagePath(std::string image_dir) +{ + if (image_dir.back() != '/') + { + image_dir += "/"; + } + + std::vector files; + + DIR* dir; + struct dirent* ptr; + + if ((dir = opendir(image_dir.c_str())) == NULL) + { + perror("Open Dir error..."); + exit(1); + } + + while ((ptr = readdir(dir)) != NULL) + { + if (strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0) + continue; + else if (ptr->d_type == 8) + files.push_back(image_dir + ptr->d_name); + } + closedir(dir); + + return files; +} + int main(int argc, char** argv) { ros::init(argc, argv, "movidius_ncs_example"); @@ -31,20 +68,16 @@ int main(int argc, char** argv) return -1; } + std::vector images_path = getImagePath(*(argv + 1)); + ros::NodeHandle n; ros::ServiceClient client; client = n.serviceClient("/movidius_ncs_image/detect_object"); object_msgs::DetectObject srv; - cv_bridge::CvImage cv_image; - sensor_msgs::Image ros_image; - cv_image.image = cv::imread(argv[1]); - cv_image.encoding = "bgr8"; - cv_image.toImageMsg(ros_image); - int width = cv_image.image.cols; - int height = cv_image.image.rows; + srv.request.images_path = images_path; - srv.request.image = ros_image; + boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time(); if (!client.call(srv)) { @@ -52,37 +85,53 @@ int main(int argc, char** argv) return 1; } - for (unsigned int i = 0; i < srv.response.objects.objects_vector.size(); i++) + boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration msdiff = end - start; + + for (unsigned int i = 0; i < srv.response.objects.size(); i++) { - std::stringstream ss; - ss << srv.response.objects.objects_vector[i].object.object_name << ": " - << srv.response.objects.objects_vector[i].object.probability * 100 << "%"; - - ROS_INFO("%d: object: %s", i, srv.response.objects.objects_vector[i].object.object_name.c_str()); - ROS_INFO("prob: %f", srv.response.objects.objects_vector[i].object.probability); - ROS_INFO("location: (%d, %d, %d, %d)", - srv.response.objects.objects_vector[i].roi.x_offset, - srv.response.objects.objects_vector[i].roi.y_offset, - srv.response.objects.objects_vector[i].roi.width, - srv.response.objects.objects_vector[i].roi.height); - - int xmin = srv.response.objects.objects_vector[i].roi.x_offset; - int ymin = srv.response.objects.objects_vector[i].roi.y_offset; - int w = srv.response.objects.objects_vector[i].roi.width; - int h = srv.response.objects.objects_vector[i].roi.height; - - int xmax = ((xmin + w) < width)? (xmin + w) : width; - int ymax = ((ymin + h) < height)? (ymin + h) : height; - - cv::Point left_top = cv::Point(xmin, ymin); - cv::Point right_bottom = cv::Point(xmax, ymax); - cv::rectangle(cv_image.image, left_top, right_bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_8, 0); - cv::rectangle(cv_image.image, cvPoint(xmin, ymin), cvPoint(xmax, ymin + 20), cv::Scalar(0, 255, 0), -1); - cv::putText(cv_image.image, ss.str(), cvPoint(xmin + 5, ymin + 20), cv::FONT_HERSHEY_PLAIN, - 1, cv::Scalar(0, 0, 255), 1); - } - ROS_INFO("inference time: %fms", srv.response.objects.inference_time_ms); + cv_bridge::CvImage cv_image; + cv_image.image = cv::imread(images_path[i]); + cv_image.encoding = "bgr8"; + int width = cv_image.image.cols; + int height = cv_image.image.rows; + + ROS_INFO("Detection result for image No.%u:", i + 1); + + for (unsigned int j = 0; j < srv.response.objects[i].objects_vector.size(); j++) + { + std::stringstream ss; + ss << srv.response.objects[i].objects_vector[j].object.object_name << ": " + << srv.response.objects[i].objects_vector[j].object.probability * 100 << "%"; + + ROS_INFO("%d: object: %s", j, srv.response.objects[i].objects_vector[j].object.object_name.c_str()); + ROS_INFO("prob: %f", srv.response.objects[i].objects_vector[j].object.probability); + ROS_INFO("location: (%d, %d, %d, %d)", srv.response.objects[i].objects_vector[j].roi.x_offset, + srv.response.objects[i].objects_vector[j].roi.y_offset, + srv.response.objects[i].objects_vector[j].roi.width, + srv.response.objects[i].objects_vector[j].roi.height); + + int xmin = srv.response.objects[i].objects_vector[j].roi.x_offset; + int ymin = srv.response.objects[i].objects_vector[j].roi.y_offset; + int w = srv.response.objects[i].objects_vector[j].roi.width; + int h = srv.response.objects[i].objects_vector[j].roi.height; + + int xmax = ((xmin + w) < width) ? (xmin + w) : width; + int ymax = ((ymin + h) < height) ? (ymin + h) : height; + + cv::Point left_top = cv::Point(xmin, ymin); + cv::Point right_bottom = cv::Point(xmax, ymax); + cv::rectangle(cv_image.image, left_top, right_bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_8, 0); + cv::rectangle(cv_image.image, cvPoint(xmin, ymin), cvPoint(xmax, ymin + 20), cv::Scalar(0, 255, 0), -1); + cv::putText(cv_image.image, ss.str(), cvPoint(xmin + 5, ymin + 20), cv::FONT_HERSHEY_PLAIN, 1, + cv::Scalar(0, 0, 255), 1); + } + cv::imshow("image_detection", cv_image.image); cv::waitKey(0); - return 0; + } + + ROS_INFO("inference %lu images during %ld ms", srv.response.objects.size(), msdiff.total_milliseconds()); + + return 0; } diff --git a/movidius_ncs_example/src/multiple_devices_classification.cpp b/movidius_ncs_example/src/multiple_devices_classification.cpp new file mode 100644 index 0000000..fd30fcc --- /dev/null +++ b/movidius_ncs_example/src/multiple_devices_classification.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#define LINESPACING 30 +#define MOVEWINDOW 1000 +#define DEFAULT_PARALLEL_SIZE 2 +#define DEFAULT_IMAGE_BASE_PATH "/opt/movidius/ncappzoo/data/images/" + +std::vector getImagePath(std::string image_dir) +{ + if (image_dir.back() != '/') + { + image_dir += "/"; + } + + std::vector files; + + DIR* dir; + struct dirent* ptr; + + if ((dir = opendir(image_dir.c_str())) == NULL) + { + perror("Open Dir error..."); + exit(1); + } + + while ((ptr = readdir(dir)) != NULL) + { + if (strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0) + continue; + else if (ptr->d_type == 8) + files.push_back(image_dir + ptr->d_name); + } + closedir(dir); + + return files; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "movidius_ncs_example_multiple_devices_classification"); + + /* + if (argc != 2) + { + ROS_INFO("Usage: rosrun movidius_ncs_example movidius_ncs_example_multiple_device_demo "); + return -1; + } + + std::vector images_path = getImagePath(argv[1]); + */ + + ros::NodeHandle n; + std::string image_base_path = DEFAULT_IMAGE_BASE_PATH; + if (!n.getParam("image_base_path", image_base_path)) + { + ROS_WARN("param image_base_path not set, use default"); + } + ROS_INFO_STREAM("use image_base_path = " << image_base_path << " for multiple devices demo"); + + std::vector images_path = getImagePath(image_base_path); + + ros::ServiceClient client_for_multiple; + client_for_multiple = n.serviceClient("/movidius_ncs_image_multiple/classify_object"); + + int parallel_size = DEFAULT_PARALLEL_SIZE; + if (!n.getParam("parallel_size", parallel_size)) + { + ROS_WARN("param parallel_size not set, use default"); + } + ROS_INFO_STREAM("use parallel_size = " << parallel_size << " for multiple devices demo"); + + while(1) + { + object_msgs::ClassifyObject srv_for_multiple; + + std::vector random_index; + for(int i = 0; i < parallel_size; i++) + { + std::random_device rd; + std::default_random_engine engine(rd()); + std::uniform_int_distribution<> dis(0, images_path.size() - 1); + auto dice = std::bind(dis, engine); + random_index.push_back(dice()); + + srv_for_multiple.request.images_path.push_back(images_path[dice()]); + } + + if (!client_for_multiple.call(srv_for_multiple)) + { + ROS_ERROR("failed to call service ClassifyObject"); + exit(1); + } + + for (unsigned int i = 0; i < srv_for_multiple.response.objects.size(); i++) + { + cv_bridge::CvImage cv_image; + cv_image.image = cv::imread(images_path[random_index[i]]); + cv_image.encoding = "bgr8"; + int cnt = 0; + + for (unsigned int j = 0; j < srv_for_multiple.response.objects[i].objects_vector.size(); j++) + { + std::stringstream ss; + ss << srv_for_multiple.response.objects[i].objects_vector[j].object_name << ": " + << srv_for_multiple.response.objects[i].objects_vector[j].probability * 100 << "%"; + + cv::putText(cv_image.image, ss.str(), cvPoint(LINESPACING, LINESPACING * (++cnt)), cv::FONT_HERSHEY_SIMPLEX, + 0.5, cv::Scalar(0, 255, 0), 1); + } + cv::namedWindow("image classification with multiple devices"); + cv::moveWindow("image classification with multiple devices", MOVEWINDOW, 0); + cv::imshow("image classification with multiple devices", cv_image.image); + cv::waitKey(20); + } + } + return 0; +} diff --git a/movidius_ncs_example/src/single_device_classification.cpp b/movidius_ncs_example/src/single_device_classification.cpp new file mode 100644 index 0000000..8b4e077 --- /dev/null +++ b/movidius_ncs_example/src/single_device_classification.cpp @@ -0,0 +1,127 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#define LINESPACING 30 +#define DEFAULT_PARALLEL_SIZE 1 +#define DEFAULT_IMAGE_BASE_PATH "/opt/movidius/ncappzoo/data/images/" + +std::vector getImagePath(std::string image_dir) +{ + if (image_dir.back() != '/') + { + image_dir += "/"; + } + + std::vector files; + + DIR* dir; + struct dirent* ptr; + + if ((dir = opendir(image_dir.c_str())) == NULL) + { + perror("Open Dir error..."); + exit(1); + } + + while ((ptr = readdir(dir)) != NULL) + { + if (strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0) + continue; + else if (ptr->d_type == 8) + files.push_back(image_dir + ptr->d_name); + } + closedir(dir); + + return files; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "movidius_ncs_example_single_device_classification"); + ros::NodeHandle n; + std::string image_base_path = DEFAULT_IMAGE_BASE_PATH; + if (!n.getParam("image_base_path", image_base_path)) + { + ROS_WARN("param image_base_path not set, use default"); + } + ROS_INFO_STREAM("use image_base_path = " << image_base_path << " for single device demo"); + + std::vector images_path = getImagePath(image_base_path); + + ros::ServiceClient client_for_single; + client_for_single = n.serviceClient("/movidius_ncs_image_single/classify_object"); + + int parallel_size = DEFAULT_PARALLEL_SIZE; + if (!n.getParam("parallel_size", parallel_size)) + { + ROS_WARN("param parallel_size not set, use default"); + } + ROS_INFO_STREAM("use parallel_size = " << parallel_size << " for single device demo"); + + while(1) + { + object_msgs::ClassifyObject srv_for_single; + + int random_index; + std::random_device rd; + std::default_random_engine engine(rd()); + std::uniform_int_distribution<> dis(0, images_path.size() - 1); + auto dice = std::bind(dis, engine); + random_index = dice(); + + srv_for_single.request.images_path.push_back(images_path[random_index]); + + if (!client_for_single.call(srv_for_single)) + { + ROS_ERROR("failed to call service ClassifyObject"); + exit(1); + } + + for (unsigned int i = 0; i < srv_for_single.response.objects.size(); i++) + { + cv_bridge::CvImage cv_image; + cv_image.image = cv::imread(images_path[dice()]); + cv_image.encoding = "bgr8"; + int cnt = 0; + + for (unsigned int j = 0; j < srv_for_single.response.objects[i].objects_vector.size(); j++) + { + std::stringstream ss; + ss << srv_for_single.response.objects[i].objects_vector[j].object_name << ": " + << srv_for_single.response.objects[i].objects_vector[j].probability * 100 << "%"; + + cv::putText(cv_image.image, ss.str(), cvPoint(LINESPACING, LINESPACING * (++cnt)), cv::FONT_HERSHEY_SIMPLEX, + 0.5, cv::Scalar(0, 255, 0), 1); + } + cv::imshow("image classification with single device", cv_image.image); + cv::waitKey(20); + } + } + + return 0; +} diff --git a/movidius_ncs_example/src/stream_classification.cpp b/movidius_ncs_example/src/stream_classification.cpp index f2edb1b..a7fb563 100644 --- a/movidius_ncs_example/src/stream_classification.cpp +++ b/movidius_ncs_example/src/stream_classification.cpp @@ -25,10 +25,32 @@ #include #include +#include + #define LINESPACING 50 -void syncCb(const sensor_msgs::ImageConstPtr& img, - const object_msgs::Objects::ConstPtr& objs) +int getFPS() +{ + static int FPS = 0; + static boost::posix_time::ptime lastTime = boost::posix_time::microsec_clock::local_time(); + static int frameCount = 0; + + frameCount++; + + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration msdiff = currentTime - lastTime; + + if (msdiff.total_milliseconds() > 1000) + { + FPS = frameCount; + frameCount = 0; + lastTime = currentTime; + } + + return FPS; +} + +void syncCb(const sensor_msgs::ImageConstPtr& img, const object_msgs::Objects::ConstPtr& objs) { cv::Mat cvImage = cv_bridge::toCvShare(img, "bgr8")->image; int cnt = 0; @@ -37,23 +59,18 @@ void syncCb(const sensor_msgs::ImageConstPtr& img, { std::stringstream ss; ss << obj.object_name << ": " << obj.probability * 100 << '%'; - cv::putText(cvImage, - ss.str(), - cvPoint(LINESPACING, LINESPACING * (++cnt)), - cv::FONT_HERSHEY_SIMPLEX, - 1, + cv::putText(cvImage, ss.str(), cvPoint(LINESPACING, LINESPACING * (++cnt)), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0)); } std::stringstream ss; - ss << "inference time: " << objs->inference_time_ms << " ms"; - cv::putText(cvImage, - ss.str(), - cvPoint(LINESPACING, LINESPACING * (++cnt)), - cv::FONT_HERSHEY_SIMPLEX, - 1, + int FPS = getFPS(); + ss << "FPS: " << FPS; + + cv::putText(cvImage, ss.str(), cvPoint(LINESPACING, LINESPACING * (++cnt)), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0)); cv::imshow("image_viewer", cvImage); + int key = cv::waitKey(5); if (key == 13 || key == 27 || key == 32 || key == 113) { @@ -61,19 +78,17 @@ void syncCb(const sensor_msgs::ImageConstPtr& img, } } - int main(int argc, char** argv) { ros::init(argc, argv, "movidius_ncs_example_stream"); ros::NodeHandle nh; - message_filters::Subscriber camSub(nh, - "/camera/color/image_raw", - 1); - message_filters::Subscriber objSub(nh, - "/movidius_ncs_nodelet/classified_objects", - 1); + + message_filters::Subscriber camSub(nh, "/camera/color/image_raw", 1); + message_filters::Subscriber objSub(nh, "/movidius_ncs_nodelet/classified_objects", 1); + message_filters::TimeSynchronizer sync(camSub, objSub, 60); sync.registerCallback(boost::bind(&syncCb, _1, _2)); + ros::spin(); return 0; } diff --git a/movidius_ncs_example/src/stream_detection.cpp b/movidius_ncs_example/src/stream_detection.cpp index b3a5f80..ee015fd 100644 --- a/movidius_ncs_example/src/stream_detection.cpp +++ b/movidius_ncs_example/src/stream_detection.cpp @@ -25,10 +25,32 @@ #include #include +#include + #define LINESPACING 20 -void syncCb(const sensor_msgs::ImageConstPtr& img, - const object_msgs::ObjectsInBoxes::ConstPtr& objs_in_boxes) +int getFPS() +{ + static int FPS = 0; + static boost::posix_time::ptime lastTime = boost::posix_time::microsec_clock::local_time(); + static int frameCount = 0; + + frameCount++; + + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration msdiff = currentTime - lastTime; + + if (msdiff.total_milliseconds() > 1000) + { + FPS = frameCount; + frameCount = 0; + lastTime = currentTime; + } + + return FPS; +} + +void syncCb(const sensor_msgs::ImageConstPtr& img, const object_msgs::ObjectsInBoxes::ConstPtr& objs_in_boxes) { cv::Mat cvImage = cv_bridge::toCvShare(img, "bgr8")->image; int width = img->width; @@ -44,44 +66,40 @@ void syncCb(const sensor_msgs::ImageConstPtr& img, int w = obj.roi.width; int h = obj.roi.height; - int xmax = ((xmin + w) < width)? (xmin + w) : width; - int ymax = ((ymin + h) < height)? (ymin + h) : height; + int xmax = ((xmin + w) < width) ? (xmin + w) : width; + int ymax = ((ymin + h) < height) ? (ymin + h) : height; cv::Point left_top = cv::Point(xmin, ymin); cv::Point right_bottom = cv::Point(xmax, ymax); cv::rectangle(cvImage, left_top, right_bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_8, 0); cv::rectangle(cvImage, cvPoint(xmin, ymin), cvPoint(xmax, ymin + 20), cv::Scalar(0, 255, 0), -1); - cv::putText(cvImage, ss.str(), cvPoint(xmin + 5, ymin + 20), cv::FONT_HERSHEY_PLAIN, - 1, cv::Scalar(0, 0, 255), 1); + cv::putText(cvImage, ss.str(), cvPoint(xmin + 5, ymin + 20), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 255), 1); } std::stringstream ss; - ss << "inference time: " << objs_in_boxes->inference_time_ms << " ms"; - cv::putText(cvImage, ss.str(), cvPoint(LINESPACING, LINESPACING), - cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0)); + int FPS = getFPS(); + ss << "FPS: " << FPS; + + cv::putText(cvImage, ss.str(), cvPoint(LINESPACING, LINESPACING), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0)); cv::imshow("image_viewer", cvImage); + int key = cv::waitKey(5); - if ( key == 13 || key == 27 || key == 32 || key == 113) + if (key == 13 || key == 27 || key == 32 || key == 113) { ros::shutdown(); } } - int main(int argc, char** argv) { ros::init(argc, argv, "movidius_ncs_example_stream"); ros::NodeHandle nh; - message_filters::Subscriber camSub(nh, - "/camera/color/image_raw", - 1); - message_filters::Subscriber objSub(nh, - "/movidius_ncs_nodelet/detected_objects", - 1); - message_filters::TimeSynchronizer sync(camSub, - objSub, - 60); + + message_filters::Subscriber camSub(nh, "/camera/color/image_raw", 1); + message_filters::Subscriber objSub(nh, "/movidius_ncs_nodelet/detected_objects", 1); + message_filters::TimeSynchronizer sync(camSub, objSub, 60); sync.registerCallback(boost::bind(&syncCb, _1, _2)); + ros::spin(); return 0; } diff --git a/movidius_ncs_image/CMakeLists.txt b/movidius_ncs_image/CMakeLists.txt index 2e36820..0a1805b 100644 --- a/movidius_ncs_image/CMakeLists.txt +++ b/movidius_ncs_image/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS image_transport cv_bridge roslint + rostest ) catkin_package( @@ -87,6 +88,7 @@ if(UNIX OR APPLE) set( CUDA_PROPAGATE_HOST_FLAGS OFF ) set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") + endif() install( @@ -104,4 +106,15 @@ if(CATKIN_ENABLE_TESTING) find_package(roslint REQUIRED) roslint_cpp() roslint_add_test() + + # rostest classification + add_executable(unittest_image_classification tests/unittest_image_classification.cpp) + target_link_libraries(unittest_image_classification ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UNITEST_LIBRARIES}) + add_dependencies(unittest_image_classification ${catkin_EXPORTED_TARGETS}) + # rostest detection + add_executable(unittest_image_detection tests/unittest_image_detection.cpp) + target_link_libraries(unittest_image_detection ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UNITEST_LIBRARIES}) + add_dependencies(unittest_image_detection ${catkin_EXPORTED_TARGETS}) + + add_rostest(tests/test_unit.test) endif() diff --git a/movidius_ncs_image/include/movidius_ncs_image/ncs_server.h b/movidius_ncs_image/include/movidius_ncs_image/ncs_server.h index 84a954a..e2f4b70 100644 --- a/movidius_ncs_image/include/movidius_ncs_image/ncs_server.h +++ b/movidius_ncs_image/include/movidius_ncs_image/ncs_server.h @@ -25,6 +25,7 @@ #include #include #include "movidius_ncs_lib/ncs.h" +#include "movidius_ncs_lib/ncs_manager.h" namespace movidius_ncs_image { @@ -37,15 +38,15 @@ class NCSServer void getParameters(); void init(); - bool cbClassifyObject(object_msgs::ClassifyObject::Request& request, - object_msgs::ClassifyObject::Response& response); - bool cbDetectObject(object_msgs::DetectObject::Request& request, - object_msgs::DetectObject::Response& response); + bool cbDetectObject(object_msgs::DetectObject::Request& request, object_msgs::DetectObject::Response& response); + bool cbClassifyObject(object_msgs::ClassifyObject::Request& request, object_msgs::ClassifyObject::Response& response); + ros::ServiceServer service_; - std::shared_ptr ncs_handle_; + std::shared_ptr ncs_manager_handle_; ros::NodeHandle nh_; + int max_device_number_; int device_index_; int log_level_; std::string cnn_type_; diff --git a/movidius_ncs_image/package.xml b/movidius_ncs_image/package.xml index 09ac5e0..f087073 100644 --- a/movidius_ncs_image/package.xml +++ b/movidius_ncs_image/package.xml @@ -36,6 +36,7 @@ limitations under the License. object_msgs movidius_ncs_lib roslint + rostest roscpp sensor_msgs diff --git a/movidius_ncs_image/src/ncs_server.cpp b/movidius_ncs_image/src/ncs_server.cpp index 7cc186d..6a85918 100644 --- a/movidius_ncs_image/src/ncs_server.cpp +++ b/movidius_ncs_image/src/ncs_server.cpp @@ -31,17 +31,18 @@ using movidius_ncs_lib::Device; namespace movidius_ncs_image { NCSServer::NCSServer(ros::NodeHandle& nh) - : ncs_handle_(nullptr), - nh_(nh), - device_index_(0), - log_level_(Device::Errors), - cnn_type_(""), - graph_file_path_(""), - category_file_path_(""), - network_dimension_(0), - mean_(0), - scale_(1.0), - top_n_(1) + : ncs_manager_handle_(nullptr) + , nh_(nh) + , max_device_number_(255) + , device_index_(0) + , log_level_(Device::Errors) + , cnn_type_("") + , graph_file_path_("") + , category_file_path_("") + , network_dimension_(0) + , mean_(0) + , scale_(1.0) + , top_n_(1) { getParameters(); init(); @@ -51,6 +52,19 @@ void NCSServer::getParameters() { ROS_DEBUG("NCSServer get parameters"); + if (!nh_.getParam("max_device_number", max_device_number_)) + { + ROS_WARN("param max_device_number not set, use default"); + } + + if (max_device_number_ <= 0) + { + ROS_ERROR_STREAM("invalid param max_device_number = " << max_device_number_); + throw std::exception(); + } + + ROS_INFO_STREAM("use max_device_number = " << max_device_number_); + if (!nh_.getParam("device_index", device_index_)) { ROS_WARN("param device_index not set, use default"); @@ -82,11 +96,10 @@ void NCSServer::getParameters() ROS_WARN("param cnn_type not set, use default"); } - if (cnn_type_.compare("alexnet") && cnn_type_.compare("googlenet") - && cnn_type_.compare("inception_v1") && cnn_type_.compare("inception_v2") - && cnn_type_.compare("inception_v3") && cnn_type_.compare("inception_v4") - && cnn_type_.compare("mobilenet") && cnn_type_.compare("squeezenet") - && cnn_type_.compare("tinyyolo_v1") && cnn_type_.compare("mobilenetssd")) + if (cnn_type_.compare("alexnet") && cnn_type_.compare("googlenet") && cnn_type_.compare("inception_v1") && + cnn_type_.compare("inception_v2") && cnn_type_.compare("inception_v3") && cnn_type_.compare("inception_v4") && + cnn_type_.compare("mobilenet") && cnn_type_.compare("squeezenet") && cnn_type_.compare("tinyyolo_v1") && + cnn_type_.compare("mobilenetssd")) { ROS_WARN_STREAM("invalid cnn_type_=" << cnn_type_); throw std::exception(); @@ -174,98 +187,86 @@ void NCSServer::getParameters() ROS_INFO_STREAM("use scale = " << scale_); } - void NCSServer::init() { ROS_DEBUG("NCSServer init"); - ncs_handle_ = std::make_shared(device_index_, - static_cast(log_level_), - cnn_type_, - graph_file_path_, - category_file_path_, - network_dimension_, - mean_, - scale_, - top_n_); - if (!cnn_type_.compare("alexnet") || !cnn_type_.compare("googlenet") - || !cnn_type_.compare("inception_v1") || !cnn_type_.compare("inception_v2") - || !cnn_type_.compare("inception_v3") || !cnn_type_.compare("inception_v4") - || !cnn_type_.compare("mobilenet") || !cnn_type_.compare("squeezenet")) + ncs_manager_handle_ = std::make_shared( + max_device_number_, device_index_, static_cast(log_level_), cnn_type_, graph_file_path_, + category_file_path_, network_dimension_, mean_, scale_, top_n_); + + if (!cnn_type_.compare("alexnet") || !cnn_type_.compare("googlenet") || !cnn_type_.compare("inception_v1") || + !cnn_type_.compare("inception_v2") || !cnn_type_.compare("inception_v3") || !cnn_type_.compare("inception_v4") || + !cnn_type_.compare("mobilenet") || !cnn_type_.compare("squeezenet")) { - service_ = nh_.advertiseService("classify_object", - &NCSServer::cbClassifyObject, - this); + service_ = nh_.advertiseService("classify_object", &NCSServer::cbClassifyObject, this); } else { - service_ = nh_.advertiseService("detect_object", - &NCSServer::cbDetectObject, - this); + service_ = nh_.advertiseService("detect_object", &NCSServer::cbDetectObject, this); } } bool NCSServer::cbClassifyObject(object_msgs::ClassifyObject::Request& request, object_msgs::ClassifyObject::Response& response) { - cv::Mat imageData = cv_bridge::toCvCopy(request.image, "bgr8")->image; - ncs_handle_->loadTensor(imageData); - ncs_handle_->classify(); - ClassificationResultPtr result = ncs_handle_->getClassificationResult(); + std::vector results = ncs_manager_handle_->classify_image(request.images_path); - if (result == nullptr) + for (unsigned int i = 0; i < results.size(); i++) { - return false; - } + object_msgs::Objects objs; + for (auto item : results[i]->items) + { + object_msgs::Object obj; + obj.object_name = item.category; + obj.probability = item.probability; + objs.objects_vector.push_back(obj); + } - for (auto item : result->items) - { - object_msgs::Object obj; - obj.object_name = item.category; - obj.probability = item.probability; - response.objects.objects_vector.push_back(obj); + objs.inference_time_ms = results[i]->time_taken; + response.objects.push_back(objs); } - response.objects.inference_time_ms = result->time_taken; return true; } bool NCSServer::cbDetectObject(object_msgs::DetectObject::Request& request, object_msgs::DetectObject::Response& response) { - cv::Mat imageData = cv_bridge::toCvCopy(request.image, "bgr8")->image; - ncs_handle_->loadTensor(imageData); - ncs_handle_->detect(); - DetectionResultPtr result = ncs_handle_->getDetectionResult(); + std::vector results = ncs_manager_handle_->detect_image(request.images_path); - if (result == nullptr) + for (unsigned int i = 0; i < results.size(); i++) { - return false; - } + object_msgs::ObjectsInBoxes objs; + for (auto item : results[i]->items_in_boxes) + { + object_msgs::ObjectInBox obj; + obj.object.object_name = item.item.category; + obj.object.probability = item.item.probability; + obj.roi.x_offset = item.bbox.x; + obj.roi.y_offset = item.bbox.y; + obj.roi.width = item.bbox.width; + obj.roi.height = item.bbox.height; + objs.objects_vector.push_back(obj); + } - for (auto item : result->items_in_boxes) - { - object_msgs::ObjectInBox obj; - obj.object.object_name = item.item.category; - obj.object.probability = item.item.probability; - obj.roi.x_offset = item.bbox.x; - obj.roi.y_offset = item.bbox.y; - obj.roi.width = item.bbox.width; - obj.roi.height = item.bbox.height; - response.objects.objects_vector.push_back(obj); + objs.inference_time_ms = results[i]->time_taken; + response.objects.push_back(objs); } - response.objects.inference_time_ms = result->time_taken; return true; } + } // namespace movidius_ncs_image int main(int argc, char** argv) { ros::init(argc, argv, "movidius_ncs_node"); + ROS_INFO("before try"); ros::NodeHandle nh("~"); try { + ROS_INFO("inside try"); movidius_ncs_image::NCSServer node(nh); ros::spin(); } diff --git a/movidius_ncs_image/tests/test_unit.test b/movidius_ncs_image/tests/test_unit.test new file mode 100644 index 0000000..8629f3c --- /dev/null +++ b/movidius_ncs_image/tests/test_unit.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/movidius_ncs_image/tests/unittest_image_classification.cpp b/movidius_ncs_image/tests/unittest_image_classification.cpp new file mode 100644 index 0000000..d3e6627 --- /dev/null +++ b/movidius_ncs_image/tests/unittest_image_classification.cpp @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include + +TEST(UnitTestClassification, testImage) +{ + ros::NodeHandle n; + ros::ServiceClient client; + client = n.serviceClient("/movidius_ncs_image/classify_object"); + object_msgs::ClassifyObject srv; + + cv_bridge::CvImage cv_image; + sensor_msgs::Image ros_image; + cv_image.image = cv::imread(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle.jpg"); + cv_image.encoding = "bgr8"; + cv_image.toImageMsg(ros_image); + srv.request.image = ros_image; + srv.request.images_path.push_back(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle.jpg"); + + client.waitForExistence(ros::Duration(60)); + EXPECT_TRUE(client.call(srv)); + EXPECT_TRUE(srv.response.objects[0].objects_vector.size()); + EXPECT_TRUE(srv.response.objects[0].objects_vector[0].object_name.find("cycle") != std::string::npos); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "movidius_ncs_image_tests"); + return RUN_ALL_TESTS(); +} diff --git a/movidius_ncs_image/tests/unittest_image_detection.cpp b/movidius_ncs_image/tests/unittest_image_detection.cpp new file mode 100644 index 0000000..072d521 --- /dev/null +++ b/movidius_ncs_image/tests/unittest_image_detection.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +TEST(UnitTestDetection, testImage) +{ + ros::NodeHandle n; + ros::ServiceClient client = n.serviceClient("/movidius_ncs_image/detect_object"); + object_msgs::DetectObject srv; + + cv_bridge::CvImage cv_image; + sensor_msgs::Image ros_image; + std::vector image_format = {".jpg", ".jpeg", ".png", ".bmp"}; + for (std::string suffix : image_format) + { + cv_image.image = cv::imread(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle"+suffix); + cv_image.encoding = "bgr8"; + cv_image.toImageMsg(ros_image); + + srv.request.image = ros_image; + srv.request.images_path.push_back(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle"+suffix); + + client.waitForExistence(ros::Duration(60)); + EXPECT_TRUE(client.call(srv)); + EXPECT_TRUE(srv.response.objects[0].objects_vector.size()); + EXPECT_EQ(srv.response.objects[0].objects_vector[0].object.object_name, "bicycle"); + EXPECT_TRUE(srv.response.objects[0].objects_vector[0].roi.x_offset > 130 && + srv.response.objects[0].objects_vector[0].roi.x_offset < 150 && + srv.response.objects[0].objects_vector[0].roi.y_offset > 90 && + srv.response.objects[0].objects_vector[0].roi.y_offset < 110 && + srv.response.objects[0].objects_vector[0].roi.width > 410 && + srv.response.objects[0].objects_vector[0].roi.width < 470 && + srv.response.objects[0].objects_vector[0].roi.height > 340 && + srv.response.objects[0].objects_vector[0].roi.height < 360); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "movidius_ncs_image_tests"); + return RUN_ALL_TESTS(); +} diff --git a/movidius_ncs_launch/launch/includes/multiple_ncs_demo.launch b/movidius_ncs_launch/launch/includes/multiple_ncs_demo.launch new file mode 100644 index 0000000..f25dcbb --- /dev/null +++ b/movidius_ncs_launch/launch/includes/multiple_ncs_demo.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/movidius_ncs_launch/launch/includes/ncs_image.launch b/movidius_ncs_launch/launch/includes/ncs_image.launch index bb7d200..af41123 100644 --- a/movidius_ncs_launch/launch/includes/ncs_image.launch +++ b/movidius_ncs_launch/launch/includes/ncs_image.launch @@ -1,7 +1,7 @@ - + + @@ -40,9 +41,9 @@ limitations under the License. - + @@ -50,4 +51,3 @@ limitations under the License. - diff --git a/movidius_ncs_launch/launch/includes/ncs_stream_classification.launch b/movidius_ncs_launch/launch/includes/ncs_stream_classification.launch index 895302f..4a656a5 100644 --- a/movidius_ncs_launch/launch/includes/ncs_stream_classification.launch +++ b/movidius_ncs_launch/launch/includes/ncs_stream_classification.launch @@ -18,6 +18,7 @@ limitations under the License. + @@ -29,6 +30,7 @@ limitations under the License. output="screen"> + diff --git a/movidius_ncs_launch/launch/includes/ncs_stream_detection.launch b/movidius_ncs_launch/launch/includes/ncs_stream_detection.launch index 7f98fe2..faba2b1 100644 --- a/movidius_ncs_launch/launch/includes/ncs_stream_detection.launch +++ b/movidius_ncs_launch/launch/includes/ncs_stream_detection.launch @@ -18,6 +18,7 @@ limitations under the License. + @@ -29,6 +30,7 @@ limitations under the License. output="screen"> + diff --git a/movidius_ncs_launch/launch/includes/single_ncs_demo.launch b/movidius_ncs_launch/launch/includes/single_ncs_demo.launch new file mode 100644 index 0000000..2386483 --- /dev/null +++ b/movidius_ncs_launch/launch/includes/single_ncs_demo.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/movidius_ncs_launch/launch/ncs_camera.launch b/movidius_ncs_launch/launch/ncs_camera.launch index 1ed1a77..fbf4cad 100644 --- a/movidius_ncs_launch/launch/ncs_camera.launch +++ b/movidius_ncs_launch/launch/ncs_camera.launch @@ -17,10 +17,10 @@ limitations under the License. - + - - + + - + - + - + @@ -63,18 +64,18 @@ limitations under the License. - + - + - + @@ -88,6 +89,7 @@ limitations under the License. + @@ -102,6 +104,7 @@ limitations under the License. + diff --git a/movidius_ncs_launch/launch/ncs_image_parallel_example.launch b/movidius_ncs_launch/launch/ncs_image_parallel_example.launch new file mode 100644 index 0000000..c302f63 --- /dev/null +++ b/movidius_ncs_launch/launch/ncs_image_parallel_example.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + diff --git a/movidius_ncs_lib/CMakeLists.txt b/movidius_ncs_lib/CMakeLists.txt index 5a009ba..a003f63 100644 --- a/movidius_ncs_lib/CMakeLists.txt +++ b/movidius_ncs_lib/CMakeLists.txt @@ -18,6 +18,7 @@ project(movidius_ncs_lib) find_package(catkin REQUIRED COMPONENTS roscpp roslint + rostest ) catkin_package( @@ -51,6 +52,7 @@ add_library(${PROJECT_NAME} src/graph.cpp src/tensor.cpp src/result.cpp + src/ncs_manager.cpp ) add_dependencies(${PROJECT_NAME} @@ -106,6 +108,8 @@ if(UNIX OR APPLE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mf16c") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") + # Add OpenMP support + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") endif() # Install nodelet library @@ -122,6 +126,18 @@ install(DIRECTORY include/${PROJECT_NAME}/ if(CATKIN_ENABLE_TESTING) find_package(roslint REQUIRED) + find_package(OpenCV REQUIRED) + find_package(catkin REQUIRED COMPONENTS roslib) roslint_cpp() roslint_add_test() + + # rostest NCS SDK environment + add_executable(unittest_environment tests/unittest_environment.cpp) + target_link_libraries(unittest_environment ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UNITEST_LIBRARIES}) + # rostest library function + add_executable(unittest_function tests/unittest_function.cpp) + target_link_libraries(unittest_function ${PROJECT_NAME} ${OpenCV_LIBS} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UNITEST_LIBRARIES}) + add_dependencies(unittest_function ${catkin_EXPORTED_TARGETS}) + + add_rostest(tests/test_all.test) endif() diff --git a/movidius_ncs_lib/include/movidius_ncs_lib/ncs_manager.h b/movidius_ncs_lib/include/movidius_ncs_lib/ncs_manager.h new file mode 100644 index 0000000..f3d30a0 --- /dev/null +++ b/movidius_ncs_lib/include/movidius_ncs_lib/ncs_manager.h @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MOVIDIUS_NCS_LIB_NCS_MANAGER_H +#define MOVIDIUS_NCS_LIB_NCS_MANAGER_H + +#include +#include +#include +#include +#include +#include +#include + +#include "boost/bind.hpp" +#include "boost/function.hpp" + +#include "movidius_ncs_lib/device.h" +#include "movidius_ncs_lib/ncs.h" + +namespace movidius_ncs_lib +{ +typedef boost::function FUNP_C; +typedef boost::function FUNP_D; + +struct ImageFrame +{ + cv::Mat image; + std_msgs::Header header; +}; + +class NcsManager +{ +public: + NcsManager(int max_device_number, int device_index, Device::LogLevel log_level, const std::string& cnn_type, + const std::string& graph_file_path, const std::string& category_file_path, const int network_dimension, + const std::vector& mean, const float& scale, const int& top_n); + ~NcsManager(); + + void startThreads(); + + void classify_stream(const cv::Mat& image, FUNP_C cbGetClassificationResult, + const sensor_msgs::ImageConstPtr& image_msg); + void detect_stream(const cv::Mat& image, FUNP_D cbGetDetectionResult, const sensor_msgs::ImageConstPtr& image_msg); + std::vector classify_image(std::vector images); + std::vector detect_image(std::vector images); + +private: + void initDeviceManager(); + void deviceThread(int device_index); + + int max_device_number_; + int device_index_; + const Device::LogLevel log_level_; + const std::string cnn_type_; + const std::string& graph_file_path_; + const std::string& category_file_path_; + const int network_dimension_; + const std::vector mean_; + const float scale_; + const int top_n_; + void* user_param_; + + std::vector> ncs_handle_list_; + int device_count_; + + FUNP_C p_c_; + FUNP_D p_d_; + + std::vector image_list_; + std::mutex mtx_; + std::vector threads_; +}; +} // namespace movidius_ncs_lib +#endif // MOVIDIUS_NCS_LIB_NCS_MANAGER_H diff --git a/movidius_ncs_lib/package.xml b/movidius_ncs_lib/package.xml index 8d8fcfc..1cfc328 100644 --- a/movidius_ncs_lib/package.xml +++ b/movidius_ncs_lib/package.xml @@ -31,6 +31,7 @@ limitations under the License. roscpp roslint + rostest roscpp roslint diff --git a/movidius_ncs_lib/src/device.cpp b/movidius_ncs_lib/src/device.cpp index 1ab2537..0fe1d50 100644 --- a/movidius_ncs_lib/src/device.cpp +++ b/movidius_ncs_lib/src/device.cpp @@ -54,7 +54,7 @@ std::string Device::getName() const void Device::open() { - ROS_INFO_STREAM("open device #" << index_ << " name=" << name_); + ROS_INFO_STREAM("opening device #" << index_ << " name=" << name_); int ret = mvncOpenDevice(name_.c_str(), &handle_); ExceptionUtil::tryToThrowMvncException(ret); } diff --git a/movidius_ncs_lib/src/ncs.cpp b/movidius_ncs_lib/src/ncs.cpp index 4980aea..cb63820 100644 --- a/movidius_ncs_lib/src/ncs.cpp +++ b/movidius_ncs_lib/src/ncs.cpp @@ -214,6 +214,8 @@ void NCS::loadGraph(const std::string& graph_file_path) { std::string graph = getFileContent(graph_file_path); graph_.reset(new Graph(device_, graph, network_dimension_)); + + ROS_INFO("graph is loaded"); } void NCS::loadCategories(const std::string& category_file_path) diff --git a/movidius_ncs_lib/src/ncs_manager.cpp b/movidius_ncs_lib/src/ncs_manager.cpp new file mode 100644 index 0000000..ab335f0 --- /dev/null +++ b/movidius_ncs_lib/src/ncs_manager.cpp @@ -0,0 +1,230 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "movidius_ncs_lib/ncs_manager.h" + +namespace movidius_ncs_lib +{ +NcsManager::NcsManager(int max_device_number, int device_index, Device::LogLevel log_level, const std::string& cnn_type, + const std::string& graph_file_path, const std::string& category_file_path, + const int network_dimension, const std::vector& mean, const float& scale, + const int& top_n) + : max_device_number_(max_device_number) + , device_index_(device_index) + , log_level_(log_level) + , cnn_type_(cnn_type) + , graph_file_path_(graph_file_path) + , category_file_path_(category_file_path) + , network_dimension_(network_dimension) + , mean_(mean) + , scale_(scale) + , top_n_(top_n) + , user_param_(nullptr) + , device_count_(0) +{ + initDeviceManager(); +} + +NcsManager::~NcsManager() +{ +} + +void NcsManager::initDeviceManager() +{ + char device_name[100]; + while (mvncGetDeviceName(device_count_, device_name, 100) != MVNC_DEVICE_NOT_FOUND) + { + auto ncs_handle = std::make_shared(device_index_, static_cast(log_level_), + cnn_type_, graph_file_path_, category_file_path_, + network_dimension_, mean_, scale_, top_n_); + ncs_handle_list_.push_back(ncs_handle); + device_count_++; + device_index_++; + + if (device_count_ == max_device_number_) + { + break; + } + } +} + +void join(std::thread& t) +{ + t.join(); +} + +void NcsManager::deviceThread(int device_index) +{ + while (1) + { + if (!image_list_.empty()) + { + mtx_.lock(); + + auto first_image = image_list_[0].image; + auto first_image_header = image_list_[0].header; + + if (!image_list_.empty()) + { + image_list_.erase(image_list_.begin()); + } + else + { + mtx_.unlock(); + continue; + } + mtx_.unlock(); + + if (!cnn_type_.compare("alexnet") || !cnn_type_.compare("googlenet") || !cnn_type_.compare("inception_v1") || + !cnn_type_.compare("inception_v2") || !cnn_type_.compare("inception_v3") || + !cnn_type_.compare("inception_v4") || !cnn_type_.compare("mobilenet") || !cnn_type_.compare("squeezenet")) + { + ncs_handle_list_[device_index]->loadTensor(first_image); + ncs_handle_list_[device_index]->classify(); + ClassificationResultPtr result = ncs_handle_list_[device_index]->getClassificationResult(); + + p_c_(result, first_image_header); + } + else + { + ncs_handle_list_[device_index]->loadTensor(first_image); + ncs_handle_list_[device_index]->detect(); + DetectionResultPtr result = ncs_handle_list_[device_index]->getDetectionResult(); + + p_d_(result, first_image_header); + } + } + } +} + +void NcsManager::startThreads() +{ + for (int i = 0; i < device_count_; i++) + { + threads_.push_back(std::thread(&NcsManager::deviceThread, this, i)); + } + + std::for_each(threads_.begin(), threads_.end(), join); + + ROS_INFO("started %d threads", device_count_); +} + +std::vector NcsManager::classify_image(std::vector images) +{ + int image_size = int(images.size()); + std::vector results(image_size); + + int parallel_group = image_size / device_count_; + int parallel_left = image_size % device_count_; + + for (int i = 0; i < parallel_group; i++) + { +#pragma omp parallel for + for (int device_index = 0; device_index < device_count_; device_index++) + { + cv::Mat imageData = cv::imread(images[i * device_count_ + device_index]); + ncs_handle_list_[device_index]->loadTensor(imageData); + ncs_handle_list_[device_index]->classify(); + ClassificationResultPtr result = ncs_handle_list_[device_index]->getClassificationResult(); + results[i * device_count_ + device_index] = std::make_shared(*result); + } + } + + for (int j = 0; j < parallel_left; j++) + { + cv::Mat imageData = cv::imread(images[parallel_group * device_count_ + j]); + ncs_handle_list_[j]->loadTensor(imageData); + ncs_handle_list_[j]->classify(); + ClassificationResultPtr result = ncs_handle_list_[j]->getClassificationResult(); + results[parallel_group * device_count_ + j] = std::make_shared(*result); + } + + return results; +} + +std::vector NcsManager::detect_image(std::vector images) +{ + int image_size = int(images.size()); + std::vector results(image_size); + + int parallel_group = image_size / device_count_; + int parallel_left = image_size % device_count_; + + for (int i = 0; i < parallel_group; i++) + { +#pragma omp parallel for + for (int device_index = 0; device_index < device_count_; device_index++) + { + cv::Mat imageData = cv::imread(images[i * device_count_ + device_index]); + ncs_handle_list_[device_index]->loadTensor(imageData); + ncs_handle_list_[device_index]->detect(); + DetectionResultPtr result = ncs_handle_list_[device_index]->getDetectionResult(); + results[i * device_count_ + device_index] = std::make_shared(*result); + } + } + + for (int i = 0; i < parallel_left; i++) + { + cv::Mat imageData = cv::imread(images[parallel_group * device_count_ + i]); + ncs_handle_list_[i]->loadTensor(imageData); + ncs_handle_list_[i]->detect(); + DetectionResultPtr result = ncs_handle_list_[i]->getDetectionResult(); + results[parallel_group * device_count_ + i] = std::make_shared(*result); + } + + return results; +} + +void NcsManager::classify_stream(const cv::Mat& image, FUNP_C cbGetClassificationResult, + const sensor_msgs::ImageConstPtr& image_msg) +{ + p_c_ = cbGetClassificationResult; + + ImageFrame imageFrame; + imageFrame.header = image_msg->header; + imageFrame.image = image; + + mtx_.lock(); + if (image_list_.size() > 10) + { + image_list_.erase(image_list_.begin()); + } + image_list_.push_back(imageFrame); + mtx_.unlock(); +} + +void NcsManager::detect_stream(const cv::Mat& image, FUNP_D cbGetDetectionResult, + const sensor_msgs::ImageConstPtr& image_msg) +{ + p_d_ = cbGetDetectionResult; + + ImageFrame imageFrame; + imageFrame.header = image_msg->header; + imageFrame.image = image; + + mtx_.lock(); + if (image_list_.size() > 10) + { + image_list_.erase(image_list_.begin()); + } + image_list_.push_back(imageFrame); + mtx_.unlock(); +} +} // namespace movidius_ncs_lib diff --git a/movidius_ncs_lib/tests/test_all.test b/movidius_ncs_lib/tests/test_all.test new file mode 100644 index 0000000..b46ccbf --- /dev/null +++ b/movidius_ncs_lib/tests/test_all.test @@ -0,0 +1,22 @@ + + + + + + + + diff --git a/movidius_ncs_lib/tests/unittest_environment.cpp b/movidius_ncs_lib/tests/unittest_environment.cpp new file mode 100644 index 0000000..6d515c9 --- /dev/null +++ b/movidius_ncs_lib/tests/unittest_environment.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include +#include + +TEST(UnitTestEnvironment, testSDK) +{ + std::system("python2 /opt/movidius/NCSDK/tests/api-check/ncs-python2-check.py > /tmp/NCS_unittest.txt"); + EXPECT_TRUE(boost::filesystem::exists("/tmp/NCS_unittest.txt")); + std::ifstream fin("/tmp/NCS_unittest.txt"); + std::string ncs_check, tmp; + while (std::getline(fin, tmp)) + { + ncs_check = tmp; + } + EXPECT_EQ(ncs_check, "NCS device working."); + std::system("rm -rf /tmp/NCS_unittest.txt > /dev/null 2>&1"); +} + +TEST(UnitTestEnvironment, testAppZoo) +{ + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo")); + std::system("cd /opt/movidius/ncappzoo && make > /dev/null 2>&1"); + std::vector caffe_dirs = { "AlexNet", "GoogLeNet", "SqueezeNet", "SSD_MobileNet", "TinyYolo" }; + std::vector tf_dirs = { "inception_v1", "inception_v2", "inception_v3", "inception_v4", "mobilenets" }; + for (std::string caffe : caffe_dirs) + { + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/caffe/" + caffe + "/graph")); + } + + for (std::string tf : tf_dirs) + { + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/tensorflow/" + tf + "/graph")); + } +} + +TEST(UnitTestEnvironment, testCategories) +{ + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/data/ilsvrc12/imagenet1000.txt")); + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/data/ilsvrc12/imagenet1001.txt")); + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/data/ilsvrc12/voc20.txt")); + EXPECT_TRUE(boost::filesystem::exists("/opt/movidius/ncappzoo/data/ilsvrc12/voc21.txt")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/movidius_ncs_lib/tests/unittest_function.cpp b/movidius_ncs_lib/tests/unittest_function.cpp new file mode 100644 index 0000000..cea2d88 --- /dev/null +++ b/movidius_ncs_lib/tests/unittest_function.cpp @@ -0,0 +1,111 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include + +#include "movidius_ncs_lib/ncs_manager.h" + +TEST(UnitTestFunction, testLibraryFunctions) +{ + std::vector caffe_dirs = { "AlexNet", "GoogLeNet", "SqueezeNet", "SSD_MobileNet", "TinyYolo" }; + std::vector tf_dirs = { "inception_v1", "inception_v2", "inception_v3", "inception_v4", "mobilenets" }; + std::vector caffe_nets = { "alexnet", "googlenet", "squeezenet", "mobilenetssd", "tinyyolo_v1" }; + std::vector tf_nets = { "inception_v1", "inception_v2", "inception_v3", "inception_v4", "mobilenet" }; + std::vector caffe_categories = + {"imagenet1000.txt", "imagenet1000.txt", "imagenet1000.txt", "voc21.txt", "voc20.txt" }; + std::vector> caffe_means = { { 104.0069879317889, 116.66876761696767, 122.6789143406786 }, + { 104.0069879317889, 116.66876761696767, 122.6789143406786 }, + { 104.0069879317889, 116.66876761696767, 122.6789143406786 }, + { 127.5, 127.5, 127.5 }, + { 0, 0, 0 } }; + + std::vector> tf_means = { { 128.0, 128.0, 128.0 }, + { 128.0, 128.0, 128.0 }, + { 128.0, 128.0, 128.0 }, + { 128.0, 128.0, 128.0 }, + { 127.5, 127.5, 127.5 } }; + std::vector caffe_scales = { 1.0, 1.0, 1.0, 0.007843, 0.00392156 }; + std::vector tf_scales = { 0.0078125, 0.0078125, 0.0078125, 0.0078125, 0.007843 }; + std::vector caffe_dimensions = { 227, 224, 227, 300, 448 }; + std::vector tf_dimensions = { 224, 224, 299, 299, 224 }; + + for (unsigned int i = 0; i < caffe_nets.size(); i++) + { + ASSERT_NO_THROW( + { + std::shared_ptr handle = + std::make_shared(50, static_cast(0), caffe_nets[i], + "/opt/movidius/ncappzoo/caffe/" + caffe_dirs[i] + "/graph", + "/opt/movidius/ncappzoo/data/ilsvrc12/" + caffe_categories[i], + caffe_dimensions[i], caffe_means[i], caffe_scales[i], 3); + EXPECT_TRUE(handle != nullptr); + std::vector images_path; + images_path.push_back(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle.jpg"); + if (i < 3) + { + handle->classify_image(images_path); + } + else + { + handle->detect_image(images_path); + } + }); + } + + for (unsigned int i = 0; i < tf_nets.size(); i++) + { + ASSERT_NO_THROW( + { + std::shared_ptr handle = std::make_shared( + 50, static_cast(0), tf_nets[i], + "/opt/movidius/ncappzoo/tensorflow/" + tf_dirs[i] + "/graph", + "/opt/movidius/ncappzoo/data/ilsvrc12/imagenet1001.txt", tf_dimensions[i], tf_means[i], tf_scales[i], 3); + EXPECT_TRUE(handle != nullptr); + std::vector images_path; + images_path.push_back(ros::package::getPath("movidius_ncs_lib") + "/../data/images/bicycle.jpg"); + handle->classify_image(images_path); + sleep(10); + }); + } +} + +TEST(UnitTestFunction, testLibraryIncorrectInputs) +{ + try + { + std::vector incorrect_mean = { 0, 0, 0 }; + std::shared_ptr handle = + std::make_shared(0, static_cast(0), "Incorrect_type", + "graph_not_exist", + "categories_not_exist", + 0, incorrect_mean, 0, 3); + } + catch(...) + { + SUCCEED(); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/movidius_ncs_stream/CMakeLists.txt b/movidius_ncs_stream/CMakeLists.txt index 1710e53..267eb25 100644 --- a/movidius_ncs_stream/CMakeLists.txt +++ b/movidius_ncs_stream/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS image_transport cv_bridge roslint + rostest object_msgs movidius_ncs_lib ) diff --git a/movidius_ncs_stream/include/movidius_ncs_stream/ncs_nodelet.h b/movidius_ncs_stream/include/movidius_ncs_stream/ncs_nodelet.h index 8ceb482..c5fae72 100644 --- a/movidius_ncs_stream/include/movidius_ncs_stream/ncs_nodelet.h +++ b/movidius_ncs_stream/include/movidius_ncs_stream/ncs_nodelet.h @@ -24,29 +24,40 @@ #include #include +#include "boost/bind.hpp" +#include "boost/function.hpp" + #include "movidius_ncs_lib/ncs.h" +#include "movidius_ncs_lib/ncs_manager.h" namespace movidius_ncs_stream { +typedef boost::function FUNP_C; +typedef boost::function FUNP_D; + class NCSImpl { public: NCSImpl(ros::NodeHandle& nh, ros::NodeHandle& pnh); ~NCSImpl(); + void cbGetClassificationResult(movidius_ncs_lib::ClassificationResultPtr result, std_msgs::Header header); + void cbGetDetectionResult(movidius_ncs_lib::DetectionResultPtr result, std_msgs::Header header); + private: void cbClassify(const sensor_msgs::ImageConstPtr& image); void cbDetect(const sensor_msgs::ImageConstPtr& image); void getParameters(); void init(); - std::shared_ptr ncs_handle_; + std::shared_ptr ncs_manager_handle_; ros::Publisher pub_; image_transport::Subscriber sub_; ros::NodeHandle nh_; ros::NodeHandle pnh_; + int max_device_number_; int device_index_; int log_level_; std::string cnn_type_; @@ -58,7 +69,7 @@ class NCSImpl int top_n_; }; -class NCSNodelet: public nodelet::Nodelet +class NCSNodelet : public nodelet::Nodelet { public: virtual ~NCSNodelet(); diff --git a/movidius_ncs_stream/package.xml b/movidius_ncs_stream/package.xml index a44b3e4..d5a903d 100644 --- a/movidius_ncs_stream/package.xml +++ b/movidius_ncs_stream/package.xml @@ -37,6 +37,7 @@ limitations under the License. object_msgs movidius_ncs_lib roslint + rostest nodelet roscpp diff --git a/movidius_ncs_stream/src/ncs_nodelet.cpp b/movidius_ncs_stream/src/ncs_nodelet.cpp index e1e6caa..447e55d 100644 --- a/movidius_ncs_stream/src/ncs_nodelet.cpp +++ b/movidius_ncs_stream/src/ncs_nodelet.cpp @@ -39,18 +39,19 @@ using movidius_ncs_lib::Device; namespace movidius_ncs_stream { NCSImpl::NCSImpl(ros::NodeHandle& nh, ros::NodeHandle& pnh) - : ncs_handle_(nullptr), - nh_(nh), - pnh_(pnh), - device_index_(0), - log_level_(Device::Errors), - cnn_type_(""), - graph_file_path_(""), - category_file_path_(""), - network_dimension_(0), - mean_(0), - scale_(1.0), - top_n_(1) + : ncs_manager_handle_(nullptr) + , nh_(nh) + , pnh_(pnh) + , max_device_number_(255) + , device_index_(0) + , log_level_(Device::Errors) + , cnn_type_("") + , graph_file_path_("") + , category_file_path_("") + , network_dimension_(0) + , mean_(0) + , scale_(1.0) + , top_n_(1) { getParameters(); init(); @@ -64,6 +65,19 @@ void NCSImpl::getParameters() { ROS_DEBUG("NCSImpl get parameters"); + if (!pnh_.getParam("max_device_number", max_device_number_)) + { + ROS_WARN("param max_device_number not set, use default"); + } + + if (max_device_number_ <= 0) + { + ROS_ERROR_STREAM("invalid param max_device_number = " << max_device_number_); + throw std::exception(); + } + + ROS_INFO_STREAM("use max_device_number = " << max_device_number_); + if (!pnh_.getParam("device_index", device_index_)) { ROS_WARN("param device_index not set, use default"); @@ -121,11 +135,10 @@ void NCSImpl::getParameters() ROS_WARN("param cnn_type not set, use default"); } - if (cnn_type_.compare("alexnet") && cnn_type_.compare("googlenet") - && cnn_type_.compare("inception_v1") && cnn_type_.compare("inception_v2") - && cnn_type_.compare("inception_v3") && cnn_type_.compare("inception_v4") - && cnn_type_.compare("mobilenet") && cnn_type_.compare("squeezenet") - && cnn_type_.compare("tinyyolo_v1") && cnn_type_.compare("mobilenetssd")) + if (cnn_type_.compare("alexnet") && cnn_type_.compare("googlenet") && cnn_type_.compare("inception_v1") && + cnn_type_.compare("inception_v2") && cnn_type_.compare("inception_v3") && cnn_type_.compare("inception_v4") && + cnn_type_.compare("mobilenet") && cnn_type_.compare("squeezenet") && cnn_type_.compare("tinyyolo_v1") && + cnn_type_.compare("mobilenetssd")) { ROS_WARN_STREAM("invalid cnn_type_=" << cnn_type_); throw std::exception(); @@ -146,7 +159,6 @@ void NCSImpl::getParameters() ROS_INFO_STREAM("use network_dimension = " << network_dimension_); - for (int i = 0; i < 3; i++) { std::ostringstream oss; @@ -190,21 +202,16 @@ void NCSImpl::getParameters() void NCSImpl::init() { ROS_DEBUG("NCSImpl onInit"); - ncs_handle_ = std::make_shared(device_index_, - static_cast(log_level_), - cnn_type_, - graph_file_path_, - category_file_path_, - network_dimension_, - mean_, - scale_, - top_n_); + + ncs_manager_handle_ = std::make_shared( + max_device_number_, device_index_, static_cast(log_level_), cnn_type_, graph_file_path_, + category_file_path_, network_dimension_, mean_, scale_, top_n_); + boost::shared_ptr it = boost::make_shared(nh_); - if (!cnn_type_.compare("alexnet") || !cnn_type_.compare("googlenet") - || !cnn_type_.compare("inception_v1") || !cnn_type_.compare("inception_v2") - || !cnn_type_.compare("inception_v3") || !cnn_type_.compare("inception_v4") - || !cnn_type_.compare("mobilenet") || !cnn_type_.compare("squeezenet")) + if (!cnn_type_.compare("alexnet") || !cnn_type_.compare("googlenet") || !cnn_type_.compare("inception_v1") || + !cnn_type_.compare("inception_v2") || !cnn_type_.compare("inception_v3") || !cnn_type_.compare("inception_v4") || + !cnn_type_.compare("mobilenet") || !cnn_type_.compare("squeezenet")) { sub_ = it->subscribe("/camera/rgb/image_raw", 1, &NCSImpl::cbClassify, this); pub_ = nh_.advertise("classified_objects", 1); @@ -214,6 +221,8 @@ void NCSImpl::init() sub_ = it->subscribe("/camera/rgb/image_raw", 1, &NCSImpl::cbDetect, this); pub_ = nh_.advertise("detected_objects", 1); } + + ncs_manager_handle_->startThreads(); } void NCSImpl::cbClassify(const sensor_msgs::ImageConstPtr& image_msg) @@ -225,12 +234,33 @@ void NCSImpl::cbClassify(const sensor_msgs::ImageConstPtr& image_msg) } cv::Mat cameraData = cv_bridge::toCvCopy(image_msg, "bgr8")->image; - boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time(); - ncs_handle_->loadTensor(cameraData); - ncs_handle_->classify(); - ClassificationResultPtr result = ncs_handle_->getClassificationResult(); - boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); - boost::posix_time::time_duration msdiff = end - start; + + FUNP_C classification_result_callback = boost::bind(&NCSImpl::cbGetClassificationResult, this, _1, _2); + ncs_manager_handle_->classify_stream(cameraData, classification_result_callback, image_msg); +} + +void NCSImpl::cbDetect(const sensor_msgs::ImageConstPtr& image_msg) +{ + if (pub_.getNumSubscribers() == 0) + { + ROS_DEBUG_STREAM("skip inferring for no subscriber"); + return; + } + + cv::Mat cameraData = cv_bridge::toCvCopy(image_msg, "bgr8")->image; + + FUNP_D detection_result_callback = boost::bind(&NCSImpl::cbGetDetectionResult, this, _1, _2); + ncs_manager_handle_->detect_stream(cameraData, detection_result_callback, image_msg); +} + +NCSNodelet::~NCSNodelet() +{ +} + +// ros::Publisher NCSImpl::pub_; + +void NCSImpl::cbGetClassificationResult(movidius_ncs_lib::ClassificationResultPtr result, std_msgs::Header header) +{ object_msgs::Objects objs; for (auto item : result->items) @@ -241,22 +271,14 @@ void NCSImpl::cbClassify(const sensor_msgs::ImageConstPtr& image_msg) objs.objects_vector.push_back(obj); } - objs.header = image_msg->header; + objs.header = header; objs.inference_time_ms = result->time_taken; - ROS_DEBUG_STREAM("Total time: " << msdiff.total_milliseconds() << "ms"); - ROS_DEBUG_STREAM("Inference time: " << objs.inference_time_ms << "ms"); + // NCSImpl::pub_.publish(objs); pub_.publish(objs); } -void NCSImpl::cbDetect(const sensor_msgs::ImageConstPtr& image_msg) +void NCSImpl::cbGetDetectionResult(movidius_ncs_lib::DetectionResultPtr result, std_msgs::Header header) { - cv::Mat cameraData = cv_bridge::toCvCopy(image_msg, "bgr8")->image; - boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time(); - ncs_handle_->loadTensor(cameraData); - ncs_handle_->detect(); - DetectionResultPtr result = ncs_handle_->getDetectionResult(); - boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); - boost::posix_time::time_duration msdiff = end - start; object_msgs::ObjectsInBoxes objs_in_boxes; for (auto item : result->items_in_boxes) @@ -271,15 +293,11 @@ void NCSImpl::cbDetect(const sensor_msgs::ImageConstPtr& image_msg) objs_in_boxes.objects_vector.push_back(obj); } - objs_in_boxes.header = image_msg->header; + objs_in_boxes.header = header; objs_in_boxes.inference_time_ms = result->time_taken; - ROS_DEBUG_STREAM("Total time: " << msdiff.total_milliseconds() << "ms"); - ROS_DEBUG_STREAM("Inference time: " << objs_in_boxes.inference_time_ms << "ms"); - pub_.publish(objs_in_boxes); -} -NCSNodelet::~NCSNodelet() -{ + // NCSImpl::pub_.publish(objs_in_boxes); + pub_.publish(objs_in_boxes); } void NCSNodelet::onInit() diff --git a/movidius_ncs_stream/tests/test_unit.test b/movidius_ncs_stream/tests/test_unit.test new file mode 100644 index 0000000..0ac130d --- /dev/null +++ b/movidius_ncs_stream/tests/test_unit.test @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/movidius_ncs_stream/tests/unittest_stream_classification.cpp b/movidius_ncs_stream/tests/unittest_stream_classification.cpp new file mode 100644 index 0000000..e9a8fdb --- /dev/null +++ b/movidius_ncs_stream/tests/unittest_stream_classification.cpp @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +static bool test_pass = false; + +void syncCb(const sensor_msgs::ImageConstPtr& img, + const object_msgs::Objects::ConstPtr& objs) +{ + test_pass = true; + EXPECT_TRUE(true); + ros::shutdown(); +} + +TEST(UnitTestStreamClassification, testClassification) +{ + ros::NodeHandle nh("~"); + std::string camera, camera_topic; + nh.getParam("camera", camera); + if (camera == "usb") + { + camera_topic = "/usb_cam/image_raw"; + } + else if (camera == "realsense") + { + camera_topic = "/camera/color/image_raw"; + } + message_filters::Subscriber camSub(nh, + camera_topic, + 1); + message_filters::Subscriber objSub(nh, + "/movidius_ncs_nodelet/classified_objects", + 1); + message_filters::TimeSynchronizer sync(camSub, objSub, 60); + sync.registerCallback(boost::bind(&syncCb, _1, _2)); + ros::spin(); + EXPECT_TRUE(test_pass); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "movidius_ncs_stream_tests"); + return RUN_ALL_TESTS(); +} diff --git a/movidius_ncs_stream/tests/unittest_stream_detection.cpp b/movidius_ncs_stream/tests/unittest_stream_detection.cpp new file mode 100644 index 0000000..884aded --- /dev/null +++ b/movidius_ncs_stream/tests/unittest_stream_detection.cpp @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +static bool test_pass = false; + +void syncCb(const sensor_msgs::ImageConstPtr& img, + const object_msgs::ObjectsInBoxes::ConstPtr& objs_in_boxes) +{ + test_pass = true; + EXPECT_TRUE(true); + ros::shutdown(); +} + +TEST(UnitTestStreamClassification, testClassification) +{ + ros::NodeHandle nh("~"); + std::string camera, camera_topic; + nh.getParam("camera", camera); + if (camera == "usb") + { + camera_topic = "/usb_cam/image_raw"; + } + else if (camera == "realsense") + { + camera_topic = "/camera/color/image_raw"; + } + message_filters::Subscriber camSub(nh, + camera_topic, + 1); + message_filters::Subscriber objSub(nh, + "/movidius_ncs_nodelet/detected_objects", + 1); + message_filters::TimeSynchronizer sync(camSub, + objSub, + 60); + sync.registerCallback(boost::bind(&syncCb, _1, _2)); + ros::spin(); + EXPECT_TRUE(test_pass); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "movidius_ncs_stream_tests"); + return RUN_ALL_TESTS(); +}