Kitti image tools added, not working.

ros/src/data_logger/launch/steering_logger.launch

@@ -1,7 +1,7 @@
 <launch>
 
     <node name="steering_logger" type="steering_logger" pkg="data_logger" output="screen">
-    	<param name="logger_path" value="/home/neil/Desktop/data" />
+    	<param name="logger_path" value="/home/neil/Desktop/data-4" />
   	</node>
 
 </launch>

ros/src/segmentation/scripts/simpleExample_transformTestResults2BEV.py

@@ -18,7 +18,7 @@
 #
 
 import os, sys
-import computeBaseline, transform2BEV
+import transform2BEV
 
 #########################################################################
 # test script to process testing data in perspective domain and 

ros/src/segmentation/scripts/transform2BEV.py

@@ -54,7 +54,7 @@ def main(dataFiles, pathToCalib, outputPath, calib_end  = '.txt'):
         assert os.path.isfile(aFile), '%s is not a file' %aFile
 
         file_key = aFile.split('/')[-1].split('.')[0]
-        print "Transforming file %s to Birds Eye View " %file_key 
+        print("Transforming file %s to Birds Eye View " %file_key)
         tags = file_key.split('_')
         data_end = aFile.split(file_key)[-1]
 
@@ -67,8 +67,8 @@ def main(dataFiles, pathToCalib, outputPath, calib_end  = '.txt'):
 
         # Check if calb file exist!
         if not os.path.isfile(calib_file):
-            print "Cannot find calib file: %s" %calib_file
-            print "Attention: It is assumed that input data and calib files have the same name (only different extension)!" 
+            print("Cannot find calib file: %s" %calib_file)
+            print("Attention: It is assumed that input data and calib files have the same name (only different extension)!")
             sys.exit(1)
 
         # Update calibration for Birds Eye View
@@ -98,12 +98,12 @@ def main(dataFiles, pathToCalib, outputPath, calib_end  = '.txt'):
     print sys.argv
     # check for correct number of arguments.
     if len(sys.argv)!=4:
-        print "Usage: python transform2BEV.py <InputFiles> <PathToCalib> <OutputPath> "
-        print "<InputFiles>: the files you want to transform to BirdsEyeView, e.g., '/home/elvis/kitti_road/data/*.png' (use quotes!)"
-        print "<PathToCalib>: containing calib data as calib-files, e.g., /home/elvis/kitti_road/calib/" 
-        print "<OutputPath>: where the BirdsEyeView data will be saved, e.g., /home/elvis/kitti_road/data_bev"
-        print "ATTENTION: It is assumed that input data and calib files have the same name (only different extension)!"
-        print  "Your provided parameters: ", sys.argv
+        print("Usage: python transform2BEV.py <InputFiles> <PathToCalib> <OutputPath> ")
+        print("<InputFiles>: the files you want to transform to BirdsEyeView, e.g., '/home/elvis/kitti_road/data/*.png' (use quotes!)")
+        print("<PathToCalib>: containing calib data as calib-files, e.g., /home/elvis/kitti_road/calib/" )
+        print("<OutputPath>: where the BirdsEyeView data will be saved, e.g., /home/elvis/kitti_road/data_bev")
+        print("ATTENTION: It is assumed that input data and calib files have the same name (only different extension)!")
+        print("Your provided parameters: ", sys.argv)
         sys.exit(1)
 
     # parse parameters

ros/src/sensors/cv_camera/CMakeLists.txt

@@ -75,23 +75,3 @@ install(DIRECTORY include/${PROJECT_NAME}/
 install(FILES cv_camera_nodelets.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   )
-
-
-#############
-## Testing ##
-#############
-
-roslint_cpp(
-  src/capture.cpp src/cv_camera_node.cpp src/cv_camera_nodelet.cpp src/driver.cpp
-  )
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test_cv_camera test/cv_camera.test
-    test/test_cv_camera.cpp)
-  target_link_libraries(test_cv_camera ${PROJECT_NAME} ${catkin_LIBRARIES})
-
-  add_rostest_gtest(test_cv_camera_no_yaml test/no_yaml.test
-    test/test_cv_camera_no_yaml.cpp)
-  target_link_libraries(test_cv_camera_no_yaml ${PROJECT_NAME} ${catkin_LIBRARIES})
-endif()

