F200 initial support

realsense_camera/CMakeLists.txt

@@ -46,6 +46,7 @@ generate_messages(
 #add dynamic reconfigure api
 generate_dynamic_reconfigure_options(
   cfg/r200_params.cfg
+  cfg/f200_params.cfg
 )
 
 #################################
@@ -69,7 +70,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME}_nodelet src/base_nodelet.cpp src/r200_nodelet.cpp)
+add_library(${PROJECT_NAME}_nodelet src/base_nodelet.cpp src/r200_nodelet.cpp src/f200_nodelet.cpp)
 target_link_libraries(${PROJECT_NAME}_nodelet
   ${catkin_LIBRARIES}
   # This should be removed once librealsense is a required component

realsense_camera/README.md

@@ -1,4 +1,5 @@
-#Intel® RealSense™ Technology - ROS Integration 
+#Intel® RealSense™ Technology - ROS Integration
+## Supported Camera Types: R200 and F200
 ###Configuration:
 | Version          | Best Known (indigo)  | Best Known (kinetic) |
 |:---------------- |:---------------------|:---------------------|
@@ -7,6 +8,7 @@
 | Backend          | video4linux          | video4linux          |
 | librealsense     | 0.9.2                | 0.9.2                |
 | R200 Firmware    | 1.0.72.06            | 1.0.72.06            |
+| F200 Firmware    | 2.60.0.0             | 2.60.0.0             |
 
 ###Installation:
 #####Getting the camera to work on Linux
@@ -25,11 +27,7 @@ If this does not work, you should first fix this issue before continuing with th
 * Compile the realsense_camera package by executing the `catkin_make` command.
   Successful execution of command will build target <b>“realsense_camera_nodelet”</b>
 
-Sample launch files are available in "realsense_camera/launch" directory
-* r200_nodelet_rgbd.launch
-* r200_nodelet_default.launch
-* r200_nodelet_modify_params.launch
-* r200_nodelet_multiple_cameras.launch
+Sample launch files are available in [launch](launch/)
 
 ###Package Features:
 
@@ -59,7 +57,7 @@ IR camera
     camera/ir/camera_info
         Calibration data
 
-IR2 camera
+IR2 camera: Available only for <b>R200</b> cameras.
 
     camera/ir2/image_raw (sensor_msgs/Image)
     camera/ir2/camera_info
@@ -76,29 +74,33 @@ IR2 camera
 
 ####Static Parameters
 
-    Stream parameters:
         serial_no (string, default: blank)
             Specify the serial_no to uniquely connect to a camera, especially if multiple cameras are detected by the nodelet.
-            You may get the serial_no from the info stream by launching "r200_nodelet_default.launch"
+            You may get the serial_no from the info stream by launching the default launch file.
         usb_port_id (string, default: blank)
             Alternatively to serial_no, this can be used to connect to a camera by its USB Port ID, which is a 
             Bus Number-Port Number in the format "Bus#-Port#". If used with serial_no, both must match correctly for 
             camera to be connected.
+        camera_type (string, default: blank)
+            Specify the type of the camera - "R200" or "F200".
         mode (string, default: preset)
             Specify the mode to start camera streams. Mode comprises of height, width and fps. 
             Preset mode enables default values whereas Manual mode enables the specified parameter values.
-        color_height (int, default: 480)
-            Specify the color camera height resolution.
+        depth_width (int, default: 480 for R200, 640 for F200)
+            Specify the depth camera width resolution.
+        depth_height (int, default: 360 for R200, 480 for F200)
+            Specify the depth camera height resolution.
         color_width (int, default: 640)
             Specify the color camera width resolution.
-        depth_height (int, default: 360)
-            Specify the depth camera height resolution.
-        depth_width (int, default: 480)
-            Specify the depth camera width resolution.
+        color_height (int, default: 480)
+            Specify the color camera height resolution.
         color_fps (int, default: 60)
             Specify the color camera FPS
         depth_fps (int, default: 60)
             Specify the depth camera FPS
+        enable_depth (bool, default: true) 
+          Specify if to enable or not the depth and infrared camera(s).
+          Note: Infrared stream(s) will be enabled or disabled along with depth stream.
         enable_color (bool, default: true) 
             Specify if to enable or not the color camera.
         enable_pointcloud (bool, default: false) 
@@ -118,12 +120,11 @@ IR2 camera
             Specify the color optical frame id of the camera.
         ir_frame_id (string, default: camera_ir_frame)
             Specify the IR frame id of the camera.
-        ir2_frame_id (string, default: camera_ir2_frame)
+
+Available only for <b>R200</b> cameras.
+
+        ir2_frame_id (string, default: camera_ir2_frame):
             Specify the IR2 frame id of the camera.
-        camera (string, default: "R200")
-            Specify the camera name.
-    Camera parameters: 
-    Following are the parameters that can be set only statically in the R200 camera:
         r200_depth_units : [1, 2147483647]
         r200_depth_clamp_min : [0, 65535]
         r200_depth_clamp_max : [0, 65535]
@@ -140,32 +141,9 @@ IR2 camera
 
 ####Dynamic Parameters
 
-    Stream parameters:
-        enable_depth (bool, default: true) 
-          Specify if to enable or not the depth and infrared camera(s).
-          Note: Infrared stream(s) will be enabled or disabled along with depth stream.
+For <b>R200</b> cameras, refer to [r200_params.cfg](cfg/r200_params.cfg)
 
-    Camera parameters: 
-    Following are the parameters that can be set dynamically as well as statically in the R200 camera.
-        color_backlight_compensation
-        color_brightness
-        color_contrast
-        color_gain
-        color_gamma
-        color_hue
-        color_saturation
-        color_sharpness
-        color_enable_auto_white_balance
-        color_white_balance            (Must be set only if color_enable_auto_white_balance is disabled)
-        r200_lr_gain
-        r200_emitter_enabled
-        r200_lr_exposure               (Must be set only if r200_lr_auto_exposure_enabled is disabled)
-    Following are the parameters that can only be set dynamically in the R200 camera.
-        r200_lr_auto_exposure_enabled
-        r200_auto_exposure_top_edge    (Must be set only if r200_lr_auto_exposure_enabled is enabled)
-        r200_auto_exposure_bottom_edge (Must be set only if r200_lr_auto_exposure_enabled is enabled)
-        r200_auto_exposure_left_edge   (Must be set only if r200_lr_auto_exposure_enabled is enabled)
-        r200_auto_exposure_right_edge  (Must be set only if r200_lr_auto_exposure_enabled is enabled)
+For <b>F200</b> cameras, refer to [f200_params.cfg](cfg/f200_params.cfg)
 
 Note: For Autoexposure edge parameters, max value will go only upto the bounds of the infrared image.
 E.g. For 320x240 infrared image, valid values are within 0-319 and 0-239)
