diff --git a/realsense_ros_person/CMakeLists.txt b/realsense_ros_person/CMakeLists.txt
old mode 100644
new mode 100755
index 1bebef0a57..68f6386aaa
--- a/realsense_ros_person/CMakeLists.txt
+++ b/realsense_ros_person/CMakeLists.txt
@@ -110,7 +110,6 @@ add_library(realsense_ros_person
${ROS_WRAPPER_SOURCES}
)
-
target_link_libraries(realsense_ros_person
${PT_LINK_LIBS}
${catkin_LIBRARIES}
@@ -132,14 +131,28 @@ target_link_libraries(${PROJECT_NAME}_sample
${OpenCV_LIBRARIES}
)
+
add_dependencies(${PROJECT_NAME}_sample ${PROJECT_NAME}_generate_messages_cpp)
+if(CATKIN_ENABLE_TESTING)
+ # build person unit tests
+ find_package(rostest REQUIRED)
+ add_rostest_gtest(tests_person test/person_detection.test test/person_test.cpp)
+ target_link_libraries(tests_person
+ ${catkin_LIBRARIES}
+ ${GTEST_LIBRARIES}
+ )
+ add_dependencies(tests_person realsense_ros_person)
+
+endif()
+
# Install launch files
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
# Install xml files
-install(FILES nodelet_plugins.xml
+install(FILES nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
+
diff --git a/realsense_ros_person/launch/demo_person_tracking_from_bag.launch b/realsense_ros_person/launch/demo_person_tracking_from_bag.launch
old mode 100644
new mode 100755
index f937804c47..c33c359088
--- a/realsense_ros_person/launch/demo_person_tracking_from_bag.launch
+++ b/realsense_ros_person/launch/demo_person_tracking_from_bag.launch
@@ -19,7 +19,7 @@ roslaunch realsense_ros_person demo_person_tracking_from_bag.launch bag_path:=my
-
+
diff --git a/realsense_ros_person/test/person_detection.test b/realsense_ros_person/test/person_detection.test
new file mode 100755
index 0000000000..079d1b0aff
--- /dev/null
+++ b/realsense_ros_person/test/person_detection.test
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/realsense_ros_person/test/person_test.cpp b/realsense_ros_person/test/person_test.cpp
new file mode 100755
index 0000000000..4ef9aa1edd
--- /dev/null
+++ b/realsense_ros_person/test/person_test.cpp
@@ -0,0 +1,157 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2017 Intel Corporation. All Rights Reserved
+
+#include
+#include
+#include
+#include
+
+#include "realsense_ros_person/Frame.h"
+#include "realsense_ros_person/Recognition.h"
+#include "realsense_ros_person/RecognitionRegister.h"
+#include "realsense_ros_person/PersonModuleState.h"
+#include "realsense_ros_person/StartTracking.h"
+
+
+#include //TODO remove this include
+
+std::string DETECTION_TOPIC = "/person_tracking_output";
+std::string REGISTER_SERVICE = "/person_tracking/register_request";
+std::string RECOGNIZE_SERVICE = "/person_tracking/recognition_request";
+std::string START_TRACKING_SERVICE = "/person_tracking/start_tracking_request";
+
+bool gDetectionRecv = false;
+bool gDetectionBBoxRecv = false;
+bool gDetectionComRecv = false;
+bool gWaveRecv = false;
+
+realsense_ros_person::User gUsers;
+ros::ServiceClient gRegisterClient;
+ros::ServiceClient gRecognizeClient;
+ros::ServiceClient gStartTrackingClient;
+
+realsense_ros_person::Frame g_latestFrameData;
+std::mutex g_latestFrameDataMutex;
+
+void detectionCallback(const realsense_ros_person::Frame& frame)
+{
+ gDetectionRecv = true;
+ {
+ std::lock_guard lockGuard(g_latestFrameDataMutex);
+ g_latestFrameData = frame;
+ }
+
+ for (realsense_ros_person::User person: frame.usersData)
+ {
+ if ((person.userRect.rectCorners[0].x != 0) || (person.userRect.rectCorners[0].y != 0) ||
+ (person.userRect.rectCorners[1].x != 0) || (person.userRect.rectCorners[1].y != 0))
+ {
+ gDetectionBBoxRecv = true;
+ }
+ else
+ {
+ gDetectionBBoxRecv = false;
+ }
+
+ if ((person.centerOfMassWorld.x != 0) || (person.centerOfMassWorld.y != 0) || (person.centerOfMassWorld.z != 0) ||
+ (person.centerOfMassImage.x != 0) || (person.centerOfMassImage.y != 0))
+ {
+ gDetectionComRecv = true;
+ }
+ else
+ {
+ gDetectionComRecv = false;
+ }
+
+ //if received minimum once while test - test passed
+ if (!gWaveRecv && person.gestures.wave.type != realsense_ros_person::Wave::WAVE_NOT_DETECTED)
+ {
+ gWaveRecv = true;
+ }
+
+ if ((gDetectionBBoxRecv == false) || (gDetectionComRecv == false))
+ {
+ break;
+ }
+ }
+}
+
+TEST(PersonTests, PersonDetection)
+{
+ EXPECT_TRUE(gDetectionRecv);
+ EXPECT_TRUE(gDetectionBBoxRecv);
+ EXPECT_TRUE(gDetectionComRecv);
+}
+
+
+TEST(PersonTests, WaveDetection)
+{
+ EXPECT_TRUE(gDetectionRecv);
+ EXPECT_TRUE(gDetectionBBoxRecv);
+ EXPECT_TRUE(gDetectionComRecv);
+ EXPECT_TRUE(gWaveRecv);
+}
+
+TEST(PersonTests, Recognition)
+{
+ //register person
+ realsense_ros_person::RecognitionRegister registerRequest;
+ {
+ std::lock_guard lockGuard(g_latestFrameDataMutex);
+ EXPECT_TRUE(g_latestFrameData.numberOfUsers > 0);
+ registerRequest.request.personId = g_latestFrameData.usersData[0].userInfo.Id;
+ }
+ gRegisterClient.call(registerRequest);
+
+
+ //recognize person
+ realsense_ros_person::Recognition recognitionRequest;
+ {
+ std::lock_guard lockGuard(g_latestFrameDataMutex);
+ EXPECT_TRUE(g_latestFrameData.numberOfUsers > 0);
+ recognitionRequest.request.personId = g_latestFrameData.usersData[0].userInfo.Id;
+ }
+ gRecognizeClient.call(recognitionRequest);
+
+ ASSERT_EQ(recognitionRequest.response.recognitionId, registerRequest.response.recognitionId);
+}
+
+TEST(PersonTests, Tracking)
+{
+ int personTrackingId = -1;
+ {
+ std::lock_guard lockGuard(g_latestFrameDataMutex);
+ EXPECT_TRUE(g_latestFrameData.numberOfUsers > 0);
+ personTrackingId = g_latestFrameData.usersData[0].userInfo.Id;
+ }
+
+ realsense_ros_person::StartTracking startTrackingRequest;
+ startTrackingRequest.request.personId = personTrackingId;
+ gStartTrackingClient.call(startTrackingRequest);
+
+ EXPECT_TRUE(startTrackingRequest.response.status);
+}
+
+int main(int argc, char **argv) try
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "test_topics");
+
+ ros::NodeHandle nh;
+ ROS_INFO_STREAM("RealSense person test - Initializing Tests...");
+
+ ros::Subscriber detection_sub = nh.subscribe(DETECTION_TOPIC, 1, detectionCallback);
+ gRegisterClient = nh.serviceClient(REGISTER_SERVICE);
+ gRecognizeClient = nh.serviceClient(RECOGNIZE_SERVICE);
+ gStartTrackingClient = nh.serviceClient(START_TRACKING_SERVICE);
+
+ ros::Rate r(10);
+ for (int i = 0; i< 100; ++i)
+ {
+ ros::spinOnce();
+ r.sleep();
+ }
+
+ return RUN_ALL_TESTS();
+}
+catch(...) {} // catch the "testing::internal::::ClassUniqueToAlwaysTrue" from gtest
diff --git a/realsense_ros_person/test/person_tracking.test b/realsense_ros_person/test/person_tracking.test
new file mode 100755
index 0000000000..fc31e51503
--- /dev/null
+++ b/realsense_ros_person/test/person_tracking.test
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/realsense_ros_person/test/recognition.test b/realsense_ros_person/test/recognition.test
new file mode 100755
index 0000000000..83267822e7
--- /dev/null
+++ b/realsense_ros_person/test/recognition.test
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/realsense_ros_person/test/wave_detection.test b/realsense_ros_person/test/wave_detection.test
new file mode 100755
index 0000000000..ed6d62492c
--- /dev/null
+++ b/realsense_ros_person/test/wave_detection.test
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+