ros/src/sensors/cv_camera/scripts/cv_bridge_2.py

@@ -0,0 +1,133 @@
+
+import sensor_msgs.msg
+import sys
+import numpy as np
+import cv2
+
+from cv_bridge import CvBridgeError
+from cv_bridge.boost.cv_bridge_boost import CV_MAT_CNWrap, CV_MAT_DEPTHWrap
+from cv_bridge.boost.cv_bridge_boost import getCvType
+from cv_bridge.boost.cv_bridge_boost import cvtColor2
+
+
+class CvBridge2(object):
+
+
+    def __init__(self):
+
+        self.cvtype_to_name = {}
+        self.cvdepth_to_numpy_depth = {cv2.CV_8U: 'uint8', cv2.CV_8S: 'int8', cv2.CV_16U: 'uint16',
+                                       cv2.CV_16S: 'int16', cv2.CV_32S:'int32', cv2.CV_32F:'float32',
+                                       cv2.CV_64F: 'float64'}
+
+        for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F"]:
+            for c in [1, 2, 3, 4]:
+                nm = "%sC%d" % (t, c)
+                self.cvtype_to_name[getattr(cv2, "CV_%s" % nm)] = nm
+
+        self.numpy_type_to_cvtype = {'uint8': '8U', 'int8': '8S', 'uint16': '16U',
+                                        'int16': '16S', 'int32': '32S', 'float32': '32F',
+                                        'float64': '64F'}
+        self.numpy_type_to_cvtype.update(dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()))
+
+    def dtype_with_channels_to_cvtype2(self, dtype, n_channels):
+        return '%sC%d' % (self.numpy_type_to_cvtype[dtype.name], n_channels)
+
+    def cvtype2_to_dtype_with_channels(self, cvtype):
+        return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype)
+
+    def encoding_to_cvtype2(self, encoding):
+
+        try:
+            return getCvType(encoding)
+        except RuntimeError as e:
+            raise CvBridgeError(e)
+
+    def encoding_to_dtype_with_channels(self, encoding):
+        return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding))
+
+    def imgmsg_to_cv2(self, img_msg, desired_encoding = "passthrough"):
+        """
+        Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`.
+
+        :param img_msg:   A :cpp:type:`sensor_msgs::Image` message
+        :param desired_encoding:  The encoding of the image data, one of the following strings:
+
+           * ``"passthrough"``
+           * one of the standard strings in sensor_msgs/image_encodings.h
+
+        :rtype: :cpp:type:`cv::Mat`
+        :raises CvBridgeError: when conversion is not possible.
+
+        If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg.
+        Otherwise desired_encoding must be one of the standard image encodings
+
+        This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
+
+        If the image only has one channel, the shape has size 2 (width and height)
+        """
+
+        dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding)
+        dtype = np.dtype(dtype)
+        dtype = dtype.newbyteorder('>' if img_msg.is_bigendian else '<')
+        if n_channels == 1:
+            im = np.ndarray(shape=(img_msg.height, img_msg.width),
+                           dtype=dtype, buffer=img_msg.data)
+        else:
+            im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels),
+                           dtype=dtype, buffer=img_msg.data)
+        # If the byt order is different between the message and the system.
+        if img_msg.is_bigendian == (sys.byteorder == 'little'):
+            im = im.byteswap().newbyteorder()
+
+        if desired_encoding == "passthrough":
+            return im
+
+        try:
+            res = cvtColor2(im, img_msg.encoding, desired_encoding)
+        except RuntimeError as e:
+            raise CvBridgeError(e)
+
+        return res
+
+    def cv2_to_imgmsg(self, cvim, encoding = "passthrough"):
+        """
+        Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.
+
+        :param cvim:      An OpenCV :cpp:type:`cv::Mat`
+        :param encoding:  The encoding of the image data, one of the following strings:
+
+           * ``"passthrough"``
+           * one of the standard strings in sensor_msgs/image_encodings.h
+
+        :rtype:           A sensor_msgs.msg.Image message
+        :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding``
+
+        If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type.
+        Otherwise desired_encoding must be one of the standard image encodings
+
+        This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
+        """
+
+        if not isinstance(cvim, (np.ndarray, np.generic)):
+            raise TypeError('Your input type is not a numpy array')
+        img_msg = sensor_msgs.msg.Image()
+        img_msg.height = cvim.shape[0]
+        img_msg.width = cvim.shape[1]
+        if len(cvim.shape) < 3:
+            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1)
+        else:
+            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2])
+        if encoding == "passthrough":
+            img_msg.encoding = cv_type
+        else:
+            img_msg.encoding = encoding
+            # Verify that the supplied encoding is compatible with the type of the OpenCV image
+            if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type:
+                raise CvBridgeError("encoding specified as %s, but image has incompatible type %s" % (encoding, cv_type))
+        if cvim.dtype.byteorder == '>':
+            img_msg.is_bigendian = True
+        img_msg.data = cvim.tostring()
+        img_msg.step = len(img_msg.data) // img_msg.height
+
+        return img_msg