@@ -181,13 +159,20 @@ Command to change dynamic parameters using commandline:
     E.g. $ rosrun dynamic_reconfigure dynparam set /R200Nodelet color_backlight_compensation 2
 
 
-###Running the R200 nodelet:
+###Running the nodelet:
 ####Getting Started
 Use the following command to launch the camera nodelet. You will notice the camera light up.
 
+For <b>R200</b> cameras
+
     $ roslaunch realsense_camera r200_nodelet_default.launch
-
-If you would like to create or use your own launch files, the nodelet name for the R200 camera is R200Nodelet. See the sample launch files for examples of how to launch the nodelet.
+
+For <b>F200</b> cameras
+
+    $ roslaunch realsense_camera f200_nodelet_default.launch
+
+Sample launch files are available in [launch](launch/). 
+Make sure the correct "camera_type" is specified to launch the desired camera.
 
 View using RVIZ:
 
@@ -198,10 +183,6 @@ You can also open RVIZ and load the provided RVIZ configuration file: realsenseR
    $ roscd realsense_camera
    $ rosrun rviz rviz -d rviz/realsenseRvizConfiguration1.rviz
 ```
-For the point cloud stream, before loading its corresponding topic, set the camera_depth_optical_frame using following command:
-
-    $ rosrun tf static_transform_publisher 0.0 0.0 0.0 0.0 0.0 0.0 map camera_depth_optical_frame 100
-
 
 View using other commands:
 For color stream
@@ -225,6 +206,9 @@ For viewing supported camera settings with current values:
     $ rosservice call /camera/get_settings
 
 ####Launching Multiple Cameras
+<b>Note:</b> The following example is based on <b>R200</b> cameras. 
+Similar options are applicable to F200 cameras too just by updating the "camera_type".
+
 For running multiple cameras simultaneously:  
 <b>Option 1:</b> Using single nodelet manager for all the cameras
 * Use "r200_nodelet_multiple_cameras.launch".
@@ -264,17 +248,17 @@ Sample test files are available in "realsense_camera/test" directory
 Refer to the following:
 * [base_nodelet.h](include/base_nodelet.h)
 * [r200_nodelet.h](include/r200_nodelet.h)
+* [f200_nodelet.h](include/f200_nodelet.h)
 
 ###Limitations:
-* Currently, the camera nodelet has been tested to work only for R200 cameras.
 
 * Currently, the camera nodelet only supports the following formats:
     * Color stream:    RGB8
     * Depth stream:    Z16
     * Infrared stream(s): Y8
 
 * Generating a Depth Registered Point Cloud is very memory intensive.
-The topic /camera/depth_registered/points, generated by launch file
+E.g. The topic /camera/depth_registered/points, generated by launch file
 "r200_nodelet_rgbd.launch", works best at 30 fps using 640x480 resolution
 on a system with 16GB of RAM.
 
@@ -287,3 +271,6 @@ Hence the launch file "r200_nodelet_rgbd.launch" will not generate data for the
     * /camera/depth_registered/disparity
 
 * The performance benchmark for multiple cameras launched at the same time has not been defined yet.
+
+* The usb-port-id logic does not work for F200 cameras due to a known librealsense [bug] (https://github.com/IntelRealSense/librealsense/issues/220). Until it gets fixed,
+ use serial_no when multiple F200 cameras are connected to the system.

realsense_camera/cfg/f200_params.cfg

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+PACKAGE="realsense_camera"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#             Name                               Type      Level       Description                     Default    Min     Max
+gen.add("enable_depth",                          bool_t,   0,          "Enable Depth",                 True)
+gen.add("color_backlight_compensation",          int_t,    0,          "Backlight Compensation",       1,         0,      4)
+gen.add("color_brightness",                      int_t,    0,          "Brightness",                   55,        0,      255)
+gen.add("color_contrast",                        int_t,    0,          "Contrast",                     32,        16,     64)
+gen.add("color_gain",                            int_t,    0,          "Gain",                         64,        64,     2540)
+gen.add("color_gamma",                           int_t,    0,          "Gamma",                        220,       100,    280)
+gen.add("color_hue",                             int_t,    0,          "Hue",                          0,         -22,    22)
+gen.add("color_saturation",                      int_t,    0,          "Saturation",                   128,       0,      255)
+gen.add("color_sharpness",                       int_t,    0,          "Sharpness",                    0,         0,      7)
+
+# Must be set only if color_enable_auto_white_balance is disabled
+gen.add("color_white_balance",                   int_t,    0,          "White Balance",                3200,      2500,   6500)
+
+gen.add("color_enable_auto_white_balance",       int_t,    0,          "Enable Auto White Balance",    1,         0,      1)
+gen.add("f200_laser_power",                      int_t,    0,          "Laser Power",                  16,        1,      16)
+gen.add("f200_accuracy",                         int_t,    0,          "Accuracy",                     2,         1,      3)
+gen.add("f200_motion_range",                     int_t,    0,          "Motion Range",                 1,         1,      100)
+gen.add("f200_filter_option",                    int_t,    0,          "Filter Option",                5,         1,      7)
+gen.add("f200_confidence_threshold",             int_t,    0,          "Confidence Threshold",         6,         1,      15)
+
+exit(gen.generate(PACKAGE, "realsense_camera", "f200_params"))

realsense_camera/cfg/r200_params.cfg

@@ -16,12 +16,20 @@ gen.add("color_gamma",                           int_t,    0,          "Gamma",
 gen.add("color_hue",                             int_t,    0,          "Hue",                          0,         -2200,  2200)
 gen.add("color_saturation",                      int_t,    0,          "Saturation",                   128,       0,      255)
 gen.add("color_sharpness",                       int_t,    0,          "Sharpness",                    0,         0,      7)
+
+# Must be set only if color_enable_auto_white_balance is disabled
 gen.add("color_white_balance",                   int_t,    0,          "White Balance",                6500,      2000,   8000)
+
 gen.add("color_enable_auto_white_balance",       int_t,    0,          "Enable Auto White Balance",    1,         0,      1)
 gen.add("r200_lr_auto_exposure_enabled",         int_t,    0,          "LR Auto Exposure Enabled",     0,         0,      1)
 gen.add("r200_lr_gain",                          int_t,    0,          "LR Gain",                      400,       100,    6399)
+
+# Must be set only if r200_lr_auto_exposure_enabled is disabled
 gen.add("r200_lr_exposure",                      int_t,    0,          "LR Exposure",                  164,       1,      164)
+
 gen.add("r200_emitter_enabled",                  int_t,    0,          "Emitter Enabled",              1,         0,      1)
+
+# The following edge parameters must be set only if r200_lr_auto_exposure_enabled is enabled
 gen.add("r200_auto_exposure_top_edge",           int_t,    0,          "Auto Exposure Top Edge",       0,         0,      479)
 gen.add("r200_auto_exposure_bottom_edge",        int_t,    0,          "Auto Exposure Bottom Edge",    479,       0,      479)
 gen.add("r200_auto_exposure_left_edge",          int_t,    0,          "Auto Exposure Left Edge",      0,         0,      639)

realsense_camera/include/base_nodelet.h

@@ -73,45 +73,47 @@ namespace realsense_camera
     // Interfaces.
     virtual void onInit();
     virtual ~BaseNodelet();
-    virtual void publishTransforms();
-    virtual void publishStreams();
-    virtual bool getCameraOptionValues(realsense_camera::cameraConfiguration::Request & req, realsense_camera::cameraConfiguration::Response & res);
+    virtual void prepareTopics();
+    virtual void prepareTransforms();
+    virtual bool getCameraOptionValues(realsense_camera::cameraConfiguration::Request & req,
+        realsense_camera::cameraConfiguration::Response & res);
 
   protected:
 
     // Member Variables.
     ros::NodeHandle nh_;
     ros::NodeHandle pnh_;
-
+    ros::Time topic_ts_;
+    ros::Publisher pointcloud_publisher_;
+    ros::ServiceServer get_options_service_;
     rs_error *rs_error_ = 0;
     rs_context *rs_context_;
     rs_device *rs_device_;
-    int height_[STREAM_COUNT];
+    std::string nodelet_name_;
+    std::string serial_no_;
+    std::string usb_port_id_;
+    std::string camera_type_;
+    std::string mode_;
+    bool enable_[STREAM_COUNT];
     int width_[STREAM_COUNT];
+    int height_[STREAM_COUNT];
     int fps_[STREAM_COUNT];
-    int stream_step_[STREAM_COUNT];
-    int stream_ts_[STREAM_COUNT];
-    int num_streams_ = 3;
+    int step_[STREAM_COUNT];
+    int ts_[STREAM_COUNT];
+    std::string frame_id_[STREAM_COUNT];
+    std::string encoding_[STREAM_COUNT];
+    cv::Mat image_[STREAM_COUNT] = {};
+    image_transport::CameraPublisher camera_publisher_[STREAM_COUNT] = {};
+    sensor_msgs::CameraInfoPtr camera_info_ptr_[STREAM_COUNT] = {};
+    std::string base_frame_id_;
+    std::string depth_frame_id_;
+    std::string color_frame_id_;
     int max_z_ = -1;
-    bool is_device_started_;
-    bool enable_color_;
-    bool enable_depth_;
     bool enable_pointcloud_;
     bool enable_tf_;
-
-    std::string frame_id_[STREAM_COUNT];
-    std::string stream_encoding_[STREAM_COUNT];
-    std::string nodelet_name_;
+    bool duplicate_depth_color_;
     const uint16_t *image_depth16_;
-
-    cv::Mat image_[STREAM_COUNT];
-
-    image_transport::CameraPublisher camera_publisher_[STREAM_COUNT];
-    sensor_msgs::CameraInfoPtr camera_info_ptr_[STREAM_COUNT];
-    ros::Publisher pointcloud_publisher_;
-    ros::ServiceServer get_options_service_;
-
-    boost::shared_ptr<boost::thread> stream_thread_;
+    boost::shared_ptr<boost::thread> topic_thread_;
     boost::shared_ptr<boost::thread> transform_thread_;
 
     struct CameraOptions
@@ -122,20 +124,26 @@ namespace realsense_camera
     std::vector<CameraOptions> camera_options_;
 
     // Member Functions.
-    virtual void listCameras(int num_of_camera);
+    virtual void getParameters();
     virtual bool connectToCamera();
+    virtual std::vector<int> listCameras(int num_of_camera);
+    virtual void advertiseTopics();
+    virtual void advertiseServices();
+    virtual std::vector<std::string> setDynamicReconfServer() { return {}; }; // must be defined in derived class
+    virtual void startDynamicReconfCallback() { return; } // must be defined in derived class
+    virtual void getCameraOptions();
+    virtual void setStaticCameraOptions(std::vector<std::string> dynamic_params);
+    virtual void setStreams();
     virtual void enableStream(rs_stream stream_index, int width, int height, rs_format format, int fps);
+    virtual void getStreamCalibData(rs_stream stream_index);
     virtual void disableStream(rs_stream stream_index);
-    virtual void prepareStreamCalibData(rs_stream stream_index);
-    virtual void prepareStreamData(rs_stream rs_strm);
-    virtual void allocateResources();
-    virtual void fillStreamEncoding();
-    virtual void setStreamOptions();
-    virtual void setStaticCameraOptions() { return; } // must be defined in derived class
-    virtual void getCameraOptions();
+    virtual void startCamera();
+    virtual void stopCamera();
+    virtual void publishTopics();
+    virtual void publishTopic(rs_stream stream_index);
+    virtual void getStreamData(rs_stream stream_index);
+    virtual void publishPCTopic();
     virtual void checkError();
-
-    virtual void publishPointCloud(cv::Mat & image_rgb, ros::Time time_stamp);
   };
 }
 #endif