diff --git a/realsense_camera/CMakeLists.txt b/realsense_camera/CMakeLists.txt index 4a90135bc9..a03a14c20d 100644 --- a/realsense_camera/CMakeLists.txt +++ b/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 diff --git a/realsense_camera/README.md b/realsense_camera/README.md index 9b2ce5ea93..6fff909622 100644 --- a/realsense_camera/README.md +++ b/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 “realsense_camera_nodelet” -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 R200 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 R200 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 R200 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 F200 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 R200 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 F200 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 +Note: The following example is based on R200 cameras. +Similar options are applicable to F200 cameras too just by updating the "camera_type". + For running multiple cameras simultaneously: Option 1: Using single nodelet manager for all the cameras * Use "r200_nodelet_multiple_cameras.launch". @@ -264,9 +248,9 @@ 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 @@ -274,7 +258,7 @@ Refer to the following: * 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. diff --git a/realsense_camera/cfg/f200_params.cfg b/realsense_camera/cfg/f200_params.cfg new file mode 100755 index 0000000000..8555dcb8c7 --- /dev/null +++ b/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")) diff --git a/realsense_camera/cfg/r200_params.cfg b/realsense_camera/cfg/r200_params.cfg index 645acaffd6..56737b9cac 100755 --- a/realsense_camera/cfg/r200_params.cfg +++ b/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) diff --git a/realsense_camera/include/base_nodelet.h b/realsense_camera/include/base_nodelet.h index ba9b5bef59..27f900bc9e 100644 --- a/realsense_camera/include/base_nodelet.h +++ b/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 stream_thread_; + boost::shared_ptr topic_thread_; boost::shared_ptr transform_thread_; struct CameraOptions @@ -122,20 +124,26 @@ namespace realsense_camera std::vector camera_options_; // Member Functions. - virtual void listCameras(int num_of_camera); + virtual void getParameters(); virtual bool connectToCamera(); + virtual std::vector listCameras(int num_of_camera); + virtual void advertiseTopics(); + virtual void advertiseServices(); + virtual std::vector 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 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 diff --git a/realsense_camera/include/constants.h b/realsense_camera/include/constants.h index 917159e345..5ccf5504b3 100644 --- a/realsense_camera/include/constants.h +++ b/realsense_camera/include/constants.h @@ -36,10 +36,10 @@ namespace realsense_camera { // Default Constants. const int STREAM_COUNT = 4; - const int DEPTH_HEIGHT = 360; const int DEPTH_WIDTH = 480; - const int COLOR_HEIGHT = 480; + const int DEPTH_HEIGHT = 360; const int COLOR_WIDTH = 640; + const int COLOR_HEIGHT = 480; const int DEPTH_FPS = 60; const int COLOR_FPS = 60; const bool ENABLE_DEPTH = true; @@ -67,7 +67,9 @@ namespace realsense_camera const double ROTATION_IDENTITY[] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // R200 Constants. - const int R200_STREAM_COUNT = 4; const int R200_MAX_Z = 10; // in meters + + // F200 Constants. + const int F200_MAX_Z = 1; // in meters } #endif diff --git a/realsense_camera/include/f200_nodelet.h b/realsense_camera/include/f200_nodelet.h new file mode 100644 index 0000000000..7230a81823 --- /dev/null +++ b/realsense_camera/include/f200_nodelet.h @@ -0,0 +1,59 @@ +/****************************************************************************** + Copyright (c) 2016, Intel Corporation + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once +#ifndef F200_NODELET +#define F200_NODELET + +#include + +#include "realsense_camera/f200_paramsConfig.h" +#include "base_nodelet.h" + +namespace realsense_camera +{ + class F200Nodelet: public realsense_camera::BaseNodelet + { + public: + + void onInit(); + + protected: + + // Member Variables. + boost::shared_ptr> dynamic_reconf_server_; + + // Member Functions. + std::vector setDynamicReconfServer(); + void startDynamicReconfCallback(); + void configCallback(realsense_camera::f200_paramsConfig &config, uint32_t level); + }; +} +#endif diff --git a/realsense_camera/include/r200_nodelet.h b/realsense_camera/include/r200_nodelet.h index 2871dd263a..fb84bc4aea 100644 --- a/realsense_camera/include/r200_nodelet.h +++ b/realsense_camera/include/r200_nodelet.h @@ -43,7 +43,6 @@ namespace realsense_camera { public: - // Initialize camera void onInit(); protected: @@ -58,14 +57,13 @@ namespace realsense_camera boost::shared_ptr> dynamic_reconf_server_; // Member Functions. - void enableStream(rs_stream stream_index, int width, int height, rs_format format, int fps); - void disableStream(rs_stream stream_index); - void setStreamOptions(); - void fillStreamEncoding(); - void allocateResources(); - void setStaticCameraOptions(); + void getParameters(); + void advertiseTopics(); + std::vector setDynamicReconfServer(); + void startDynamicReconfCallback(); void configCallback(realsense_camera::r200_paramsConfig &config, uint32_t level); + void setStreams(); + void publishTopics(); }; } - #endif diff --git a/realsense_camera/launch/f200_nodelet_default.launch b/realsense_camera/launch/f200_nodelet_default.launch new file mode 100644 index 0000000000..10ca083eca --- /dev/null +++ b/realsense_camera/launch/f200_nodelet_default.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/realsense_camera/launch/includes/nodelet.launch.xml b/realsense_camera/launch/includes/nodelet.launch.xml index 6a25be991b..80712c6e34 100644 --- a/realsense_camera/launch/includes/nodelet.launch.xml +++ b/realsense_camera/launch/includes/nodelet.launch.xml @@ -13,6 +13,7 @@ args="load realsense_camera/$(arg camera_type)Nodelet $(arg manager)"> + diff --git a/realsense_camera/launch/includes/nodelet_rgbd.launch.xml b/realsense_camera/launch/includes/nodelet_rgbd.launch.xml index 1735a58844..8708330aef 100644 --- a/realsense_camera/launch/includes/nodelet_rgbd.launch.xml +++ b/realsense_camera/launch/includes/nodelet_rgbd.launch.xml @@ -39,6 +39,7 @@ args="load realsense_camera/$(arg camera_type)Nodelet $(arg manager)"> + diff --git a/realsense_camera/launch/r200_nodelet_modify_params.launch b/realsense_camera/launch/r200_nodelet_modify_params.launch index 7b706a2cef..d34804bdd4 100644 --- a/realsense_camera/launch/r200_nodelet_modify_params.launch +++ b/realsense_camera/launch/r200_nodelet_modify_params.launch @@ -43,4 +43,3 @@ - diff --git a/realsense_camera/nodelet_plugins.xml b/realsense_camera/nodelet_plugins.xml index f6c7e0472e..5087c4b31b 100644 --- a/realsense_camera/nodelet_plugins.xml +++ b/realsense_camera/nodelet_plugins.xml @@ -4,4 +4,10 @@ Intel(R) RealSense(TM) R200 Camera nodelet. + + + + Intel(R) RealSense(TM) F200 Camera nodelet. + + diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index fb94fbdc8f..4666b9000d 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -36,136 +36,104 @@ using namespace std; PLUGINLIB_EXPORT_CLASS (realsense_camera::BaseNodelet, nodelet::Nodelet) namespace realsense_camera { - /* - * Public Methods. + * Nodelet Destructor. */ BaseNodelet::~BaseNodelet() { - stream_thread_->join(); + topic_thread_->join(); if (enable_tf_ == true) { transform_thread_->join(); } - // Stop device. - if (is_device_started_ == true) - { - ROS_INFO_STREAM(nodelet_name_ << " - Stopping camera"); - rs_stop_device(rs_device_, 0); - rs_delete_context(rs_context_, &rs_error_); - checkError(); - } + stopCamera(); + + rs_delete_context(rs_context_, &rs_error_); + checkError(); ROS_INFO_STREAM(nodelet_name_ << " - Stopping..."); ros::shutdown(); } /* - * Initialize the realsense camera + * Initialize the nodelet. */ void BaseNodelet::onInit() { - // Set default configurations. - is_device_started_ = false; + getParameters(); - for (int i = 0; i < num_streams_; ++i) - { - camera_info_ptr_[i] = NULL; - stream_ts_[i] = -1; - } - - setStreamOptions(); - - if (enable_depth_ == false && enable_color_ == false) + if (enable_[RS_STREAM_DEPTH] == false && enable_[RS_STREAM_COLOR] == false) { ROS_ERROR_STREAM(nodelet_name_ << " - None of the streams are enabled. Exiting!"); ros::shutdown(); } - // Advertise the various topics and services. - image_transport::ImageTransport it (nh_); - camera_publisher_[RS_STREAM_COLOR] = it.advertiseCamera(COLOR_TOPIC, 1); - camera_publisher_[RS_STREAM_DEPTH] = it.advertiseCamera(DEPTH_TOPIC, 1); - camera_publisher_[RS_STREAM_INFRARED] = it.advertiseCamera(IR_TOPIC, 1); - - pointcloud_publisher_ = nh_.advertise(PC_TOPIC, 1); - - get_options_service_ = nh_.advertiseService(SETTINGS_SERVICE, &BaseNodelet::getCameraOptionValues, this); - - // Poll for camera and connect if found - while (!connectToCamera()) + while (!connectToCamera()) // Poll for camera and connect if found { ROS_INFO_STREAM(nodelet_name_ << " - Sleeping 5 seconds then retrying to connect"); ros::Duration(5).sleep(); } - // Start working thread. - stream_thread_ = - boost::shared_ptr (new boost::thread (boost::bind(&BaseNodelet::publishStreams, this))); + advertiseTopics(); + advertiseServices(); + std::vector dynamic_params = setDynamicReconfServer(); + getCameraOptions(); + setStaticCameraOptions(dynamic_params); + setStreams(); + startCamera(); + + // Start topic thread. + topic_thread_ = + boost::shared_ptr (new boost::thread (boost::bind(&BaseNodelet::prepareTopics, this))); + // Start tranform thread. if (enable_tf_ == true) { transform_thread_ = - boost::shared_ptr(new boost::thread (boost::bind(&BaseNodelet::publishTransforms, this))); + boost::shared_ptr(new boost::thread (boost::bind(&BaseNodelet::prepareTransforms, this))); } + + // Start dynamic reconfigure callback + startDynamicReconfCallback(); } /* - *Protected Methods. + * Get the nodelet parameters. */ - void BaseNodelet::enableStream(rs_stream stream_index, int width, int height, rs_format format, int fps) + void BaseNodelet::getParameters() { - std::string stream_mode; - pnh_.param("mode", stream_mode, DEFAULT_MODE); - - // Enable streams. - if (stream_mode.compare("manual") == 0) - { - ROS_INFO_STREAM(nodelet_name_ << " - Enabling " << STREAM_DESC[stream_index] << " stream: manual mode"); - rs_enable_stream(rs_device_, stream_index, width, height, format, fps, &rs_error_); - checkError(); - } - else - { - ROS_INFO_STREAM(nodelet_name_ << " - Enabling " << STREAM_DESC[stream_index] << " stream: preset mode"); - rs_enable_stream_preset(rs_device_, stream_index, RS_PRESET_BEST_QUALITY, &rs_error_); - checkError(); - } - - if (camera_info_ptr_[stream_index] == NULL) - { - prepareStreamCalibData(stream_index); - } - } - - void BaseNodelet::disableStream(rs_stream stream_index) - { - ROS_INFO_STREAM(nodelet_name_ << " - Disabling " << STREAM_DESC[stream_index] << " stream"); - rs_disable_stream(rs_device_, stream_index, &rs_error_); - checkError(); - } - - void BaseNodelet::checkError() - { - if (rs_error_) - { - ROS_ERROR_STREAM(nodelet_name_ << " - Error calling " << rs_get_failed_function(rs_error_) << " ( " - << rs_get_failed_args(rs_error_) << " ): \n" << rs_get_error_message(rs_error_) << " \n"); - rs_free_error(rs_error_); - - ros::shutdown(); - - exit (EXIT_FAILURE); - } + nodelet_name_ = getName(); + nh_ = getNodeHandle(); + pnh_ = getPrivateNodeHandle(); + pnh_.getParam("serial_no", serial_no_); + pnh_.getParam("usb_port_id", usb_port_id_); + pnh_.getParam("camera_type", camera_type_); + pnh_.param("mode", mode_, DEFAULT_MODE); + pnh_.param("enable_depth", enable_[RS_STREAM_DEPTH], ENABLE_DEPTH); + pnh_.param("enable_color", enable_[RS_STREAM_COLOR], ENABLE_COLOR); + pnh_.param("enable_pointcloud", enable_pointcloud_, ENABLE_PC); + pnh_.param("enable_tf", enable_tf_, ENABLE_TF); + pnh_.param("depth_width", width_[RS_STREAM_DEPTH], DEPTH_WIDTH); + pnh_.param("depth_height", height_[RS_STREAM_DEPTH], DEPTH_HEIGHT); + pnh_.param("color_width", width_[RS_STREAM_COLOR], COLOR_WIDTH); + pnh_.param("color_height", height_[RS_STREAM_COLOR], COLOR_HEIGHT); + pnh_.param("depth_fps", fps_[RS_STREAM_DEPTH], DEPTH_FPS); + pnh_.param("color_fps", fps_[RS_STREAM_COLOR], COLOR_FPS); + pnh_.param("depth_optical_frame_id", frame_id_[RS_STREAM_DEPTH], DEFAULT_DEPTH_OPTICAL_FRAME_ID); + pnh_.param("color_optical_frame_id", frame_id_[RS_STREAM_COLOR], DEFAULT_COLOR_OPTICAL_FRAME_ID); + pnh_.param("base_frame_id", base_frame_id_, DEFAULT_BASE_FRAME_ID); + pnh_.param("color_frame_id", color_frame_id_, DEFAULT_COLOR_FRAME_ID); + pnh_.param("depth_frame_id", depth_frame_id_, DEFAULT_DEPTH_FRAME_ID); + pnh_.param("ir_frame_id", frame_id_[RS_STREAM_INFRARED], DEFAULT_IR_FRAME_ID); } + /* + * Connect to camera. + */ bool BaseNodelet::connectToCamera() { - std::string serial_no; - std::string usb_port_id; - rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_); checkError(); @@ -182,15 +150,22 @@ namespace realsense_camera } // Print list of all cameras found - listCameras(num_of_cameras); + std::vector camera_type_index = listCameras(num_of_cameras); - pnh_.getParam("serial_no", serial_no); - pnh_.getParam("usb_port_id", usb_port_id); + // Exit with error if no cameras of correct type are connected. + if (camera_type_index.size() < 1) + { + ROS_ERROR_STREAM(nodelet_name_ << " - No '" << camera_type_ << "' cameras detected!"); + rs_delete_context(rs_context_, &rs_error_); + checkError(); + return false; + } - // Exit with error if no serial number or usb_port_id is specified and multiple cameras are detected. - if (serial_no.empty() && usb_port_id.empty() && num_of_cameras > 1) + // Exit with error if no serial_no or usb_port_id is specified and multiple cameras are detected. + if (serial_no_.empty() && usb_port_id_.empty() && camera_type_index.size() > 1) { - ROS_ERROR_STREAM(nodelet_name_ << " - Multiple cameras detected but no input serial_no or usb_port_id specified"); + ROS_ERROR_STREAM(nodelet_name_ << + " - Multiple cameras of same type detected but no input serial_no or usb_port_id specified"); rs_delete_context(rs_context_, &rs_error_); checkError(); return false; @@ -200,16 +175,18 @@ namespace realsense_camera rs_device_ = nullptr; // find camera - for (int i = 0; i < num_of_cameras; i++) + for (int i: camera_type_index) { rs_device* rs_detected_device = rs_get_device(rs_context_, i, &rs_error_); + checkError(); // check serial_no and usb_port_id - if ((serial_no.empty() || serial_no == rs_get_device_serial(rs_detected_device, &rs_error_)) && - (usb_port_id.empty() || usb_port_id == rs_get_device_usb_port_id(rs_detected_device, &rs_error_))) + if ((serial_no_.empty() || serial_no_ == rs_get_device_serial(rs_detected_device, &rs_error_)) && + (usb_port_id_.empty() || usb_port_id_ == rs_get_device_usb_port_id(rs_detected_device, &rs_error_))) { // device found rs_device_= rs_detected_device; + break; } // continue loop } @@ -218,8 +195,8 @@ namespace realsense_camera { // camera not found string error_msg = " - Couldn't find camera to connect with "; - error_msg += "serial_no = " + serial_no + ", "; - error_msg += "usb_port_id = " + usb_port_id; + error_msg += "serial_no = " + serial_no_ + ", "; + error_msg += "usb_port_id = " + usb_port_id_ ; ROS_ERROR_STREAM(nodelet_name_ << error_msg); @@ -231,56 +208,89 @@ namespace realsense_camera // print device info ROS_INFO_STREAM(nodelet_name_ << " - Connecting to camera with Serial No: " << rs_get_device_serial(rs_device_, &rs_error_) << - " USB Port ID: " << rs_get_device_usb_port_id(rs_device_, &rs_error_)); + ", USB Port ID: " << rs_get_device_usb_port_id(rs_device_, &rs_error_)); checkError(); - - // Enable streams. - if (enable_color_ == true) - { - enableStream(RS_STREAM_COLOR, width_[RS_STREAM_COLOR], height_[RS_STREAM_COLOR], COLOR_FORMAT, fps_[RS_STREAM_COLOR]); - } - if (enable_depth_ == true) - { - enableStream(RS_STREAM_DEPTH, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], DEPTH_FORMAT, fps_[RS_STREAM_DEPTH]); - enableStream(RS_STREAM_INFRARED, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], IR_FORMAT, fps_[RS_STREAM_DEPTH]); - } - getCameraOptions(); - setStaticCameraOptions(); - - // Start device. - ROS_INFO_STREAM(nodelet_name_ << " - Starting camera"); - rs_start_device(rs_device_, &rs_error_); - checkError(); - - is_device_started_ = true; - - allocateResources(); - return true; } - void BaseNodelet::listCameras(int num_of_cameras) + /* + * List details of the detected cameras. + */ + std::vector BaseNodelet::listCameras(int num_of_cameras) { + // print list of detected cameras + std::vector camera_type_index; std::string detected_camera_msg = " - Detected the following cameras:"; for (int i = 0; i < num_of_cameras; i++) { // get device rs_device* rs_detected_device = rs_get_device(rs_context_, i, &rs_error_); + // get camera name + std::string camera_name = rs_get_device_name(rs_detected_device, &rs_error_); + checkError(); + + if (camera_name.find(camera_type_) != std::string::npos) + { + camera_type_index.push_back(i); + } // print device details detected_camera_msg = detected_camera_msg + "\n\t\t\t\t- Serial No: " + rs_get_device_serial(rs_detected_device, &rs_error_) + - "; Firmware: " + rs_get_device_firmware_version(rs_detected_device, &rs_error_) + - "; Name: " + rs_get_device_name(rs_detected_device, &rs_error_) + - "; USB Port ID: " + rs_get_device_usb_port_id(rs_detected_device, &rs_error_); + ", USB Port ID: " + rs_get_device_usb_port_id(rs_detected_device, &rs_error_) + + ", Name: " + camera_name + + ", Firmware: " + rs_get_device_firmware_version(rs_detected_device, &rs_error_); checkError(); } ROS_INFO_STREAM(nodelet_name_ + detected_camera_msg); + return camera_type_index; + } + + /* + * Advertise topics. + */ + void BaseNodelet::advertiseTopics() + { + image_transport::ImageTransport image_transport(nh_); + camera_publisher_[RS_STREAM_COLOR] = image_transport.advertiseCamera(COLOR_TOPIC, 1); + camera_publisher_[RS_STREAM_DEPTH] = image_transport.advertiseCamera(DEPTH_TOPIC, 1); + camera_publisher_[RS_STREAM_INFRARED] = image_transport.advertiseCamera(IR_TOPIC, 1); + pointcloud_publisher_ = nh_.advertise(PC_TOPIC, 1); + } + + /* + * Advertise services. + */ + void BaseNodelet::advertiseServices() + { + get_options_service_ = nh_.advertiseService(SETTINGS_SERVICE, &BaseNodelet::getCameraOptionValues, this); + } + + /* + * Get the latest values of the camera options. + */ + bool BaseNodelet::getCameraOptionValues(realsense_camera::cameraConfiguration::Request & req, + realsense_camera::cameraConfiguration::Response & res) + { + std::string get_options_result_str; + std::string opt_name, opt_value; + + for (CameraOptions o: camera_options_) + { + opt_name = rs_option_to_string(o.opt); + std::transform(opt_name.begin(), opt_name.end(), opt_name.begin(), ::tolower); + o.value = rs_get_device_option(rs_device_, o.opt, 0); + opt_value = boost::lexical_cast(o.value); + get_options_result_str += opt_name + ":" + opt_value + ";"; + } + + res.configuration_str = get_options_result_str; + return true; } /* - * Gets the options supported by the camera along with their min, max and step values. + * Get the options supported by the camera along with their min, max and step values. */ void BaseNodelet::getCameraOptions() { @@ -303,22 +313,147 @@ namespace realsense_camera } /* - * Define buffer for images and prepare camera info for each enabled stream. + * Set the static camera options. + */ + void BaseNodelet::setStaticCameraOptions(std::vector dynamic_params) + { + ROS_INFO_STREAM(nodelet_name_ << " - Setting static camera options"); + + // Iterate through the supported camera options + for (CameraOptions o: camera_options_) + { + std::string opt_name = rs_option_to_string(o.opt); + bool found = false; + + for (std::string param_name: dynamic_params) + { + std::transform(opt_name.begin(), opt_name.end(), opt_name.begin(), ::tolower); + if (opt_name.compare(param_name) == 0) + { + found = true; + break; + } + } + // Skip the dynamic options and set only the static camera options. + if (found == false) + { + std::string key; + double val; + + if (pnh_.searchParam(opt_name, key)) + { + double opt_val; + pnh_.getParam(key, val); + + // Validate and set the input values within the min-max range + if (val < o.min) + { + opt_val = o.min; + } + else if (val > o.max) + { + opt_val = o.max; + } + else + { + opt_val = val; + } + ROS_DEBUG_STREAM(nodelet_name_ << " - " << opt_name << " = " << opt_val); + rs_set_device_option(rs_device_, o.opt, opt_val, &rs_error_); + checkError(); + } + } + } + } + + /* + * Set the streams according to their corresponding flag values. */ - void BaseNodelet::allocateResources() + void BaseNodelet::setStreams() { - // Prepare camera for enabled streams (color/depth/infrared) - fillStreamEncoding(); + // Enable streams. + if (enable_[RS_STREAM_COLOR] == true) + { + enableStream(RS_STREAM_COLOR, width_[RS_STREAM_COLOR], height_[RS_STREAM_COLOR], COLOR_FORMAT, + fps_[RS_STREAM_COLOR]); + if (camera_info_ptr_[RS_STREAM_COLOR] == NULL) + { + ROS_DEBUG_STREAM(nodelet_name_ << " - Allocating resources for " << STREAM_DESC[RS_STREAM_COLOR]); + getStreamCalibData(RS_STREAM_COLOR); + step_[RS_STREAM_COLOR] = camera_info_ptr_[RS_STREAM_COLOR]->width * sizeof(unsigned char) * 3; + image_[RS_STREAM_COLOR] = cv::Mat(camera_info_ptr_[RS_STREAM_COLOR]->height, + camera_info_ptr_[RS_STREAM_COLOR]->width, CV_8UC3, cv::Scalar(0, 0, 0)); + } + ts_[RS_STREAM_COLOR] = -1; + encoding_[RS_STREAM_COLOR] = sensor_msgs::image_encodings::RGB8; + } + else if (enable_[RS_STREAM_COLOR] == false) + { + disableStream(RS_STREAM_COLOR); + } + + if (enable_[RS_STREAM_DEPTH] == true) + { + enableStream(RS_STREAM_DEPTH, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], DEPTH_FORMAT, + fps_[RS_STREAM_DEPTH]); + if (camera_info_ptr_[RS_STREAM_DEPTH] == NULL) + { + ROS_DEBUG_STREAM(nodelet_name_ << " - Allocating resources for " << STREAM_DESC[RS_STREAM_DEPTH]); + getStreamCalibData(RS_STREAM_DEPTH); + step_[RS_STREAM_DEPTH] = camera_info_ptr_[RS_STREAM_DEPTH]->width * sizeof(uint16_t); + image_[RS_STREAM_DEPTH] = cv::Mat(camera_info_ptr_[RS_STREAM_DEPTH]->height, + camera_info_ptr_[RS_STREAM_DEPTH]->width, CV_16UC1, cv::Scalar(0, 0, 0)); + } + ts_[RS_STREAM_DEPTH] = -1; + encoding_[RS_STREAM_DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1; + + enableStream(RS_STREAM_INFRARED, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], IR_FORMAT, + fps_[RS_STREAM_DEPTH]); + if (camera_info_ptr_[RS_STREAM_INFRARED] == NULL) + { + ROS_DEBUG_STREAM(nodelet_name_ << " - Allocating resources for " << STREAM_DESC[RS_STREAM_INFRARED]); + getStreamCalibData(RS_STREAM_INFRARED); + step_[RS_STREAM_INFRARED] = camera_info_ptr_[RS_STREAM_INFRARED]->width * sizeof(unsigned char); + image_[RS_STREAM_INFRARED] = cv::Mat(camera_info_ptr_[RS_STREAM_INFRARED]->height, + camera_info_ptr_[RS_STREAM_INFRARED]->width, CV_8UC1, cv::Scalar(0, 0, 0)); + } + + ts_[RS_STREAM_INFRARED] = -1; + encoding_[RS_STREAM_INFRARED] = sensor_msgs::image_encodings::TYPE_8UC1; + } + else if (enable_[RS_STREAM_DEPTH] == false) + { + disableStream(RS_STREAM_DEPTH); + disableStream(RS_STREAM_INFRARED); + } + } - image_[RS_STREAM_COLOR] = cv::Mat(height_[RS_STREAM_COLOR], width_[RS_STREAM_COLOR], CV_8UC3, cv::Scalar (0, 0, 0)); - image_[RS_STREAM_DEPTH] = cv::Mat(height_[RS_STREAM_DEPTH], width_[RS_STREAM_DEPTH], CV_16UC1, cv::Scalar (0)); - image_[RS_STREAM_INFRARED] = cv::Mat(height_[RS_STREAM_DEPTH], width_[RS_STREAM_DEPTH], CV_8UC1, cv::Scalar (0)); + /* + * Enable individual streams. + */ + void BaseNodelet::enableStream(rs_stream stream_index, int width, int height, rs_format format, int fps) + { + if (rs_is_stream_enabled(rs_device_, stream_index, 0) == 0) + { + if (mode_.compare("manual") == 0) + { + ROS_INFO_STREAM(nodelet_name_ << " - Enabling " << STREAM_DESC[stream_index] << " in manual mode"); + rs_enable_stream(rs_device_, stream_index, width, height, format, fps, &rs_error_); + checkError(); + } + else + { + ROS_INFO_STREAM(nodelet_name_ << " - Enabling " << STREAM_DESC[stream_index] << " in preset mode"); + rs_enable_stream_preset(rs_device_, stream_index, RS_PRESET_BEST_QUALITY, &rs_error_); + checkError(); + } + } } /* * Prepare camera_info for each enabled stream. */ - void BaseNodelet::prepareStreamCalibData(rs_stream stream_index) + void BaseNodelet::getStreamCalibData(rs_stream stream_index) { rs_intrinsics intrinsic; rs_get_stream_intrinsics(rs_device_, stream_index, &intrinsic, &rs_error_); @@ -383,121 +518,70 @@ namespace realsense_camera } /* - * Get the latest values of the camera options. + * Disable individual streams. */ - bool BaseNodelet::getCameraOptionValues(realsense_camera::cameraConfiguration::Request & req, - realsense_camera::cameraConfiguration::Response & res) + void BaseNodelet::disableStream(rs_stream stream_index) { - std::string get_options_result_str; - std::string opt_name, opt_value; - - for (CameraOptions o: camera_options_) + if (rs_is_stream_enabled(rs_device_, stream_index, 0) == 1) { - opt_name = rs_option_to_string(o.opt); - std::transform(opt_name.begin(), opt_name.end(), opt_name.begin(), ::tolower); - o.value = rs_get_device_option(rs_device_, o.opt, 0); - opt_value = boost::lexical_cast(o.value); - get_options_result_str += opt_name + ":" + opt_value + ";"; + ROS_INFO_STREAM(nodelet_name_ << " - Disabling " << STREAM_DESC[stream_index] << " stream"); + rs_disable_stream(rs_device_, stream_index, &rs_error_); + checkError(); } - - res.configuration_str = get_options_result_str; - return true; } /* - * Set the stream options based on input params. + * Start camera. */ - void BaseNodelet::setStreamOptions() + void BaseNodelet::startCamera() { - pnh_.param("enable_depth", enable_depth_, ENABLE_DEPTH); - pnh_.param("enable_color", enable_color_, ENABLE_COLOR); - pnh_.param("enable_pointcloud", enable_pointcloud_, ENABLE_PC); - pnh_.param("enable_tf", enable_tf_, ENABLE_TF); - pnh_.param("depth_width", width_[RS_STREAM_DEPTH], DEPTH_WIDTH); - pnh_.param("depth_height", height_[RS_STREAM_DEPTH], DEPTH_HEIGHT); - pnh_.param("color_width", width_[RS_STREAM_COLOR], COLOR_WIDTH); - pnh_.param("color_height", height_[RS_STREAM_COLOR], COLOR_HEIGHT); - pnh_.param("depth_fps", fps_[RS_STREAM_DEPTH], DEPTH_FPS); - pnh_.param("color_fps", fps_[RS_STREAM_COLOR], COLOR_FPS); - pnh_.param("depth_optical_frame_id", frame_id_[RS_STREAM_DEPTH], DEFAULT_DEPTH_OPTICAL_FRAME_ID); - pnh_.param("color_optical_frame_id", frame_id_[RS_STREAM_COLOR], DEFAULT_COLOR_OPTICAL_FRAME_ID); - pnh_.param("ir_frame_id", frame_id_[RS_STREAM_INFRARED], DEFAULT_IR_FRAME_ID); + if (rs_is_device_streaming(rs_device_, 0) == 0) + { + ROS_INFO_STREAM(nodelet_name_ << " - Starting camera"); + rs_start_device(rs_device_, &rs_error_); + checkError(); + } } - /* - * Copy frame data from realsense to member cv images. + * Stop camera. */ - void BaseNodelet::prepareStreamData(rs_stream rs_strm) + void BaseNodelet::stopCamera() { - if (rs_strm == RS_STREAM_DEPTH) + if (rs_is_device_streaming(rs_device_, 0) == 1) { - // fill depth buffer - image_depth16_ = reinterpret_cast (rs_get_frame_data(rs_device_, RS_STREAM_DEPTH, 0)); + ROS_INFO_STREAM(nodelet_name_ << " - Stopping camera"); + rs_stop_device(rs_device_, 0); + checkError(); } - // fill image buffer for stream - image_[(uint32_t) rs_strm].data = (unsigned char *) (rs_get_frame_data(rs_device_, rs_strm, 0)); } /* - * Populate the encodings for each stream. + * Copy frame data from realsense to member cv images. */ - void BaseNodelet::fillStreamEncoding() + void BaseNodelet::getStreamData(rs_stream stream_index) { - stream_encoding_[RS_STREAM_COLOR] = "rgb8"; - stream_step_[RS_STREAM_COLOR] = width_[RS_STREAM_COLOR] * sizeof (unsigned char) * 3; - stream_encoding_[RS_STREAM_DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1; - stream_step_[RS_STREAM_DEPTH] = width_[RS_STREAM_DEPTH] * sizeof (uint16_t); - stream_encoding_[RS_STREAM_INFRARED] = sensor_msgs::image_encodings::TYPE_8UC1; - stream_step_[RS_STREAM_INFRARED] = width_[RS_STREAM_DEPTH] * sizeof (unsigned char); + if (stream_index == RS_STREAM_DEPTH) + { + // fill depth buffer + image_depth16_ = reinterpret_cast (rs_get_frame_data(rs_device_, RS_STREAM_DEPTH, 0)); + } + // fill image buffer for stream + image_[(uint32_t) stream_index].data = (unsigned char *) (rs_get_frame_data(rs_device_, stream_index, 0)); } /* - * Publish streams. + * Prepare and publish topics. */ - void BaseNodelet::publishStreams() + void BaseNodelet::prepareTopics() { while (ros::ok()) { - if (enable_depth_ == false && rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 1) + if (enable_[RS_STREAM_DEPTH] != rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0)) { - if (rs_is_device_streaming(rs_device_, 0) == 1) - { - ROS_INFO_STREAM(nodelet_name_ << " - Stopping camera"); - rs_stop_device(rs_device_, &rs_error_); - checkError(); - } - - // disable Depth and IR stream - disableStream(RS_STREAM_DEPTH); - disableStream(RS_STREAM_INFRARED); - - if (rs_is_device_streaming(rs_device_, 0) == 0) - { - ROS_INFO_STREAM(nodelet_name_ << " - Starting camera"); - rs_start_device(rs_device_, &rs_error_); - checkError(); - } - } - - if (enable_depth_ == true && rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 0) - { - if (rs_is_device_streaming(rs_device_, 0) == 1) - { - ROS_INFO_STREAM(nodelet_name_ << " - Stopping camera"); - rs_stop_device(rs_device_, &rs_error_); - checkError(); - } - - enableStream(RS_STREAM_DEPTH, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], DEPTH_FORMAT, fps_[RS_STREAM_DEPTH]); - enableStream(RS_STREAM_INFRARED, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], IR_FORMAT, fps_[RS_STREAM_DEPTH]); - - if (rs_is_device_streaming(rs_device_, 0) == 0) - { - ROS_INFO_STREAM(nodelet_name_ << " - Starting camera"); - rs_start_device(rs_device_, &rs_error_); - checkError(); - } + stopCamera(); + setStreams(); + startCamera(); } if (rs_is_device_streaming(rs_device_, 0) == 1) @@ -505,68 +589,84 @@ namespace realsense_camera rs_wait_for_frames(rs_device_, &rs_error_); checkError(); - ros::Time time_stamp = ros::Time::now(); - bool duplicate_depth_color = false; - - for (int stream_index = 0; stream_index < num_streams_; ++stream_index) - { - // Publish image stream only if there is at least one subscriber. - if (camera_publisher_[stream_index].getNumSubscribers() > 0 && - rs_is_stream_enabled(rs_device_, (rs_stream) stream_index, 0) == 1) - { - int current_ts = rs_get_frame_timestamp(rs_device_, (rs_stream) stream_index, 0); - if (stream_ts_[stream_index] != current_ts) // Publish frames only if its not duplicate - { - prepareStreamData((rs_stream) stream_index); - - sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), - stream_encoding_[stream_index], - image_[stream_index]).toImageMsg(); - - msg->header.frame_id = frame_id_[stream_index]; - msg->header.stamp = time_stamp; // Publish timestamp to synchronize frames. - msg->width = image_[stream_index].cols; - msg->height = image_[stream_index].rows; - msg->is_bigendian = false; - msg->step = stream_step_[stream_index]; + topic_ts_ = ros::Time::now(); + duplicate_depth_color_ = false; - camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; - camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); - } - else - { - if ((stream_index == RS_STREAM_DEPTH) || (stream_index == RS_STREAM_COLOR)) - { - duplicate_depth_color = true; // Set this flag to true if Depth and/or Color frame is duplicate - } - } - stream_ts_[stream_index] = current_ts; - } - } + publishTopics(); if (pointcloud_publisher_.getNumSubscribers() > 0 && rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 1 && enable_pointcloud_ == true && - (duplicate_depth_color == false)) // Skip publishing PointCloud if Depth and/or Color frame was duplicate + (duplicate_depth_color_ == false)) // Skip publishing PointCloud if Depth and/or Color frame was duplicate { if (camera_publisher_[RS_STREAM_DEPTH].getNumSubscribers() <= 0) { - prepareStreamData(RS_STREAM_DEPTH); + getStreamData(RS_STREAM_DEPTH); } if (camera_publisher_[RS_STREAM_COLOR].getNumSubscribers() <= 0) { - prepareStreamData(RS_STREAM_COLOR); + getStreamData(RS_STREAM_COLOR); } - publishPointCloud(image_[RS_STREAM_COLOR], time_stamp); + publishPCTopic(); } } } } /* - * Publish pointcloud. + * Call publicTopic() for all streams. */ - void BaseNodelet::publishPointCloud (cv::Mat & image_color, ros::Time time_stamp) + void BaseNodelet::publishTopics() { + publishTopic(RS_STREAM_DEPTH); + publishTopic(RS_STREAM_COLOR); + publishTopic(RS_STREAM_INFRARED); + } + + /* + * Publish topic. + */ + void BaseNodelet::publishTopic(rs_stream stream_index) + { + // Publish stream only if there is at least one subscriber. + if (camera_publisher_[stream_index].getNumSubscribers() > 0 && + rs_is_stream_enabled(rs_device_, (rs_stream) stream_index, 0) == 1) + { + int current_ts = rs_get_frame_timestamp(rs_device_, (rs_stream) stream_index, 0); + if (ts_[stream_index] != current_ts) // Publish frames only if its not duplicate + { + getStreamData(stream_index); + + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), + encoding_[stream_index], + image_[stream_index]).toImageMsg(); + + msg->header.frame_id = frame_id_[stream_index]; + msg->header.stamp = topic_ts_; // Publish timestamp to synchronize frames. + msg->width = image_[stream_index].cols; + msg->height = image_[stream_index].rows; + msg->is_bigendian = false; + msg->step = step_[stream_index]; + + camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; + camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); + } + else + { + if ((stream_index == RS_STREAM_DEPTH) || (stream_index == RS_STREAM_COLOR)) + { + duplicate_depth_color_ = true; // Set this flag to true if Depth and/or Color frame is duplicate + } + } + ts_[stream_index] = current_ts; + } + } + + /* + * Publish pointcloud topic. + */ + void BaseNodelet::publishPCTopic() + { + cv::Mat & image_color = image_[RS_STREAM_COLOR]; // Publish pointcloud only if there is at least one subscriber. if (pointcloud_publisher_.getNumSubscribers() > 0) { @@ -577,7 +677,7 @@ namespace realsense_camera rs_get_stream_intrinsics(rs_device_, RS_STREAM_DEPTH, &z_intrinsic, &rs_error_); checkError(); - if (enable_color_ == true) + if (enable_[RS_STREAM_COLOR] == true) { rs_get_stream_intrinsics(rs_device_, RS_STREAM_COLOR, &color_intrinsic, &rs_error_); checkError(); @@ -589,7 +689,7 @@ namespace realsense_camera sensor_msgs::PointCloud2 msg_pointcloud; msg_pointcloud.width = width_[RS_STREAM_DEPTH]; msg_pointcloud.height = height_[RS_STREAM_DEPTH]; - msg_pointcloud.header.stamp = time_stamp; + msg_pointcloud.header.stamp = topic_ts_; msg_pointcloud.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); @@ -641,7 +741,7 @@ namespace realsense_camera *iter_g = (uint8_t) 255; *iter_b = (uint8_t) 255; - if (enable_color_ == true) + if (enable_[RS_STREAM_COLOR] == true) { rs_transform_point_to_point(color_point, &z_extrinsic, depth_point); rs_project_point_to_pixel(color_pixel, &color_intrinsic, color_point); @@ -675,10 +775,11 @@ namespace realsense_camera pointcloud_publisher_.publish(msg_pointcloud); } } + /* - * Publish camera transforms + * Prepare and publish transforms. */ - void BaseNodelet::publishTransforms() + void BaseNodelet::prepareTransforms() { // publish transforms for the cameras ROS_INFO_STREAM(nodelet_name_ << " - Publishing camera transforms"); @@ -691,43 +792,51 @@ namespace realsense_camera rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); checkError(); - std::string base_frame_id; - std::string color_frame_id; - std::string depth_frame_id; - pnh_.param("base_frame_id", base_frame_id, DEFAULT_BASE_FRAME_ID); - pnh_.param("color_frame_id", color_frame_id, DEFAULT_COLOR_FRAME_ID); - pnh_.param("depth_frame_id", depth_frame_id, DEFAULT_DEPTH_FRAME_ID); - ros::Duration sleeper(0.1); // 100ms while (ros::ok()) { // time stamp is future dated to be valid for given duration - ros::Time time_stamp = ros::Time::now() + sleeper; + ros::Time transform_ts = ros::Time::now() + sleeper; // transform base frame to depth frame tr.setOrigin(tf::Vector3(z_extrinsic.translation[2], -z_extrinsic.translation[0], -z_extrinsic.translation[1])); tr.setRotation(tf::Quaternion(0, 0, 0, 1)); - tf_broadcaster.sendTransform(tf::StampedTransform(tr, time_stamp, base_frame_id, depth_frame_id)); + tf_broadcaster.sendTransform(tf::StampedTransform(tr, transform_ts, base_frame_id_, depth_frame_id_)); // transform depth frame to depth optical frame tr.setOrigin(tf::Vector3(0,0,0)); q.setEuler( M_PI/2, 0.0, -M_PI/2 ); tr.setRotation( q ); - tf_broadcaster.sendTransform(tf::StampedTransform(tr, time_stamp, depth_frame_id, frame_id_[RS_STREAM_DEPTH])); + tf_broadcaster.sendTransform(tf::StampedTransform(tr, transform_ts, depth_frame_id_, frame_id_[RS_STREAM_DEPTH])); // transform base frame to color frame (these are the same) tr.setOrigin(tf::Vector3(0,0,0)); tr.setRotation(tf::Quaternion(0, 0, 0, 1)); - tf_broadcaster.sendTransform(tf::StampedTransform(tr, time_stamp, base_frame_id, color_frame_id)); + tf_broadcaster.sendTransform(tf::StampedTransform(tr, transform_ts, base_frame_id_, color_frame_id_)); // transform color frame to color optical frame tr.setOrigin(tf::Vector3(0,0,0)); q.setEuler( M_PI/2, 0.0, -M_PI/2 ); tr.setRotation( q ); - tf_broadcaster.sendTransform(tf::StampedTransform(tr, time_stamp, color_frame_id, frame_id_[RS_STREAM_COLOR])); + tf_broadcaster.sendTransform(tf::StampedTransform(tr, transform_ts, color_frame_id_, frame_id_[RS_STREAM_COLOR])); sleeper.sleep(); // need sleep or transform won't publish correctly } } + + /* + * Display error details and shutdown ROS. + */ + void BaseNodelet::checkError() + { + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Error calling " << rs_get_failed_function(rs_error_) << " ( " + << rs_get_failed_args(rs_error_) << " ): \n" << rs_get_error_message(rs_error_) << " \n"); + rs_free_error(rs_error_); + ros::shutdown(); + exit (EXIT_FAILURE); + } + } } // end namespace diff --git a/realsense_camera/src/f200_nodelet.cpp b/realsense_camera/src/f200_nodelet.cpp new file mode 100644 index 0000000000..a29910e65e --- /dev/null +++ b/realsense_camera/src/f200_nodelet.cpp @@ -0,0 +1,123 @@ +/****************************************************************************** + Copyright (c) 2016, Intel Corporation + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#include "f200_nodelet.h" + +PLUGINLIB_EXPORT_CLASS (realsense_camera::F200Nodelet, nodelet::Nodelet) + +namespace realsense_camera +{ + /* + * Initialize the nodelet. + */ + void F200Nodelet::onInit() + { + max_z_ = F200_MAX_Z; + BaseNodelet::onInit(); + } + + /* + * Set Dynamic Reconfigure Server and return the dynamic params. + */ + std::vector F200Nodelet::setDynamicReconfServer() + { + dynamic_reconf_server_.reset(new dynamic_reconfigure::Server(pnh_)); + + // Get dynamic options from the dynamic reconfigure server. + realsense_camera::f200_paramsConfig params_config; + dynamic_reconf_server_->getConfigDefault(params_config); + std::vector param_desc = + params_config.__getParamDescriptions__(); + std::vector dynamic_params; + for (realsense_camera::f200_paramsConfig::AbstractParamDescriptionConstPtr param_desc_ptr: param_desc) + { + dynamic_params.push_back((* param_desc_ptr).name); + } + + return dynamic_params; + } + + /* + * Start Dynamic Reconfigure Callback. + */ + void F200Nodelet::startDynamicReconfCallback() + { + dynamic_reconf_server_->setCallback(boost::bind(&F200Nodelet::configCallback, this, _1, _2)); + } + + /* + * Get the dynamic param values. + */ + void F200Nodelet::configCallback(realsense_camera::f200_paramsConfig &config, uint32_t level) + { + ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options"); + // Set flags + if (config.enable_depth == false) + { + if (enable_[RS_STREAM_COLOR] == false) + { + ROS_INFO_STREAM(nodelet_name_ << " - Color stream is also disabled. Cannot disable depth stream"); + config.enable_depth = true; + } + else + { + enable_[RS_STREAM_DEPTH] = false; + } + } + else + { + enable_[RS_STREAM_DEPTH] = true; + } + + // Set common options + rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, + config.color_enable_auto_white_balance, 0); + if (config.color_enable_auto_white_balance == 0) + { + rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0); + } + + // Set F200 specific options + rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, config.f200_laser_power, 0); + rs_set_device_option(rs_device_, RS_OPTION_F200_ACCURACY, config.f200_accuracy, 0); + rs_set_device_option(rs_device_, RS_OPTION_F200_MOTION_RANGE, config.f200_motion_range, 0); + rs_set_device_option(rs_device_, RS_OPTION_F200_FILTER_OPTION, config.f200_filter_option, 0); + rs_set_device_option(rs_device_, RS_OPTION_F200_CONFIDENCE_THRESHOLD, config.f200_confidence_threshold, 0); + } +} // end namespace + diff --git a/realsense_camera/src/r200_nodelet.cpp b/realsense_camera/src/r200_nodelet.cpp index 40f84f0287..2d6fc7bd07 100644 --- a/realsense_camera/src/r200_nodelet.cpp +++ b/realsense_camera/src/r200_nodelet.cpp @@ -35,103 +35,111 @@ PLUGINLIB_EXPORT_CLASS (realsense_camera::R200Nodelet, nodelet::Nodelet) namespace realsense_camera { /* - * Public Methods. - */ - - /* - * Initialize the realsense camera + * Initialize the nodelet. */ void R200Nodelet::onInit() { - // set member vars used in base class - nodelet_name_ = getName(); - nh_ = getNodeHandle(); - pnh_ = getPrivateNodeHandle(); - num_streams_ = R200_STREAM_COUNT; max_z_ = R200_MAX_Z; - - // create dynamic reconfigure server - this must be done before calling base class onInit() - // onInit() calls setStaticCameraOptions() which relies on this being set already - dynamic_reconf_server_.reset(new dynamic_reconfigure::Server(pnh_)); - - // call base class onInit() method BaseNodelet::onInit(); + } + + /* + * Get the nodelet parameters. + */ + void R200Nodelet::getParameters() + { + BaseNodelet::getParameters(); + pnh_.param("ir2_frame_id", frame_id_[RS_STREAM_INFRARED2], DEFAULT_IR2_FRAME_ID); + } - // Set up the IR2 frame and topics - image_transport::ImageTransport it (nh_); - camera_publisher_[RS_STREAM_INFRARED2] = it.advertiseCamera(IR2_TOPIC, 1); + /* + * Advertise topics. + */ + void R200Nodelet::advertiseTopics() + { + BaseNodelet::advertiseTopics(); - // setCallback can only be called after rs_device_ is set by base class connectToCamera() - dynamic_reconf_server_->setCallback(boost::bind(&R200Nodelet::configCallback, this, _1, _2)); + image_transport::ImageTransport image_transport(nh_); + camera_publisher_[RS_STREAM_INFRARED2] = image_transport.advertiseCamera(IR2_TOPIC, 1); } /* - *Protected Methods. + * Set Dynamic Reconfigure Server and return the dynamic params. */ - void R200Nodelet::enableStream(rs_stream stream_index, int width, int height, rs_format format, int fps) + std::vector R200Nodelet::setDynamicReconfServer() { - BaseNodelet::enableStream(stream_index, width, height, format, fps); - if (stream_index == RS_STREAM_INFRARED) + dynamic_reconf_server_.reset(new dynamic_reconfigure::Server(pnh_)); + + // Get dynamic options from the dynamic reconfigure server. + realsense_camera::r200_paramsConfig params_config; + dynamic_reconf_server_->getConfigDefault(params_config); + std::vector param_desc = + params_config.__getParamDescriptions__(); + std::vector dynamic_params; + for (realsense_camera::r200_paramsConfig::AbstractParamDescriptionConstPtr param_desc_ptr: param_desc) { - enableStream(RS_STREAM_INFRARED2, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], IR_FORMAT, fps_[RS_STREAM_DEPTH]); + dynamic_params.push_back((* param_desc_ptr).name); } + + return dynamic_params; } - void R200Nodelet::disableStream(rs_stream stream_index) + /* + * Start Dynamic Reconfigure Callback. + */ + void R200Nodelet::startDynamicReconfCallback() { - BaseNodelet::disableStream(stream_index); - if (stream_index == RS_STREAM_INFRARED) - { - disableStream(RS_STREAM_INFRARED2); - } + dynamic_reconf_server_->setCallback(boost::bind(&R200Nodelet::configCallback, this, _1, _2)); } + /* + * Get the dynamic param values. + */ void R200Nodelet::configCallback(realsense_camera::r200_paramsConfig &config, uint32_t level) { - // Set common options - rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0); - rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, config.color_enable_auto_white_balance, 0); - - if (config.color_enable_auto_white_balance == 0) - { - rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0); - } - + ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options"); + // Set flags if (config.enable_depth == false) { - if (enable_color_ == false) + if (enable_[RS_STREAM_COLOR] == false) { ROS_INFO_STREAM(nodelet_name_ << " - Color stream is also disabled. Cannot disable depth stream"); config.enable_depth = true; } else { - enable_depth_ = false; + enable_[RS_STREAM_DEPTH] = false; } } else { - enable_depth_ = true; + enable_[RS_STREAM_DEPTH] = true; } - //R200 camera specific options - rs_set_device_option(rs_device_, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, config.r200_lr_auto_exposure_enabled, 0); + // Set common options + rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0); + rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, + config.color_enable_auto_white_balance, 0); + if (config.color_enable_auto_white_balance == 0) + { + rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0); + } + // Set R200 specific options + rs_set_device_option(rs_device_, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, config.r200_lr_auto_exposure_enabled, 0); if (config.r200_lr_auto_exposure_enabled == 0) { rs_set_device_option(rs_device_, RS_OPTION_R200_LR_EXPOSURE, config.r200_lr_exposure, 0); } - rs_set_device_option(rs_device_, RS_OPTION_R200_LR_GAIN, config.r200_lr_gain, 0); rs_set_device_option(rs_device_, RS_OPTION_R200_EMITTER_ENABLED, config.r200_emitter_enabled, 0); - if (config.r200_lr_auto_exposure_enabled == 1) { if (config.r200_auto_exposure_top_edge >= height_[RS_STREAM_DEPTH]) @@ -150,110 +158,51 @@ namespace realsense_camera { config.r200_auto_exposure_right_edge = width_[RS_STREAM_DEPTH] - 1; } - double edge_values_[4]; edge_values_[0] = config.r200_auto_exposure_left_edge; edge_values_[1] = config.r200_auto_exposure_top_edge; edge_values_[2] = config.r200_auto_exposure_right_edge; edge_values_[3] = config.r200_auto_exposure_bottom_edge; - rs_set_device_options(rs_device_, edge_options_, 4, edge_values_, 0); } } /* - * Define buffer for images and prepare camera info for each enabled stream. - */ - void R200Nodelet::allocateResources() - { - // call base class method first - BaseNodelet::allocateResources(); - // set IR2 image buffer - image_[(uint32_t) RS_STREAM_INFRARED2] = cv::Mat(height_[RS_STREAM_DEPTH], width_[RS_STREAM_DEPTH], CV_8UC1, cv::Scalar (0)); - } - - /* - * Set the stream options based on input params. + * Set the streams according to their corresponding flag values. */ - void R200Nodelet::setStreamOptions() + void R200Nodelet::setStreams() { - // call base class method first - BaseNodelet::setStreamOptions(); - // setup R200 specific frame - pnh_.param("ir2_frame_id", frame_id_[RS_STREAM_INFRARED2], DEFAULT_IR2_FRAME_ID); - } + BaseNodelet::setStreams(); - /* - * Populate the encodings for each stream. - */ - void R200Nodelet::fillStreamEncoding() - { - // Call base class method first - BaseNodelet::fillStreamEncoding(); - // Setup IR2 stream - stream_encoding_[(uint32_t) RS_STREAM_INFRARED2] = sensor_msgs::image_encodings::TYPE_8UC1; - stream_step_[(uint32_t) RS_STREAM_INFRARED2] = width_[RS_STREAM_DEPTH] * sizeof (unsigned char); + if (enable_[RS_STREAM_DEPTH] == true) + { + enableStream(RS_STREAM_INFRARED2, width_[RS_STREAM_DEPTH], height_[RS_STREAM_DEPTH], IR_FORMAT, + fps_[RS_STREAM_DEPTH]); + if (camera_info_ptr_[RS_STREAM_INFRARED2] == NULL) + { + ROS_DEBUG_STREAM(nodelet_name_ << " - Allocating resources for " << STREAM_DESC[RS_STREAM_INFRARED2]); + getStreamCalibData(RS_STREAM_INFRARED2); + step_[RS_STREAM_INFRARED2] = camera_info_ptr_[RS_STREAM_INFRARED2]->width * sizeof(unsigned char); + image_[RS_STREAM_INFRARED2] = cv::Mat(camera_info_ptr_[RS_STREAM_INFRARED2]->height, + camera_info_ptr_[RS_STREAM_INFRARED2]->width, CV_8UC1, cv::Scalar(0, 0, 0)); + } + ts_[RS_STREAM_INFRARED2] = -1; + encoding_[RS_STREAM_INFRARED2] = sensor_msgs::image_encodings::TYPE_8UC1; + } + else if (enable_[RS_STREAM_DEPTH] == false) + { + disableStream(RS_STREAM_INFRARED2); + } } /* - * Set the static camera options. + * Publish topics for native streams. */ - void R200Nodelet::setStaticCameraOptions() + void R200Nodelet::publishTopics() { - ROS_INFO_STREAM(nodelet_name_ << " - Setting camera options"); - - // Get dynamic options from the dynamic reconfigure server. - realsense_camera::r200_paramsConfig params_config; - dynamic_reconf_server_->getConfigDefault(params_config); - - std::vector param_desc = params_config.__getParamDescriptions__(); - - // Iterate through the supported camera options - for (CameraOptions o: camera_options_) - { - std::string opt_name = rs_option_to_string(o.opt); - bool found = false; - - std::vector::iterator it; - for (realsense_camera::r200_paramsConfig::AbstractParamDescriptionConstPtr param_desc_ptr: param_desc) - { - std::transform(opt_name.begin(), opt_name.end(), opt_name.begin(), ::tolower); - if (opt_name.compare((* param_desc_ptr).name) == 0) - { - found = true; - break; - } - } - // Skip the dynamic options and set only the static camera options. - if (found == false) - { - std::string key; - double val; + BaseNodelet::publishTopics(); - if (pnh_.searchParam(opt_name, key)) - { - double opt_val; - pnh_.getParam(key, val); - - // Validate and set the input values within the min-max range - if (val < o.min) - { - opt_val = o.min; - } - else if (val > o.max) - { - opt_val = o.max; - } - else - { - opt_val = val; - } - ROS_INFO_STREAM(nodelet_name_ << " - Static Options: " << opt_name << " = " << opt_val); - rs_set_device_option(rs_device_, o.opt, opt_val, &rs_error_); - checkError(); - } - } - } + publishTopic(RS_STREAM_INFRARED2); } } // end namespace diff --git a/realsense_camera/test/camera_core.cpp b/realsense_camera/test/camera_core.cpp index 9404131a80..e51b3aacde 100644 --- a/realsense_camera/test/camera_core.cpp +++ b/realsense_camera/test/camera_core.cpp @@ -86,6 +86,11 @@ void imageInfrared1Callback(const sensor_msgs::ImageConstPtr &msg, const sensor_ getMsgInfo(RS_STREAM_INFRARED, msg); getCameraInfo(RS_STREAM_INFRARED, info_msg); + for (unsigned int i = 0; i < 5; i++) + { + g_infrared1_caminfo_D_recv[i] = info_msg->D[i]; + } + g_infrared1_recv = true; } @@ -114,6 +119,11 @@ void imageInfrared2Callback(const sensor_msgs::ImageConstPtr &msg, const sensor_ getMsgInfo(RS_STREAM_INFRARED2, msg); getCameraInfo(RS_STREAM_INFRARED2, info_msg); + for (unsigned int i = 0; i < 5; i++) + { + g_infrared2_caminfo_D_recv[i] = info_msg->D[i]; + } + g_infrared2_recv = true; } @@ -141,6 +151,11 @@ void imageDepthCallback(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs getMsgInfo(RS_STREAM_DEPTH, msg); getCameraInfo(RS_STREAM_DEPTH, info_msg); + for (unsigned int i = 0; i < 5; i++) + { + g_depth_caminfo_D_recv[i] = info_msg->D[i]; + } + g_depth_recv = true; } @@ -268,15 +283,18 @@ TEST(RealsenseTests, testColorCameraInfo) EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_COLOR][10] != (double) 0); EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_COLOR][11], (double) 0); - float color_caminfo_D = 1; - // ignoring the 5th value since it always appears to be 0.0 on tested R200 cameras - for (unsigned int i = 0; i < 4; i++) + // R200 camera has Color distortion parameters + if (g_camera_type == "R200") { - color_caminfo_D = color_caminfo_D && g_color_caminfo_D_recv[i]; + float color_caminfo_D = 1; + // Ignoring the 5th value since it always appears to be 0.0 + for (unsigned int i = 0; i < 4; i++) + { + color_caminfo_D = color_caminfo_D && g_color_caminfo_D_recv[i]; + } + EXPECT_TRUE(color_caminfo_D != (float) 0); } - EXPECT_TRUE(color_caminfo_D != (float) 0); - } } @@ -286,13 +304,21 @@ TEST(RealsenseTests, testIsDepthStreamEnabled) { EXPECT_TRUE(g_depth_recv); EXPECT_TRUE(g_infrared1_recv); - EXPECT_TRUE(g_infrared2_recv); + // R200 camera has IR2 + if (g_camera_type == "R200") + { + EXPECT_TRUE(g_infrared2_recv); + } } else { EXPECT_FALSE(g_depth_recv); EXPECT_FALSE(g_infrared1_recv); - EXPECT_FALSE(g_infrared2_recv); + // R200 camera has IR2 + if (g_camera_type == "R200") + { + EXPECT_FALSE(g_infrared2_recv); + } } } @@ -360,6 +386,18 @@ TEST(RealsenseTests, testDepthCameraInfo) EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_DEPTH][9], (double) 0); EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_DEPTH][10] != (double) 0); EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_DEPTH][11] != (double) 0); + + // F200 camera has Depth distortion parameters + if (g_camera_type == "F200") + { + float depth_caminfo_D = 1; + for (unsigned int i = 0; i < 5; i++) + { + depth_caminfo_D = depth_caminfo_D && g_depth_caminfo_D_recv[i]; + } + EXPECT_TRUE(depth_caminfo_D != (float) 0); + } + } } @@ -426,71 +464,95 @@ TEST(RealsenseTests, testInfrared1CameraInfo) EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED][9], (double) 0); EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED][10] != (double) 0); EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED][11], (double) 0); + + // F200 camera has IR distortion parameters + if (g_camera_type == "F200") + { + float infrared1_caminfo_D = 1; + for (unsigned int i = 0; i < 5; i++) + { + infrared1_caminfo_D = infrared1_caminfo_D && g_infrared1_caminfo_D_recv[i]; + } + EXPECT_TRUE(infrared1_caminfo_D != (float) 0); + } + } } TEST(RealsenseTests, testInfrared2Stream) { - if (g_enable_depth) - { - EXPECT_TRUE(g_infrared2_avg > 0); - EXPECT_TRUE(g_infrared2_recv); - } - else + // R200 camera has IR2 + if (g_camera_type == "R200") { - EXPECT_FALSE(g_infrared2_recv); + if (g_enable_depth) + { + EXPECT_TRUE(g_infrared2_avg > 0); + EXPECT_TRUE(g_infrared2_recv); + } + else + { + EXPECT_FALSE(g_infrared2_recv); + } } } TEST(RealsenseTests, testInfrared2Resolution) { - if (g_enable_depth) + // R200 camera has IR2 + if (g_camera_type == "R200") { - if (g_depth_width_exp > 0) - { - EXPECT_EQ(g_depth_width_exp, g_width_recv[RS_STREAM_INFRARED2]); - } - if (g_depth_height_exp > 0) + if (g_enable_depth) { - EXPECT_EQ(g_depth_height_exp, g_height_recv[RS_STREAM_INFRARED2]); + if (g_depth_width_exp > 0) + { + EXPECT_EQ(g_depth_width_exp, g_width_recv[RS_STREAM_INFRARED2]); + } + if (g_depth_height_exp > 0) + { + EXPECT_EQ(g_depth_height_exp, g_height_recv[RS_STREAM_INFRARED2]); + } } } } TEST(RealsenseTests, testInfrared2CameraInfo) { - if (g_enable_depth) + // R200 camera has IR2 + if (g_camera_type == "R200") { - EXPECT_EQ(g_width_recv[RS_STREAM_INFRARED2], g_caminfo_width_recv[RS_STREAM_INFRARED2]); - EXPECT_EQ(g_height_recv[RS_STREAM_INFRARED2], g_caminfo_height_recv[RS_STREAM_INFRARED2]); - EXPECT_STREQ(g_dmodel_recv[RS_STREAM_INFRARED2].c_str (), "plumb_bob"); - - // verify rotation is equal to identity matrix - for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++) + if (g_enable_depth) { - EXPECT_EQ(ROTATION_IDENTITY[i], g_caminfo_rotation_recv[RS_STREAM_INFRARED2][i]); - } + EXPECT_EQ(g_width_recv[RS_STREAM_INFRARED2], g_caminfo_width_recv[RS_STREAM_INFRARED2]); + EXPECT_EQ(g_height_recv[RS_STREAM_INFRARED2], g_caminfo_height_recv[RS_STREAM_INFRARED2]); + EXPECT_STREQ(g_dmodel_recv[RS_STREAM_INFRARED2].c_str (), "plumb_bob"); - // check projection matrix values are set - EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][0] != (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][1], (double) 0); - EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][2] != (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][3], (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][4], (double) 0); - EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][5] != (double) 0); - EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][6] != (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][7], (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][8], (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][9], (double) 0); - EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][10] != (double) 0); - EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][11], (double) 0); + // verify rotation is equal to identity matrix + for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++) + { + EXPECT_EQ(ROTATION_IDENTITY[i], g_caminfo_rotation_recv[RS_STREAM_INFRARED2][i]); + } + + // check projection matrix values are set + EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][0] != (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][1], (double) 0); + EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][2] != (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][3], (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][4], (double) 0); + EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][5] != (double) 0); + EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][6] != (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][7], (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][8], (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][9], (double) 0); + EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_INFRARED2][10] != (double) 0); + EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_INFRARED2][11], (double) 0); + } } } TEST(RealsenseTests, testPointCloud) { - if (g_enable_depth) + if (g_enable_pointcloud) { ROS_INFO_STREAM("RealSense Camera - pc_depth_avg: " << g_pc_depth_avg); EXPECT_TRUE(g_pc_depth_avg > 0); @@ -507,14 +569,19 @@ TEST(RealsenseTests, testTransforms) // make sure all transforms are being broadcast as expected tf::TransformListener tf_listener; - EXPECT_TRUE(tf_listener.waitForTransform (DEFAULT_DEPTH_FRAME_ID, DEFAULT_BASE_FRAME_ID, ros::Time(0), ros::Duration(3.0))); - EXPECT_TRUE(tf_listener.waitForTransform (DEFAULT_DEPTH_OPTICAL_FRAME_ID, DEFAULT_DEPTH_FRAME_ID, ros::Time(0), ros::Duration(3.0))); - EXPECT_TRUE(tf_listener.waitForTransform (DEFAULT_COLOR_FRAME_ID, DEFAULT_BASE_FRAME_ID, ros::Time(0), ros::Duration(3.0))); - EXPECT_TRUE(tf_listener.waitForTransform (DEFAULT_COLOR_OPTICAL_FRAME_ID, DEFAULT_COLOR_FRAME_ID, ros::Time(0), ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform(DEFAULT_DEPTH_FRAME_ID, DEFAULT_BASE_FRAME_ID, ros::Time(0), + ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform(DEFAULT_DEPTH_OPTICAL_FRAME_ID, DEFAULT_DEPTH_FRAME_ID, ros::Time(0), + ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform(DEFAULT_COLOR_FRAME_ID, DEFAULT_BASE_FRAME_ID, ros::Time(0), + ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform(DEFAULT_COLOR_OPTICAL_FRAME_ID, DEFAULT_COLOR_FRAME_ID, ros::Time(0), + ros::Duration(3.0))); } TEST(RealsenseTests, testCameraOptions) { + g_service_client.call(g_srv); stringstream settings_ss (g_srv.response.configuration_str); string setting; string setting_name; @@ -527,9 +594,9 @@ TEST(RealsenseTests, testCameraOptions) setting_value = (setting.substr (setting.rfind (":") + 1)); if (g_config_args.find (setting_name) != g_config_args.end ()) { - int actual_value = atoi (setting_value.c_str ()); - int expected_value = atoi (g_config_args.at (setting_name).c_str ()); - EXPECT_EQ(expected_value, actual_value); + int option_recv = atoi(setting_value.c_str()); + int option_exp = atoi(g_config_args.at (setting_name).c_str()); + EXPECT_EQ(option_exp, option_recv) << setting_name; } } } @@ -558,6 +625,12 @@ void fillConfigMap(int argc, char **argv) if (argc > 1) { + if (g_config_args.find("camera_type") != g_config_args.end()) + { + ROS_INFO("RealSense Camera - Setting %s to %s", "camera_type", g_config_args.at("camera_type").c_str()); + g_camera_type = g_config_args.at("camera_type").c_str(); + } + // Set depth arguments. if (g_config_args.find("enable_depth") != g_config_args.end()) { @@ -625,6 +698,21 @@ void fillConfigMap(int argc, char **argv) ROS_INFO("RealSense Camera - Setting %s to %s", "color_step", g_config_args.at("color_step").c_str()); g_color_step_exp = atoi(g_config_args.at("color_step").c_str()); } + + // Set pointcloud arguments. + if (g_config_args.find("enable_pointcloud") != g_config_args.end()) + { + ROS_INFO("RealSense Camera - Setting %s to %s", "enable_pointcloud", + g_config_args.at("enable_pointcloud").c_str()); + if (strcmp((g_config_args.at("enable_pointcloud").c_str()),"true") == 0) + { + g_enable_pointcloud = true; + } + else + { + g_enable_pointcloud = false; + } + } } } @@ -641,7 +729,8 @@ int main(int argc, char **argv) try g_camera_subscriber[0] = it.subscribeCamera(DEPTH_TOPIC, 1, imageDepthCallback, 0); g_camera_subscriber[1] = it.subscribeCamera(COLOR_TOPIC, 1, imageColorCallback, 0); g_camera_subscriber[2] = it.subscribeCamera(IR_TOPIC, 1, imageInfrared1Callback, 0); - if (g_camera.find("R200") != std::string::npos) + // R200 camera has IR2 + if (g_camera_type == "R200") { g_camera_subscriber[3] = it.subscribeCamera(IR2_TOPIC, 1, imageInfrared2Callback, 0); } diff --git a/realsense_camera/test/camera_core.h b/realsense_camera/test/camera_core.h index 44a22f0b28..4e77c93b15 100644 --- a/realsense_camera/test/camera_core.h +++ b/realsense_camera/test/camera_core.h @@ -68,7 +68,7 @@ uint32_t g_infrared2_step_exp; // Expected infrared2 step. bool g_enable_color = true; bool g_enable_depth = true; -bool g_enable_pointcloud = true; +bool g_enable_pointcloud = false; std::string g_depth_encoding_exp; // Expected depth encoding. std::string g_color_encoding_exp; // Expected color encoding. @@ -104,11 +104,14 @@ std::string g_encoding_recv[STREAM_COUNT]; // Expected stream encoding. int g_caminfo_height_recv[STREAM_COUNT] = {0}; int g_caminfo_width_recv[STREAM_COUNT] = {0}; float g_color_caminfo_D_recv[5] = {0}; +float g_depth_caminfo_D_recv[5] = {0}; +float g_infrared1_caminfo_D_recv[5] = {0}; +float g_infrared2_caminfo_D_recv[5] = {0}; double g_caminfo_rotation_recv[STREAM_COUNT][9] = {0}; double g_caminfo_projection_recv[STREAM_COUNT][12] = {0}; std::string g_dmodel_recv[STREAM_COUNT]; -std::string g_camera = "R200"; +std::string g_camera_type; realsense_camera::cameraConfiguration g_srv; diff --git a/realsense_camera/test/f200_nodelet_camera_options.test b/realsense_camera/test/f200_nodelet_camera_options.test new file mode 100644 index 0000000000..462afbc41b --- /dev/null +++ b/realsense_camera/test/f200_nodelet_camera_options.test @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense_camera/test/f200_nodelet_disable_color.test b/realsense_camera/test/f200_nodelet_disable_color.test new file mode 100644 index 0000000000..1c8c2a6ba1 --- /dev/null +++ b/realsense_camera/test/f200_nodelet_disable_color.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/realsense_camera/test/f200_nodelet_disable_depth.test b/realsense_camera/test/f200_nodelet_disable_depth.test new file mode 100644 index 0000000000..322d901867 --- /dev/null +++ b/realsense_camera/test/f200_nodelet_disable_depth.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/realsense_camera/test/f200_nodelet_modify_params.test b/realsense_camera/test/f200_nodelet_modify_params.test new file mode 100644 index 0000000000..0e0dc23215 --- /dev/null +++ b/realsense_camera/test/f200_nodelet_modify_params.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/realsense_camera/test/f200_nodelet_resolution.test b/realsense_camera/test/f200_nodelet_resolution.test new file mode 100644 index 0000000000..03dc971fc3 --- /dev/null +++ b/realsense_camera/test/f200_nodelet_resolution.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/realsense_camera/test/f200_nodelet_rgbd.test b/realsense_camera/test/f200_nodelet_rgbd.test new file mode 100644 index 0000000000..40b8c40b99 --- /dev/null +++ b/realsense_camera/test/f200_nodelet_rgbd.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/realsense_camera/test/r200_nodelet_camera_options.test b/realsense_camera/test/r200_nodelet_camera_options.test new file mode 100644 index 0000000000..e354ff4a82 --- /dev/null +++ b/realsense_camera/test/r200_nodelet_camera_options.test @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense_camera/test/r200_nodelet_disable_color.test b/realsense_camera/test/r200_nodelet_disable_color.test index 5835f8e8ba..c5ca97c978 100644 --- a/realsense_camera/test/r200_nodelet_disable_color.test +++ b/realsense_camera/test/r200_nodelet_disable_color.test @@ -1,14 +1,10 @@ - + - - - @@ -16,5 +12,5 @@ + args="camera_type $(arg camera_type) enable_color $(arg enable_color)" /> diff --git a/realsense_camera/test/r200_nodelet_disable_depth.test b/realsense_camera/test/r200_nodelet_disable_depth.test index 5e878b899b..4f1d749cf3 100644 --- a/realsense_camera/test/r200_nodelet_disable_depth.test +++ b/realsense_camera/test/r200_nodelet_disable_depth.test @@ -1,6 +1,6 @@ - + @@ -12,5 +12,5 @@ + args="camera_type $(arg camera_type) enable_depth $(arg enable_depth)" /> diff --git a/realsense_camera/test/r200_nodelet_dynamic_params.launch b/realsense_camera/test/r200_nodelet_dynamic_params.launch deleted file mode 100644 index dc9149eeaa..0000000000 --- a/realsense_camera/test/r200_nodelet_dynamic_params.launch +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/realsense_camera/test/r200_nodelet_modify_params.test b/realsense_camera/test/r200_nodelet_modify_params.test index d404a53a2c..11121bf584 100644 --- a/realsense_camera/test/r200_nodelet_modify_params.test +++ b/realsense_camera/test/r200_nodelet_modify_params.test @@ -1,28 +1,37 @@ - - - - - - - - - + + + + + + + + + + - - - - - - - - - + + + + + + + + + + + args="camera_type $(arg camera_type) + enable_depth $(arg enable_depth) + enable_color $(arg enable_color) + enable_pointcloud $(arg enable_pointcloud) + depth_width $(arg depth_width) + depth_height $(arg depth_height) + color_width $(arg color_width) + color_height $(arg color_height)" /> diff --git a/realsense_camera/test/r200_nodelet_resolution.test b/realsense_camera/test/r200_nodelet_resolution.test index 2d3b5daae2..55d6b8afe0 100644 --- a/realsense_camera/test/r200_nodelet_resolution.test +++ b/realsense_camera/test/r200_nodelet_resolution.test @@ -1,26 +1,33 @@ - - - - - - - - + + + + + + + + + - - - - - - - - + + + + + + + + + + args="camera_type $(arg camera_type) + enable_pointcloud $(arg enable_pointcloud) + depth_width $(arg depth_width) + depth_height $(arg depth_height) + color_width $(arg color_width) + color_height $(arg color_height)" /> diff --git a/realsense_camera/test/r200_nodelet_rgbd.test b/realsense_camera/test/r200_nodelet_rgbd.test index a51a254b55..a1682ea956 100644 --- a/realsense_camera/test/r200_nodelet_rgbd.test +++ b/realsense_camera/test/r200_nodelet_rgbd.test @@ -1,6 +1,11 @@ + + + - + + + diff --git a/realsense_camera/test/r200_nodelet_static_params.launch b/realsense_camera/test/r200_nodelet_static_params.launch deleted file mode 100644 index 170d9190b4..0000000000 --- a/realsense_camera/test/r200_nodelet_static_params.launch +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -