Travis CI build and test (#437)

* fix issue #335 according to solution lsolanka as suggested in pull request #336.

* moving all the properties and material definitions inside the macro as suggested by @felixvd

* add compilation flag SET_USER_BREAK_AT_STARTUP to create user waiting point for debugging purposes.

add reading from bagfile option by using <rosbag_filename> parameter in launch file.
base_realsense_node.cpp: add option - by specifying width, height or fps as 0, pick up on the first sensor profile available.
scripts/rs2_listener.py, rs2_test.py - initial version for file based, standalone unitest.

* add .travis.yml file

.gitignore

@@ -3,3 +3,5 @@
 *.bag
 *.pyc
 *.ini
+.idea/
+.vs/

.travis.yml

@@ -0,0 +1,56 @@
+sudo: required
+dist: xenial
+
+  # - git clone -v --progress https://github.com/doronhi/realsense.git  # This is Done automatically by TravisCI
+before_install:
+  - echo 'deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main' || sudo tee /etc/apt/sources.list.d/realsense-public.list
+  - sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keys.gnupg.net:80 --recv-key C8B3A55A6F3EFCDE
+  - sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main"
+  - sudo apt-get update -qq
+  - sudo apt-get install librealsense2-dkms -y
+  - sudo apt-get install librealsense2-dev -y
+
+install:
+  # install ROS:
+  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+  - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
+  - sudo apt-get update -qq
+  - sudo apt-get install ros-kinetic-ros-base -y
+  - sudo rosdep init
+  - rosdep update
+  - echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
+  - sudo apt-get install ros-kinetic-cv-bridge -y
+  - sudo apt-get install ros-kinetic-image-transport
+  - sudo apt-get install ros-kinetic-tf -y
+  - sudo apt-get install ros-kinetic-diagnostic-updater -y
+  - source ~/.bashrc
+  - mkdir -p ~/catkin_ws/src/realsense
+
+  # install realsense2-camera
+  - mv * ~/catkin_ws/src/realsense/     # This leaves behind .git, .gitignore and .travis.yml but no matter.
+  - cd ~/catkin_ws/src/
+  - catkin_init_workspace 
+  - cd ..
+  - catkin_make clean
+  - catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
+  - catkin_make install
+  - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
+  - source ~/.bashrc
+
+    # download data:
+  - bag_filename="http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag";
+  - wget $bag_filename -P "records/"
+
+  # Run test:
+script:
+  - python src/realsense/realsense2_camera/scripts/rs2_test.py --all
+
+
+before_cache:
+  - rm -f  $HOME/.gradle/caches/modules-2/modules-2.lock
+  - rm -fr $HOME/.gradle/caches/*/plugin-resolution/
+cache:
+  directories:
+    - $HOME/.gradle/caches/
+    - $HOME/.gradle/wrapper/
+    - $HOME/.android/build-cache

realsense2_camera/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(realsense2_camera)
 
 option(BUILD_WITH_OPENMP "Use OpenMP" OFF)
+option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF)
 
 find_package(catkin REQUIRED COMPONENTS
     message_generation
@@ -26,6 +27,11 @@ if(BUILD_WITH_OPENMP)
     endif()
 endif()
 
+if(SET_USER_BREAK_AT_STARTUP)
+	message("GOT FLAG IN CmakeLists.txt")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG")
+endif()
+
 find_package(realsense2)
 if(NOT realsense2_FOUND)
     message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")

realsense2_camera/include/base_realsense_node.h

@@ -85,6 +85,7 @@ namespace realsense2_camera
         void getParameters();
         void setupDevice();
         void setupPublishers();
+		void enable_devices();
         void setupStreams();
         void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
         tf::Quaternion rotationMatrixToQuaternion(const float rotation[3]) const;

realsense2_camera/launch/includes/nodelet.launch.xml

@@ -2,6 +2,7 @@
   <arg name="manager"             default="realsense2_camera_manager"/>
   <arg name="serial_no"           default=""/>
   <arg name="json_file_path"      default=""/>
+  <arg name="rosbag_filename"     default=""/>
   <arg name="depth"               default="depth"/>
   <arg name="infra1"              default="infra1"/>
   <arg name="infra2"              default="infra2"/>
@@ -47,6 +48,7 @@
   <node pkg="nodelet" type="nodelet" name="realsense2_camera" args="load realsense2_camera/RealSenseNodeFactory $(arg manager)">
     <param name="serial_no"                type="str"  value="$(arg serial_no)"/>
     <param name="json_file_path"           type="str"  value="$(arg json_file_path)"/>
+    <param name="rosbag_filename"          type="str"  value="$(arg rosbag_filename)"/>
 
     <param name="enable_pointcloud"        type="bool" value="$(arg enable_pointcloud)"/>
     <param name="enable_sync"              type="bool" value="$(arg enable_sync)"/>

realsense2_camera/launch/rs_from_file.launch

@@ -0,0 +1,80 @@
+<launch>
+  <arg name="serial_no"    default=""/>
+  <arg name="json_file_path"  default=""/>
+  <arg name="rosbag_filename"  default=""/>
+
+  <arg name="fisheye_width"       default="0"/>
+  <arg name="fisheye_height"      default="0"/>
+  <arg name="enable_fisheye"      default="true"/>
+
+  <arg name="depth_width"         default="0"/>
+  <arg name="depth_height"        default="0"/>
+  <arg name="enable_depth"        default="true"/>
+
+  <arg name="infra1_width"        default="0"/>
+  <arg name="infra1_height"       default="0"/>
+  <arg name="enable_infra1"       default="true"/>
+
+  <arg name="infra2_width"        default="0"/>
+  <arg name="infra2_height"       default="0"/>
+  <arg name="enable_infra2"       default="true"/>
+
+  <arg name="color_width"         default="0"/>
+  <arg name="color_height"        default="0"/>
+  <arg name="enable_color"        default="true"/>
+
+  <arg name="fisheye_fps"         default="0"/>
+  <arg name="depth_fps"           default="0"/>
+  <arg name="infra1_fps"          default="0"/>
+  <arg name="infra2_fps"          default="0"/>
+  <arg name="color_fps"           default="0"/>
+  <arg name="gyro_fps"            default="1000"/>
+  <arg name="accel_fps"           default="1000"/>
+  <arg name="enable_imu"          default="true"/>
+
+  <arg name="enable_pointcloud"   default="false"/>
+  <arg name="enable_sync"         default="false"/>
+
+  <arg name="align_depth"         default="false"/>
+
+  <group ns="camera">
+    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+      <arg name="serial_no"                value="$(arg serial_no)"/>
+      <arg name="json_file_path"           value="$(arg json_file_path)"/>
+      <arg name="rosbag_filename"           value="$(arg rosbag_filename)"/>
+
+      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
+      <arg name="enable_sync"              value="$(arg enable_sync)"/>
+      <arg name="align_depth"              value="$(arg align_depth)"/>
+
+      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
+      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
+      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>
+
+      <arg name="depth_width"              value="$(arg depth_width)"/>
+      <arg name="depth_height"             value="$(arg depth_height)"/>
+      <arg name="enable_depth"             value="$(arg enable_depth)"/>
+
+      <arg name="color_width"              value="$(arg color_width)"/>
+      <arg name="color_height"             value="$(arg color_height)"/>
+      <arg name="enable_color"             value="$(arg enable_color)"/>
+
+      <arg name="infra1_width"             value="$(arg infra1_width)"/>
+      <arg name="infra1_height"            value="$(arg infra1_height)"/>
+      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
+
+      <arg name="infra2_width"             value="$(arg infra2_width)"/>
+      <arg name="infra2_height"            value="$(arg infra2_height)"/>
+      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
+
+      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
+      <arg name="depth_fps"                value="$(arg depth_fps)"/>
+      <arg name="infra1_fps"               value="$(arg infra1_fps)"/>
+      <arg name="infra2_fps"               value="$(arg infra2_fps)"/>
+      <arg name="color_fps"                value="$(arg color_fps)"/>
+      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
+      <arg name="accel_fps"                value="$(arg accel_fps)"/>
+      <arg name="enable_imu"               value="$(arg enable_imu)"/>
+    </include>
+  </group>
+</launch>

realsense2_camera/scripts/rs2_listener.py

@@ -0,0 +1,141 @@
+import sys
+import time
+import rospy
+from sensor_msgs.msg import Image as msg_Image
+import numpy as np
+from cv_bridge import CvBridge, CvBridgeError
+import inspect
+
+
+
+class CWaitForMessage:
+    def __init__(self, params={}):
+        self.result = None
+
+        self.break_timeout = False
+        self.timeout = params.get('timeout_secs', -1)
+        self.seq = params.get('seq', -1)
+        self.time = params.get('time', None)
+        self.node_name = params.get('node_name', 'rs2_listener')
+        self.bridge = CvBridge()
+
+        self.themes = {'depthStream': {'topic': '', 'callback': self.imageDepthCallback, 'msg_type': msg_Image},
+                       'colorStream': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}}
+
+        self.func_data = dict()
+
+    def imageColorCallback(self, data):
+        self.prev_time = time.time()
+        func_name = inspect.stack()[0][3]
+        theme_name = [key for key, value in self.themes.items() if func_name in value['callback'].__name__][0]
+        self.func_data[theme_name].setdefault('avg', [])
+        self.func_data[theme_name].setdefault('num_channels', [])
+
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+        except CvBridgeError as e:
+            print(e)
+            return
+        (rows, cols, channels) = cv_image.shape
+        pyimg = np.asarray(cv_image)
+        self.func_data[theme_name]['avg'].append(pyimg.mean())
+        self.func_data[theme_name]['num_channels'].append(channels)
+
+    def imageDepthCallback(self, data):
+        pass
+
+    def wait_for_message(self, params):
+        topic = params['topic']
+        print 'connect to ROS with name: %s' % self.node_name
+        rospy.init_node(self.node_name, anonymous=True)
+
+        rospy.loginfo('Subscribing on topic: %s' % topic)
+        self.sub = rospy.Subscriber(topic, msg_Image, self.callback)
+
+        self.prev_time = time.time()
+        break_timeout = False
+        while not any([rospy.core.is_shutdown(), break_timeout, self.result]):
+            rospy.rostime.wallsleep(0.5)
+            if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
+                break_timeout = True
+                self.sub.unregister()
+
+        return self.result
+
+    @staticmethod
+    def unregister_all(registers):
+        for test_name in registers:
+            rospy.loginfo('Un-Subscribing test %s' % test_name)
+            registers[test_name]['sub'].unregister()
+
+    def wait_for_messages(self, themes):
+        # tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}}
+        self.func_data = dict([[theme_name, {}] for theme_name in themes])
+
+        print 'connect to ROS with name: %s' % self.node_name
+        rospy.init_node(self.node_name, anonymous=True)
+        for theme_name in themes:
+            theme = self.themes[theme_name]
+            rospy.loginfo('Subscribing %s on topic: %s' % (theme_name, theme['topic']))
+            self.func_data[theme_name]['sub'] = rospy.Subscriber(theme['topic'], theme['msg_type'], theme['callback'])
+
+        self.prev_time = time.time()
+        break_timeout = False
+        while not any([rospy.core.is_shutdown(), break_timeout]):
+            rospy.rostime.wallsleep(0.5)
+            if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
+                break_timeout = True
+                self.unregister_all(self.func_data)
+
+        return self.func_data
+
+    def callback(self, data):
+        rospy.loginfo('Got message. Seq %d, secs: %d, nsecs: %d' % (data.header.seq, data.header.stamp.secs, data.header.stamp.nsecs))
+
+        self.prev_time = time.time()
+        if any([self.seq > 0 and data.header.seq >= self.seq,
+                self.time and data.header.stamp.secs == self.time['secs'] and data.header.stamp.nsecs == self.time['nsecs']]):
+            self.result = data
+            self.sub.unregister()
+
+
+
+def main():
+    if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv:
+        print 'USAGE:'
+        print '------'
+        print 'rs2_listener.py <topic> [Options]'
+        print 'example: rs2_listener.py /camera/color/image_raw --time 1532423022.044515610 --timeout 3'
+        print ''
+        print 'Application subscribes on <topic>, wait for the first message matching [Options].'
+        print 'When found, prints the timestamp.'
+        print
+        print '[Options:]'
+        print '-s <sequential number>'
+        print '--time <secs.nsecs>'
+        print '--timeout <secs>'
+        exit(-1)
+
+    # wanted_topic = '/device_0/sensor_0/Depth_0/image/data'
+    # wanted_seq = 58250
+
+    wanted_topic = sys.argv[1]
+    msg_params = {'topic': wanted_topic}
+
+    for idx in range(2, len(sys.argv)):
+        if sys.argv[idx] == '-s':
+            msg_params['seq'] = int(sys.argv[idx + 1])
+        if sys.argv[idx] == '--time':
+            msg_params['time'] = dict(zip(['secs', 'nsecs'], [int(part) for part in sys.argv[idx + 1].split('.')]))
+        if sys.argv[idx] == '--timeout':
+            msg_params['timeout_secs'] = int(sys.argv[idx + 1])
+
+    msg_retriever = CWaitForMessage(msg_params)
+    res = msg_retriever.wait_for_message()
+
+    rospy.loginfo('Got message: %s' % res.header)
+
+
+if __name__ == '__main__':
+    main()
+