uvc and astra driver integration

orbbec · Jun 7, 2019 · efd14c7 · efd14c7
1 parent f2e7a84
commit efd14c7
Show file tree

Hide file tree

Showing 56 changed files with 1,079 additions and 16,880 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,6 +5,7 @@ find_package(catkin REQUIRED camera_info_manager dynamic_reconfigure image_trans
 
 find_package(Boost REQUIRED COMPONENTS system thread)
 
+find_package(libuvc REQUIRED)
 
 #MACHINE = $(shell uname -m)
 execute_process(COMMAND uname -m  OUTPUT_VARIABLE  MACHINES )
@@ -31,7 +32,8 @@ ELSEIF ( (${MACHINES} MATCHES "aarch64") AND (${MACHINES_BIT} MATCHES "32")  )
       set(HOST_PLATFORM  "arm")
 ENDIF ()
 
-MESSAGE(STATUS "ORRBEC : ${HOST_PLATFORM}")
+message(STATUS "ORRBEC : ${HOST_PLATFORM}")
+message(STATUS "libuvc ${libuvc_VERSION_MAJOR}.${libuvc_VERSION_MINOR}.${libuvc_VERSION_PATCH}")
 
 generate_dynamic_reconfigure_options(cfg/Astra.cfg)
 
@@ -46,11 +48,12 @@ add_service_files(
   SetIRExposure.srv
   ResetIRGain.srv
   ResetIRExposure.srv
+  GetCameraInfo.srv
 )
 
 generate_messages(
   DEPENDENCIES
-  std_msgs
+  std_msgs sensor_msgs
 )
 
 catkin_package(
@@ -60,15 +63,22 @@ catkin_package(
   DEPENDS libastra
 )
 
+add_definitions(-Dlibuvc_VERSION_MAJOR=${libuvc_VERSION_MAJOR})
+add_definitions(-Dlibuvc_VERSION_MINOR=${libuvc_VERSION_MINOR})
+add_definitions(-Dlibuvc_VERSION_PATCH=${libuvc_VERSION_PATCH})
+
 set(ORRBEC_OPENNI2_REDIST  "${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/${HOST_PLATFORM}")
-link_directories(${ORRBEC_OPENNI2_REDIST})
+link_directories(${ORRBEC_OPENNI2_REDIST}
+                 ${catkin_LINK_DIRS}
+                 )
 
 #MESSAGE(STATUS "ORRBEC : ${ORRBEC_OPENNI2_REDIST}")
 
 include_directories(include
                     ${catkin_INCLUDE_DIRS}
                     ${Boost_INCLUDE_DIRS}
                     ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2
+                    ${libuvc_INCLUDE_DIRS}
                     )
 
 
@@ -89,13 +99,13 @@ add_library(astra_driver_lib
                 src/astra_driver.cpp
 )
 target_link_libraries(astra_driver_lib astra_wrapper ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
-add_dependencies(astra_driver_lib ${PROJECT_NAME}_gencfg)
+add_dependencies(astra_driver_lib ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)
 
 add_library(astra_camera_nodelet
    ros/astra_camera_nodelet.cpp
 )
 target_link_libraries(astra_camera_nodelet astra_driver_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
-add_dependencies(astra_camera_nodelet ${PROJECT_NAME}_gencfg)
+add_dependencies(astra_camera_nodelet ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)
 
 add_executable(astra_camera_node
    ros/astra_camera_node.cpp
@@ -115,6 +125,14 @@ if (UNIX AND NOT APPLE)
   set(ADDITIONAL_EXECUTABLES "astra_usb_reset")
 endif()
 
+add_executable(camera_node src/libuvc_camera/main.cpp src/libuvc_camera/camera_driver.cpp)
+target_link_libraries(camera_node ${libuvc_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+add_dependencies(camera_node ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)
+
+add_library(libuvc_camera_nodelet src/libuvc_camera/nodelet.cpp src/libuvc_camera/camera_driver.cpp)
+target_link_libraries(libuvc_camera_nodelet ${libuvc_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+add_dependencies(libuvc_camera_nodelet ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)
+
 install(TARGETS astra_wrapper astra_camera_nodelet astra_camera_node astra_list_devices astra_driver_lib ${ADDITIONAL_EXECUTABLES}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -153,3 +171,13 @@ install(FILES 56-orbbec-usb.rules
 install(DIRECTORY scripts
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+install(TARGETS camera_node libuvc_camera_nodelet
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES libuvc_camera_nodelet.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/README.md b/README.md
@@ -40,13 +40,3 @@ You will need to clone the launch files from [astra_launch repository](https://g
 `roslaunch astra_launch stereo_s.launch`
 
 You can use **rviz** or **image_view** to verify the outputs.
-
-# Windows 10 Subsystem
-
-If you encountered `udev` error on Windows 10 Subsystem, try to remove `upstart` and `udev`.
-
-```sh
-apt-get remove upstart
-apt-get remove udev
-apt-get autoremove
-```
diff --git a/astra_nodelets.xml b/astra_nodelets.xml
@@ -6,4 +6,12 @@
     </description>
   </class>
 
+  <class name="libuvc_camera/driver"
+         type="libuvc_camera::CameraNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description> 
+      UVC camera driver nodelet.
+    </description>
+  </class>
+
 </library>
diff --git a/cfg/UVCCamera.cfg b/cfg/UVCCamera.cfg
@@ -0,0 +1,191 @@
+#! /usr/bin/env python2
+# Derived from camera1394 cfg
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+RECONFIGURE_CLOSE = 3 # Parameters that need a sensor to be stopped completely when changed
+RECONFIGURE_STOP = 1 # Parameters that need a sensor to stop streaming when changed
+RECONFIGURE_RUNNING = 0 # Parameters that can be changed while a sensor is streaming
+
+gen = ParameterGenerator()
+
+#       Name, Type, Reconfiguration level, Description, Default, Min, Max
+
+gen.add("vendor", str_t, RECONFIGURE_CLOSE,
+        "Vendor ID, hex digits (use camera of any vendor if null).",
+        "")
+
+gen.add("product", str_t, RECONFIGURE_CLOSE,
+        "Product ID, hex digits (use camera of any model if null).",
+        "")
+
+gen.add("serial", str_t, RECONFIGURE_CLOSE,
+        "Serial number, arbitrary string (use camera of any serial number if null).",
+        "")
+
+gen.add("index", int_t, RECONFIGURE_CLOSE,
+        "Index into the list of cameras that match the above parameters.",
+        0, 0)
+
+gen.add("width", int_t, RECONFIGURE_CLOSE,
+        "Image width.", 640, 0)
+
+gen.add("height", int_t, RECONFIGURE_CLOSE,
+        "Image height.", 480, 0)
+
+video_modes = gen.enum([gen.const("uncompressed", str_t, "uncompressed", "Use any uncompressed format"),
+                        gen.const("compressed", str_t, "compressed", "User any compressed format"),
+                        gen.const("yuyv", str_t, "yuyv", "YUYV"),
+                        gen.const("uyvy", str_t, "uyvy", "UYVY"),
+                        gen.const("rgb", str_t, "rgb", "RGB"),
+                        gen.const("bgr", str_t, "bgr", "BGR"),
+                        gen.const("mjpeg", str_t, "mjpeg", "MJPEG"),
+                        gen.const("gray8", str_t, "gray8", "gray8")],
+                       "Video stream format")
+
+gen.add("video_mode", str_t, RECONFIGURE_CLOSE,
+        "Format of video stream from camera.", "uncompressed",
+        edit_method = video_modes)
+
+gen.add("frame_rate", double_t, RECONFIGURE_CLOSE,
+        "Camera speed, frames per second.", 15.0, 0.1, 1000.0)
+
+timestamp_methods = gen.enum([gen.const("PubTime", str_t, "pub", "Time of publication"),
+                              gen.const("FrameStartTime", str_t, "start", "Time when raw frame capture began"),
+                              gen.const("FrameStopTime", str_t, "stop", "Time when raw frame capture ended"),
+                              gen.const("HostReceiptTime", str_t, "hostrcpt", "Time when camera-to-host transfer completed")],
+                             "Methods for determining the timestamp")
+
+gen.add("timestamp_method", str_t, RECONFIGURE_CLOSE,
+        "Method for determining the timestamp.", "start",
+        edit_method = timestamp_methods)
+
+gen.add("frame_id", str_t, RECONFIGURE_RUNNING,
+        "ROS tf frame of reference, resolved with tf_prefix unless absolute.",
+        "camera")
+
+gen.add("camera_info_url", str_t, RECONFIGURE_RUNNING,
+        "Path to camera calibration file.", "")
+
+# Camera Terminal controls
+
+scanning_modes = gen.enum([gen.const("Interlaced", int_t, 0, ""),
+                           gen.const("Progressive", int_t, 1, "")],
+                          "Scanning modes")
+
+gen.add("scanning_mode", int_t, RECONFIGURE_RUNNING,
+        "Scanning mode.", 0, 0, 1,
+        edit_method = scanning_modes)
+
+auto_exposure_modes = gen.enum([gen.const("Manual", int_t, 0, "Manual exposure, manual iris"),
+                                gen.const("Auto", int_t, 1, "Auto exposure, auto iris"),
+                                gen.const("Shutter_Priority", int_t, 2, "manual exposure, auto iris"),
+                                gen.const("Aperture_Priority", int_t, 3, "auto exposure, manual iris")],
+                               "Auto-exposure modes")
+
+gen.add("auto_exposure", int_t, RECONFIGURE_RUNNING,
+        "Auto exposure mode.",
+        3, 0, 3, edit_method = auto_exposure_modes)
+
+gen.add("auto_exposure_priority", int_t, RECONFIGURE_RUNNING,
+        "In auto mode or shutter priority mode, allow the device to vary frame rate.",
+        0, 0, 1)
+
+gen.add("exposure_absolute", double_t, RECONFIGURE_RUNNING,
+        "Length of exposure, seconds.", 0., 0.0001, 10.0)
+
+# TODO: relative exposure time
+
+gen.add("iris_absolute", double_t, RECONFIGURE_RUNNING,
+        "Aperture, f.", 0., 0., 655.35)
+
+# TODO: relative iris
+
+gen.add("auto_focus", bool_t, RECONFIGURE_RUNNING,
+        "Maintain focus automatically.", True)
+
+gen.add("focus_absolute", int_t, RECONFIGURE_RUNNING,
+        "Absolute focal distance, millimeters.", 0, 0, 65536)
+
+# TODO: relative focus
+
+# TODO: zoom
+
+gen.add("pan_absolute", int_t, RECONFIGURE_RUNNING,
+        "Pan (clockwise), arc seconds.", 0, -180*3600, 180*3600)
+
+gen.add("tilt_absolute", int_t, RECONFIGURE_RUNNING,
+        "Tilt (up), arc seconds.", 0, -180*3600, 180*3600)
+
+# TODO: relative pan/tilt
+
+gen.add("roll_absolute", int_t, RECONFIGURE_RUNNING,
+        "Roll (clockwise), degrees.", 0, -180, 180)
+
+# TODO: relative roll
+
+gen.add("privacy", bool_t, RECONFIGURE_RUNNING,
+        "Image capture disabled.", False)
+
+# Processing Unit controls
+
+gen.add("backlight_compensation", int_t, RECONFIGURE_RUNNING,
+        "Backlight compensation, device-dependent (zero for none, increasing compensation above zero).",
+        0, 0, 65536)
+
+
+gen.add("brightness", int_t, RECONFIGURE_RUNNING,
+        "Brightness, device dependent.", 0, -32768, 32767)
+
+gen.add("contrast", int_t, RECONFIGURE_RUNNING,
+        "Contrast, device dependent.", 0, -32768, 32767)
+
+gen.add("gain", int_t, RECONFIGURE_RUNNING,
+        "Gain, device dependent.", 0, 0, 65536)
+
+power_line_frequency_modes = gen.enum([gen.const("Disabled", int_t, 0, "Disabled"),
+                                       gen.const("Freq_50", int_t, 1, "50 Hz"),
+                                       gen.const("Freq_60", int_t, 1, "60 Hz")],
+                                      "Power line frequency modes")
+
+gen.add("power_line_frequency", int_t, RECONFIGURE_RUNNING,
+        "Power line frequency anti-flicker processing.",
+        0, 0, 2,
+        edit_method = power_line_frequency_modes)
+
+gen.add("auto_hue", bool_t, RECONFIGURE_RUNNING,
+        "Automatic hue control.", False)
+
+gen.add("hue", double_t, RECONFIGURE_RUNNING,
+        "Hue, degrees.", 0., -180., 180.)
+
+gen.add("saturation", int_t, RECONFIGURE_RUNNING,
+        "Saturation, device dependent (zero for grayscale).", 0, 0, 65536)
+
+gen.add("sharpness", int_t, RECONFIGURE_RUNNING,
+        "Image sharpness, device dependent.",
+        0, 0, 65536)
+
+# TODO: check range definition
+gen.add("gamma", double_t, RECONFIGURE_RUNNING,
+        "Gamma.", 1.0, 0.01, 5.0)
+
+gen.add("auto_white_balance", bool_t, RECONFIGURE_RUNNING,
+        "Automatic white balance.", False)
+
+gen.add("white_balance_temperature", int_t, RECONFIGURE_RUNNING,
+        "White balance temperature, degrees.", 0, 0, 65536)
+
+gen.add("white_balance_BU", double_t, RECONFIGURE_RUNNING,
+        "Blue or U component of white balance, device-dependent.",
+        0, 0, 65536)
+
+gen.add("white_balance_RV", double_t, RECONFIGURE_RUNNING,
+        "Red or V component of white balance, device-dependent.",
+        0, 0, 65536)
+
+# TODO: digital multiplier {,limit}
+
+# TODO: analog video standard, analog video lock
+
+exit(gen.generate('libuvc_camera', "libuvc_camera", "UVCCamera"))
diff --git a/include/astra_camera/astra_device.h b/include/astra_camera/astra_device.h
@@ -67,7 +67,7 @@ class AstraFrameListener;
 class AstraDevice
 {
 public:
-  AstraDevice(const std::string& device_URI) throw (AstraException);
+  AstraDevice(const std::string& device_URI);
   virtual ~AstraDevice();
 
   const std::string getUri() const;
@@ -99,12 +99,12 @@ class AstraDevice
   bool isDepthStreamStarted();
 
   bool isImageRegistrationModeSupported() const;
-  void setImageRegistrationMode(bool enabled) throw (AstraException);
-  void setDepthColorSync(bool enabled) throw (AstraException);
+  void setImageRegistrationMode(bool enabled);
+  void setDepthColorSync(bool enabled);
 
-  const AstraVideoMode getIRVideoMode() throw (AstraException);
-  const AstraVideoMode getColorVideoMode() throw (AstraException);
-  const AstraVideoMode getDepthVideoMode() throw (AstraException);
+  const AstraVideoMode getIRVideoMode();
+  const AstraVideoMode getColorVideoMode();
+  const AstraVideoMode getDepthVideoMode();
 
   const std::vector<AstraVideoMode>& getSupportedIRVideoModes() const;
   const std::vector<AstraVideoMode>& getSupportedColorVideoModes() const;
@@ -114,9 +114,9 @@ class AstraDevice
   bool isColorVideoModeSupported(const AstraVideoMode& video_mode) const;
   bool isDepthVideoModeSupported(const AstraVideoMode& video_mode) const;
 
-  void setIRVideoMode(const AstraVideoMode& video_mode) throw (AstraException);
-  void setColorVideoMode(const AstraVideoMode& video_mode) throw (AstraException);
-  void setDepthVideoMode(const AstraVideoMode& video_mode) throw (AstraException);
+  void setIRVideoMode(const AstraVideoMode& video_mode);
+  void setColorVideoMode(const AstraVideoMode& video_mode);
+  void setDepthVideoMode(const AstraVideoMode& video_mode);
 
   void setIRFrameCallback(FrameCallbackFunction callback);
   void setColorFrameCallback(FrameCallbackFunction callback);
@@ -137,8 +137,8 @@ class AstraDevice
   void setIRExposure(int exposure);
   void setLaser(bool enable);
 
-  void setAutoExposure(bool enable) throw (AstraException);
-  void setAutoWhiteBalance(bool enable) throw (AstraException);
+  void setAutoExposure(bool enable);
+  void setAutoWhiteBalance(bool enable);
 
   bool getAutoExposure() const;
   bool getAutoWhiteBalance() const;
@@ -148,9 +148,9 @@ class AstraDevice
 protected:
   void shutdown();
 
-  boost::shared_ptr<openni::VideoStream> getIRVideoStream() const throw (AstraException);
-  boost::shared_ptr<openni::VideoStream> getColorVideoStream() const throw (AstraException);
-  boost::shared_ptr<openni::VideoStream> getDepthVideoStream() const throw (AstraException);
+  boost::shared_ptr<openni::VideoStream> getIRVideoStream() const;
+  boost::shared_ptr<openni::VideoStream> getColorVideoStream() const;
+  boost::shared_ptr<openni::VideoStream> getDepthVideoStream() const;
 
   boost::shared_ptr<openni::Device> openni_device_;
   boost::shared_ptr<openni::DeviceInfo> device_info_;