ros/src/sensors/cv_camera/scripts/python_cam.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+#
+# By Neil Nie
+# (c) 2018, All Rights Reserved
+
+import rospy
+import cv2
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+import time
+
+
+class python_cam(object):
+
+    def __init__(self):
+
+        self.bridge = CvBridge()
+
+        self.numpy_type_to_cvtype = {'uint8': '8U', 'int8': '8S', 'uint16': '16U',
+                                        'int16': '16S', 'int32': '32S', 'float32': '32F',
+                                        'float64': '64F'}
+        self.numpy_type_to_cvtype.update(dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()))
+
+        rospy.init_node('python_cam')
+
+        self.publisher = rospy.Publisher('/cv_camera_node/image_raw', data_class=Image, queue_size=5)
+
+        cap = cv2.VideoCapture(0)
+
+        # Find OpenCV version
+        # With webcam get(CV_CAP_PROP_FPS) does not work.
+        # Let's see for ourselves.
+
+        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1024)
+        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 512)
+        cap.set(cv2.CAP_PROP_FPS, 24)
+
+        fps = cap.get(cv2.CAP_PROP_FPS)
+        print "Frames per second using video.get(cv2.CAP_PROP_FPS) : {0}".format(fps)
+
+        rate = rospy.Rate(30)
+
+        while not rospy.is_shutdown():
+
+            start_time = time.time()
+
+            ret, frame = cap.read()
+            print("--- %s seconds ---" % (time.time() - start_time))
+
+            # Our operations on the frame come here
+            color = frame # cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+
+            img_msg = self.cv_to_imgmsg(color, "bgr8")
+
+            self.publisher.publish(img_msg)
+
+
+            rate.sleep()
+
+    def cv_to_imgmsg(self, cvim, encoding="passthrough"):
+        """
+        Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.
+
+        :param cvim:      An OpenCV :cpp:type:`cv::Mat`
+        :param encoding:  The encoding of the image data, one of the following strings:
+
+           * ``"passthrough"``
+           * one of the standard strings in sensor_msgs/image_encodings.h
+
+        :rtype:           A sensor_msgs.msg.Image message
+        :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding``
+
+        If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type.
+        Otherwise desired_encoding must be one of the standard image encodings
+
+        This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
+        """
+
+        img_msg = Image()
+        img_msg.height = cvim.shape[0]
+        img_msg.width = cvim.shape[1]
+        if len(cvim.shape) < 3:
+            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1)
+        else:
+            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2])
+        if encoding == "passthrough":
+            img_msg.encoding = cv_type
+        else:
+            img_msg.encoding = encoding
+
+        img_msg.data = cvim.tostring()
+        img_msg.step = len(img_msg.data) // img_msg.height
+
+        return img_msg
+
+    def dtype_with_channels_to_cvtype2(self, dtype, n_channels):
+        return '%sC%d' % (self.numpy_type_to_cvtype[dtype.name], n_channels)
+
+
+if __name__ == "__main__":
+
+    try:
+        python_cam()
+    except rospy.ROSInterruptException:
+        pass

ros/src/simulation/scripts/camera_sim_node

@@ -15,7 +15,6 @@ Contact: contact@neilnie.com
 """
 
 import cv2
-import numpy as np
 import os
 import rospy
 from sensor_msgs.msg import Image