diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md new file mode 100644 index 0000000000..78e8f72d51 --- /dev/null +++ b/.github/ISSUE_TEMPLATE.md @@ -0,0 +1,32 @@ +### System Configuration +Please complete Your Configuration detail below. + +| Version | Known Good | Your Configuration | +| ---------------- | ------------:| ------------------:| +| Operating System | ubuntu 14.04 | ??? | +| Kernel Version | 4.4.0-13 | ?.?.?-? | +| R200 FW | 1.0.72.06 | ?.?.?.? | +| librealsense | 0.9.1 | ?.?.? | + +--- +#### How to collect Configuration Data +*This section can be delete before submission.* + +| Version | Method | +| ---------------- | ------------ | +| Operating System | `cat /etc/*elease*` | +| Kernel Version | `uname -r` | +| R200 FW | View the ROS log from running node **OR** `/librealsense/bin/cpp-enumerate-devices | grep -i firmware` | +| librealsense | `cat /librealsense/readme.md | grep release-image | awk -F- '{print $3}'` | +--- + + +### Expected Behavior + + +### Actual Behavior + + +### Steps to Reproduce + + diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000000..e0ebfd2a0d --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,8 @@ +Fixes Issue: # + +Changes proposed in this pull request: +- +- +- + + diff --git a/camera/CHANGELOG.rst b/camera/CHANGELOG.rst index 7fd84f81ec..f54638fe9d 100644 --- a/camera/CHANGELOG.rst +++ b/camera/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package realsense_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.2 (2016-03-28) +------------------ +* Added functionality to access camera using Serial No: + This fixes Issue #18. + Added code to handle various use cases for input serial number such as + --Exit with error if no serial number is specified and multiple cameras are detected. + --Exit with error if no camera is detected that matches the input serial number. + --Prints all the detected cameras. + Updated launch files with a placeholder for serial number. + Updated rgbd_launch test code to remove hardcoded topic names. +* Modified all parameters to lowercase for consistency + This fixes Issue #13. +* Removed support for R200_DISPARITY_MULTIPLIER camera option +* Added missing install targets + This fixes Pull Request #2. + This fixes Issue #17. +* Contributors: Reagan Lopez, Rajvi Jingar + 1.0.1 (2016-03-17) ------------------ * Convert command line args to ROS params diff --git a/camera/CMakeLists.txt b/camera/CMakeLists.txt index 29dc548ee9..2625b7cb29 100644 --- a/camera/CMakeLists.txt +++ b/camera/CMakeLists.txt @@ -43,29 +43,47 @@ generate_dynamic_reconfigure_options( cfg/camera_params.cfg ) - - include_directories( ${catkin_INCLUDE_DIRS} ) add_library(realsense_camera_nodelet src/realsense_camera_nodelet.cpp) -target_link_libraries(realsense_camera_nodelet +target_link_libraries(realsense_camera_nodelet ${catkin_LIBRARIES} /usr/local/lib/librealsense.so ) add_dependencies(realsense_camera_nodelet realsense_camera_generate_messages_cpp ${PROJECT_NAME}_gencfg) add_executable(realsense_camera_test test/realsense_camera_test_node.cpp) -target_link_libraries(realsense_camera_test +target_link_libraries(realsense_camera_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ) add_dependencies(realsense_camera_test realsense_camera_generate_messages_cpp ${PROJECT_NAME}_gencfg) add_executable(realsense_camera_test_rgbd test/realsense_camera_test_rgbd_node.cpp) -target_link_libraries(realsense_camera_test_rgbd +target_link_libraries(realsense_camera_test_rgbd ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ) add_dependencies(realsense_camera_test_rgbd realsense_camera_generate_messages_cpp) + +# Install nodelet library +install(TARGETS realsense_camera_nodelet LIBRARY + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# Install launch files +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +# Install rviz files +install(DIRECTORY rviz/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz +) + +# Install xml files +install(FILES package.xml realsense_camera_nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/camera/README.md b/camera/README.md index 6c787802a2..d512a13bf5 100644 --- a/camera/README.md +++ b/camera/README.md @@ -73,6 +73,9 @@ Infrared2 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. + This feature has been tested to work only on kernel version 4.4.0-040400-generic. 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. @@ -98,19 +101,19 @@ Infrared2 camera 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] - R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT : [0 - 255] - R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT : [0 - 255] - R200_DEPTH_CONTROL_MEDIAN_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD : [0 - 31] - R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD : [0 - 1023] - R200_DEPTH_CONTROL_LR_THRESHOLD : [0 - 2047] + r200_depth_units : [1, 2147483647] + r200_depth_clamp_min : [0, 65535] + r200_depth_clamp_max : [0, 65535] + r200_depth_control_estimate_median_decrement : [0 - 255] + r200_depth_control_estimate_median_increment : [0 - 255] + r200_depth_control_median_threshold : [0 - 1023] + r200_depth_control_score_minimum_threshold : [0 - 1023] + r200_depth_control_score_maximum_threshold : [0 - 1023] + r200_depth_control_texture_count_threshold : [0 - 31] + r200_depth_control_texture_difference_threshold : [0 - 1023] + r200_depth_control_second_peak_threshold : [0 - 1023] + r200_depth_control_neighbor_threshold : [0 - 1023] + r200_depth_control_lr_threshold : [0 - 2047] ####Services get_settings (camera/get_settings) @@ -126,28 +129,27 @@ To get supported camera options with current value set. It returns string in opt 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_DISPARITY_MULTIPLIER (This parameter does not work in the latest R200 firmware release. Likely to be removed in the next ros-realsense release.) - 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) - -Note: For Autoexposure EDGE parameters, max value will go only upto the bounds of the infrared image. + 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) + +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) Use rqt_reconfigure GUI to view and edit the parameters that are accessible via dynamic_reconfigure. @@ -158,7 +160,7 @@ Command to launch GUI: Command to change dynamic parameters using commandline: $ rosrun dynamic_reconfigure dynparam set / - E.g. $ rosrun dynamic_reconfigure dynparam set /RealsenseNodelet COLOR_BACKLIGHT_COMPENSATION 2 + E.g. $ rosrun dynamic_reconfigure dynparam set /RealsenseNodelet color_backlight_compensation 2 ###Running the R200 nodelet @@ -245,17 +247,22 @@ Refer to the function definitions in [realsense_camera_nodelet.h](src/realsense_ ###Limitations: -Currently, the ROS camera nodelet only supports the following formats: -* Color stream: RGB8 -* Depth stream: Y16 -* Infrared stream: Y8 - -Note: The camera does not provide hardware based depth registration/projector data. Hence the launch file "realsense_r200_rgbd.launch" will not generate data for the following topics: -/camera/depth_registered/hw_registered/image_rect_raw -/camera/depth_registered/points -/camera/depth_registered/hw_registered/image_rect -/camera/depth_registered/image -/camera/depth/disparity -/camera/depth_registered/disparity - +* 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: Y16 + * Infrared stream: Y8 + +* The camera does not provide hardware based depth registration/projector data. +Hence the launch file "realsense_r200_rgbd.launch" will not generate data for the following topics: + * /camera/depth_registered/hw_registered/image_rect_raw + * /camera/depth_registered/points + * /camera/depth_registered/hw_registered/image_rect + * /camera/depth_registered/image + * /camera/depth/disparity + * /camera/depth_registered/disparity + +* If there are multiple R200 cameras connected to a system, the nodelet can be launched for a particular camera +by specifing the serial_no parameter in the launch file. But it has not been tested to launch nodelets simultaneouly for multiple cameras. diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index 17cca2072d..8619283ba5 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -8,24 +8,23 @@ 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", 56, 0, 255) -gen.add("COLOR_CONTRAST", int_t, 0, "Contrast", 32, 16, 64) -gen.add("COLOR_GAIN", int_t, 0, "Gain", 32, 0, 256) -gen.add("COLOR_GAMMA", int_t, 0, "Gamma", 220, 100, 280) -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) -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) -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) -gen.add("R200_DISPARITY_MULTIPLIER", int_t, 0, "Disparity Multiplier", 32, 1, 1000) -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) -gen.add("R200_AUTO_EXPOSURE_RIGHT_EDGE", int_t, 0, "Auto Exposure Right Edge", 639, 0, 639) +gen.add("color_backlight_compensation", int_t, 0, "Backlight Compensation", 1, 0, 4) +gen.add("color_brightness", int_t, 0, "Brightness", 56, 0, 255) +gen.add("color_contrast", int_t, 0, "Contrast", 32, 16, 64) +gen.add("color_gain", int_t, 0, "Gain", 32, 0, 256) +gen.add("color_gamma", int_t, 0, "Gamma", 220, 100, 280) +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) +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) +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) +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) +gen.add("r200_auto_exposure_right_edge", int_t, 0, "Auto Exposure Right Edge", 639, 0, 639) exit(gen.generate(PACKAGE, "realsense_camera", "camera_params")) diff --git a/camera/launch/includes/realsense_r200_nodelet.launch.xml b/camera/launch/includes/realsense_r200_nodelet.launch.xml index 361fe6ed63..97f64f3912 100644 --- a/camera/launch/includes/realsense_r200_nodelet.launch.xml +++ b/camera/launch/includes/realsense_r200_nodelet.launch.xml @@ -1,7 +1,6 @@ - @@ -9,6 +8,7 @@ + @@ -25,6 +25,7 @@ + diff --git a/camera/launch/realsense_r200_navigation.launch b/camera/launch/realsense_r200_navigation.launch index 1a4105604d..9ca8d38cb3 100644 --- a/camera/launch/realsense_r200_navigation.launch +++ b/camera/launch/realsense_r200_navigation.launch @@ -1,5 +1,5 @@ - + @@ -10,6 +10,7 @@ + diff --git a/camera/launch/realsense_r200_nodelet_standalone_manual.launch b/camera/launch/realsense_r200_nodelet_standalone_manual.launch index c4159e27dd..e66365bba4 100644 --- a/camera/launch/realsense_r200_nodelet_standalone_manual.launch +++ b/camera/launch/realsense_r200_nodelet_standalone_manual.launch @@ -1,5 +1,6 @@ + @@ -16,6 +17,7 @@ + diff --git a/camera/launch/realsense_r200_nodelet_standalone_preset.launch b/camera/launch/realsense_r200_nodelet_standalone_preset.launch index dd3d1a09c0..186d2e12bf 100644 --- a/camera/launch/realsense_r200_nodelet_standalone_preset.launch +++ b/camera/launch/realsense_r200_nodelet_standalone_preset.launch @@ -1,5 +1,5 @@ - + @@ -10,6 +10,7 @@ + diff --git a/camera/launch/realsense_r200_rgbd.launch b/camera/launch/realsense_r200_rgbd.launch index 4a9e760ac7..e1a51a40a6 100644 --- a/camera/launch/realsense_r200_rgbd.launch +++ b/camera/launch/realsense_r200_rgbd.launch @@ -1,9 +1,13 @@ - + + + + + @@ -51,6 +55,7 @@ + diff --git a/camera/package.xml b/camera/package.xml index 688fa556a6..3d8481fa4f 100644 --- a/camera/package.xml +++ b/camera/package.xml @@ -1,7 +1,7 @@ realsense_camera - 1.0.1 + 1.0.2 RealSense Camera package allowing access to Intel 3D cameras and advanced modules Rajvi Jingar @@ -11,7 +11,7 @@ http://www.ros.org/wiki/RealSense - Rajvi Jingar + Rajvi Jingar Reagan Lopez Matt Hansen @@ -28,7 +28,7 @@ rostest pcl_ros dynamic_reconfigure - + roscpp nodelet cv_bridge @@ -44,7 +44,6 @@ rgbd_launch - + - diff --git a/camera/src/realsense_camera_nodelet_plugins.xml b/camera/realsense_camera_nodelet_plugins.xml similarity index 100% rename from camera/src/realsense_camera_nodelet_plugins.xml rename to camera/realsense_camera_nodelet_plugins.xml diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index bc16d9e8d6..56f7677cfe 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -44,9 +44,9 @@ namespace realsense_camera /* * Public Methods. */ - RealsenseNodelet::~RealsenseNodelet () + RealsenseNodelet::~RealsenseNodelet() { - device_thread_->join (); + device_thread_->join(); if (enable_tf_ == true) { @@ -56,19 +56,19 @@ namespace realsense_camera // Stop device. if (is_device_started_ == true) { - rs_stop_device (rs_device_, 0); - rs_delete_context (rs_context_, &rs_error_); - checkError (); + rs_stop_device(rs_device_, 0); + rs_delete_context(rs_context_, &rs_error_); + checkError(); } ROS_INFO_STREAM ("RealSense Camera - Stopping camera nodelet."); - ros::shutdown (); + ros::shutdown(); } /* * Initialize the realsense code. */ - void RealsenseNodelet::onInit () + void RealsenseNodelet::onInit() { ROS_INFO_STREAM ("RealSense Camera - Starting camera nodelet."); @@ -79,9 +79,11 @@ namespace realsense_camera frame_id_[RS_STREAM_INFRARED2] = IR2_DEF_FRAME; for (int i = 0; i < STREAM_COUNT; ++i) + { camera_info_[i] = NULL; + } - ros::NodeHandle & nh = getNodeHandle (); + ros::NodeHandle & nh = getNodeHandle(); pnh_ = getPrivateNodeHandle(); // Set up the topics. @@ -90,41 +92,41 @@ namespace realsense_camera setStreamOptions(); // Advertise the various topics and services. - 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 (IR1_TOPIC, 1); + 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(IR1_TOPIC, 1); if (camera_.find (R200) != std::string::npos) { - camera_publisher_[RS_STREAM_INFRARED2] = it.advertiseCamera (IR2_TOPIC, 1); + camera_publisher_[RS_STREAM_INFRARED2] = it.advertiseCamera(IR2_TOPIC, 1); } - pointcloud_publisher_ = nh.advertise < sensor_msgs::PointCloud2 > (PC_TOPIC, 1); + pointcloud_publisher_ = nh.advertise(PC_TOPIC, 1); - get_options_service_ = nh.advertiseService (SETTINGS_SERVICE, &RealsenseNodelet::getCameraOptionValues, this); + get_options_service_ = nh.advertiseService(SETTINGS_SERVICE, &RealsenseNodelet::getCameraOptionValues, this); dynamic_reconf_server_.reset(new dynamic_reconfigure::Server(getPrivateNodeHandle())); bool connected = false; - connected = connectToCamera (); + connected = connectToCamera(); if (connected == true) { // Start working thread. device_thread_ = - boost::shared_ptr < boost::thread > (new boost::thread (boost::bind (&RealsenseNodelet::devicePoll, this))); + boost::shared_ptr (new boost::thread (boost::bind(&RealsenseNodelet::devicePoll, this))); if (enable_tf_ == true) { transform_thread_ = - boost::shared_ptr < boost::thread > (new boost::thread (boost::bind (&RealsenseNodelet::publishTransforms, this))); + boost::shared_ptr(new boost::thread (boost::bind(&RealsenseNodelet::publishTransforms, this))); } } else { - ros::shutdown (); + ros::shutdown(); } dynamic_reconf_server_->setCallback(boost::bind(&RealsenseNodelet::configCallback, this, _1, _2)); @@ -139,17 +141,19 @@ namespace realsense_camera if (mode_.compare ("manual") == 0) { ROS_INFO_STREAM ("RealSense Camera - Enabling Color Stream: manual mode"); - rs_enable_stream (rs_device_, RS_STREAM_COLOR, color_width_, color_height_, COLOR_FORMAT, color_fps_, &rs_error_); - checkError (); + rs_enable_stream(rs_device_, RS_STREAM_COLOR, color_width_, color_height_, COLOR_FORMAT, color_fps_, &rs_error_); + checkError(); } else { ROS_INFO_STREAM ("RealSense Camera - Enabling Color Stream: preset mode"); - rs_enable_stream_preset (rs_device_, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &rs_error_); checkError (); + rs_enable_stream_preset(rs_device_, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &rs_error_); + checkError(); } uint32_t stream_index = (uint32_t) RS_STREAM_COLOR; - if(camera_info_[stream_index] == NULL) { + if (camera_info_[stream_index] == NULL) + { prepareStreamCalibData (RS_STREAM_COLOR); } } @@ -160,17 +164,18 @@ namespace realsense_camera if (mode_.compare ("manual") == 0) { ROS_INFO_STREAM ("RealSense Camera - Enabling Depth Stream: manual mode"); - rs_enable_stream (rs_device_, RS_STREAM_DEPTH, depth_width_, depth_height_, DEPTH_FORMAT, depth_fps_, &rs_error_); - checkError (); + rs_enable_stream(rs_device_, RS_STREAM_DEPTH, depth_width_, depth_height_, DEPTH_FORMAT, depth_fps_, &rs_error_); + checkError(); } else { ROS_INFO_STREAM ("RealSense Camera - Enabling Depth Stream: preset mode"); - rs_enable_stream_preset (rs_device_, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &rs_error_); checkError (); + rs_enable_stream_preset(rs_device_, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &rs_error_); + checkError(); } uint32_t stream_index = (uint32_t) RS_STREAM_DEPTH; - if(camera_info_[stream_index] == NULL) + if (camera_info_[stream_index] == NULL) { prepareStreamCalibData (RS_STREAM_DEPTH); } @@ -182,17 +187,18 @@ namespace realsense_camera if (mode_.compare ("manual") == 0) { ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared Stream: manual mode"); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED, depth_width_, depth_height_, IR1_FORMAT, depth_fps_, &rs_error_); - checkError (); + rs_enable_stream(rs_device_, RS_STREAM_INFRARED, depth_width_, depth_height_, IR1_FORMAT, depth_fps_, &rs_error_); + checkError(); } else { ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared Stream: preset mode"); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &rs_error_); checkError (); + rs_enable_stream_preset(rs_device_, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &rs_error_); + checkError(); } uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED; - if(camera_info_[stream_index] == NULL) + if (camera_info_[stream_index] == NULL) { prepareStreamCalibData (RS_STREAM_INFRARED); } @@ -204,16 +210,16 @@ namespace realsense_camera if (mode_.compare ("manual") == 0) { ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared2 Stream: manual mode"); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED2, depth_width_, depth_height_, IR2_FORMAT, depth_fps_, 0); + rs_enable_stream(rs_device_, RS_STREAM_INFRARED2, depth_width_, depth_height_, IR2_FORMAT, depth_fps_, 0); } else { ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared2 Stream: preset mode"); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED2, RS_PRESET_BEST_QUALITY, 0); + rs_enable_stream_preset(rs_device_, RS_STREAM_INFRARED2, RS_PRESET_BEST_QUALITY, 0); } uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED2; - if(camera_info_[stream_index] == NULL) + if (camera_info_[stream_index] == NULL) { prepareStreamCalibData (RS_STREAM_INFRARED2); } @@ -223,56 +229,61 @@ namespace realsense_camera void RealsenseNodelet::configCallback(realsense_camera::camera_paramsConfig &config, uint32_t level) { - 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); + 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); + if (config.color_enable_auto_white_balance == 0) + { + rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0); } //R200 camera specific options - rs_set_device_option(rs_device_, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, config.R200_LR_AUTO_EXPOSURE_ENABLED, 0); + 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); + 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); - rs_set_device_option(rs_device_, RS_OPTION_R200_DISPARITY_MULTIPLIER, config.R200_DISPARITY_MULTIPLIER, 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_lr_auto_exposure_enabled == 1) { - if(config.R200_AUTO_EXPOSURE_TOP_EDGE >= depth_height_) { - config.R200_AUTO_EXPOSURE_TOP_EDGE = depth_height_ - 1; + if (config.r200_auto_exposure_top_edge >= depth_height_) + { + config.r200_auto_exposure_top_edge = depth_height_ - 1; } - if(config.R200_AUTO_EXPOSURE_BOTTOM_EDGE >= depth_height_) { - config.R200_AUTO_EXPOSURE_BOTTOM_EDGE = depth_height_ - 1; + if (config.r200_auto_exposure_bottom_edge >= depth_height_) + { + config.r200_auto_exposure_bottom_edge = depth_height_ - 1; } - if(config.R200_AUTO_EXPOSURE_LEFT_EDGE >= depth_width_) { - config.R200_AUTO_EXPOSURE_LEFT_EDGE = depth_width_ - 1; + if (config.r200_auto_exposure_left_edge >= depth_width_) + { + config.r200_auto_exposure_left_edge = depth_width_ - 1; } - if(config.R200_AUTO_EXPOSURE_RIGHT_EDGE >= depth_width_) { - config.R200_AUTO_EXPOSURE_RIGHT_EDGE = depth_width_ - 1; + if (config.r200_auto_exposure_right_edge >= depth_width_) + { + config.r200_auto_exposure_right_edge = depth_width_ - 1; } - 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; + 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); } - if(config.enable_depth == false) + if (config.enable_depth == false) { - if(enable_color_ == false) + if (enable_color_ == false) { ROS_INFO_STREAM ("RealSense Camera - Color stream is also disabled. Cannot disable depth stream"); config.enable_depth = true; @@ -288,15 +299,15 @@ namespace realsense_camera } } - void RealsenseNodelet::checkError () + void RealsenseNodelet::checkError() { if (rs_error_) { - ROS_ERROR_STREAM ("RealSense Camera - 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_ERROR_STREAM ("RealSense Camera - 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 (); + ros::shutdown(); exit (EXIT_FAILURE); } @@ -305,43 +316,72 @@ namespace realsense_camera /* * Query the data for every frame and publish it to the different topics. */ - void RealsenseNodelet::devicePoll () + void RealsenseNodelet::devicePoll() { - while (ros::ok ()) + while (ros::ok()) { - publishStreams (); + publishStreams(); } } - bool RealsenseNodelet::connectToCamera () + bool RealsenseNodelet::connectToCamera() { if (enable_depth_ == false && enable_color_ == false) { - ROS_INFO_STREAM ("RealSense Camera - None of the streams are enabled. Exiting."); + ROS_ERROR_STREAM("RealSense Camera - None of the streams are enabled. Exiting."); return false; } - rs_context_ = rs_create_context (RS_API_VERSION, &rs_error_); - checkError (); + rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_); + checkError(); + + num_of_cameras_ = rs_get_device_count(rs_context_, &rs_error_); + checkError(); - int num_of_cameras = rs_get_device_count (rs_context_, NULL); - if (num_of_cameras < 1) + // Exit with error if no cameras are connected. + if (num_of_cameras_ < 1) { - ROS_ERROR_STREAM ("RealSense Camera - No cameras are connected."); + ROS_ERROR_STREAM("RealSense Camera - No cameras detected. Exiting!"); return false; } - rs_device_ = rs_get_device (rs_context_, 0, &rs_error_); - checkError (); + rs_device_ = getCameraBySerialNumber(); // Get rs_device_ using input serial number. + std::string detected_camera_msg = "RealSense Camera - Detected following cameras:"; + for (rs_device *rs_detected_device: rs_detected_devices_) + { + 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_); + checkError(); + } + ROS_INFO_STREAM(detected_camera_msg); - ROS_INFO_STREAM ("RealSense Camera - Number of cameras connected: " << num_of_cameras); - ROS_INFO_STREAM ("RealSense Camera - Firmware version: " << - rs_get_device_firmware_version (rs_device_, &rs_error_)); - checkError (); - ROS_INFO_STREAM ("RealSense Camera - Name: " << rs_get_device_name (rs_device_, &rs_error_)); - checkError (); - ROS_INFO_STREAM ("RealSense Camera - Serial no: " << rs_get_device_serial (rs_device_, &rs_error_)); - checkError (); + // Exit with error if no serial number is specified and multiple cameras are detected. + if ((serial_no_.empty() == true) && (num_of_cameras_ > 1)) + { + ROS_ERROR_STREAM("RealSense Camera - Multiple cameras detected but no input serial_no specified. Exiting!"); + return false; + } + + // Exit with error if no camera is detected that matches the input serial number. + if ((serial_no_.empty() != true) && (rs_device_ == NULL)) + { + ROS_ERROR_STREAM("RealSense Camera - No camera detected with input serial_no = " << serial_no_ << ". Exiting!"); + return false; + } + + // At this point, rs_device_ will be null if no input serial number was specified and only one camera is connected. + // This is a valid use case and the code will proceed. + if (rs_device_ == NULL) + { + rs_device_ = rs_get_device(rs_context_, 0, &rs_error_); + checkError(); + } + + ROS_INFO_STREAM("RealSense Camera - Connecting to camera with Serial No: " << + rs_get_device_serial(rs_device_, &rs_error_)); + checkError(); // Enable streams. if (enable_color_ == true) @@ -360,16 +400,40 @@ namespace realsense_camera setStaticCameraOptions(); // Start device. - rs_start_device (rs_device_, &rs_error_); - checkError (); + rs_start_device(rs_device_, &rs_error_); + checkError(); is_device_started_ = true; - allocateResources (); + allocateResources(); return true; } + /* + * Returns the device details of the camera that matches the input serial_no. + * Returns NULL if no camera is found that matches the input serial_no. + * Also populates the list of detected cameras. + */ + rs_device * RealsenseNodelet::getCameraBySerialNumber() + { + rs_device *rs_device_detect; + rs_device *rs_device_connect = NULL; + for (int i = 0; i < num_of_cameras_; ++i) + { + rs_device_detect = rs_get_device(rs_context_, i, &rs_error_); + checkError(); + std::string device_serial_no = rs_get_device_serial(rs_device_detect, &rs_error_); + checkError(); + if (device_serial_no.compare(serial_no_) == 0) + { + rs_device_connect = rs_device_detect; + } + rs_detected_devices_.push_back(rs_device_detect); + } + return rs_device_connect; + } + /* * Gets the options supported by the camera along with their min, max and step values. */ @@ -395,31 +459,31 @@ namespace realsense_camera /* * Define buffer for images and prepare camera info for each enabled stream. */ - void RealsenseNodelet::allocateResources () + void RealsenseNodelet::allocateResources() { // Prepare camera for enabled streams (color/depth/infrared/infrared2). - fillStreamEncoding (); + fillStreamEncoding(); - image_[(uint32_t) RS_STREAM_COLOR] = cv::Mat (color_height_, color_width_, CV_8UC3, cv::Scalar (0, 0, 0)); - image_[(uint32_t) RS_STREAM_DEPTH] = cv::Mat (depth_height_, depth_width_, CV_16UC1, cv::Scalar (0)); - image_[(uint32_t) RS_STREAM_INFRARED] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); - if (camera_.find (R200) != std::string::npos) + image_[(uint32_t) RS_STREAM_COLOR] = cv::Mat(color_height_, color_width_, CV_8UC3, cv::Scalar (0, 0, 0)); + image_[(uint32_t) RS_STREAM_DEPTH] = cv::Mat(depth_height_, depth_width_, CV_16UC1, cv::Scalar (0)); + image_[(uint32_t) RS_STREAM_INFRARED] = cv::Mat(depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); + if (camera_.find(R200) != std::string::npos) { - image_[(uint32_t) RS_STREAM_INFRARED2] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); + image_[(uint32_t) RS_STREAM_INFRARED2] = cv::Mat(depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); } } /* * Prepare camera_info for each enabled stream. */ - void RealsenseNodelet::prepareStreamCalibData (rs_stream rs_strm) + void RealsenseNodelet::prepareStreamCalibData(rs_stream rs_strm) { uint32_t stream_index = (uint32_t) rs_strm; rs_intrinsics intrinsic; - rs_get_stream_intrinsics (rs_device_, rs_strm, &intrinsic, &rs_error_); - checkError (); + rs_get_stream_intrinsics(rs_device_, rs_strm, &intrinsic, &rs_error_); + checkError(); - camera_info_[stream_index] = new sensor_msgs::CameraInfo (); + camera_info_[stream_index] = new sensor_msgs::CameraInfo(); camera_info_ptr_[stream_index] = sensor_msgs::CameraInfoPtr (camera_info_[stream_index]); camera_info_[stream_index]->header.frame_id = frame_id_[stream_index]; @@ -451,7 +515,8 @@ namespace realsense_camera { // set depth to color translation values in Projection matrix (P) rs_extrinsics z_extrinsic; - rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); checkError (); + rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + checkError(); camera_info_[stream_index]->P.at(3) = z_extrinsic.translation[0]/1000; // Tx camera_info_[stream_index]->P.at(7) = z_extrinsic.translation[1]/1000; // Ty camera_info_[stream_index]->P.at(11) = z_extrinsic.translation[2]/1000; // Tz @@ -471,7 +536,9 @@ namespace realsense_camera camera_info_[stream_index]->R.at(8) = (double) 1; for (int i = 0; i < 5; i++) - camera_info_[stream_index]->D.push_back (0); + { + camera_info_[stream_index]->D.push_back(0); + } } /* @@ -486,8 +553,9 @@ namespace realsense_camera for (option_str o: 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 < std::string > (o.value); + opt_value = boost::lexical_cast(o.value); get_options_result_str += opt_name + ":" + opt_value + ";"; } @@ -500,6 +568,7 @@ namespace realsense_camera */ void RealsenseNodelet::setStreamOptions() { + pnh_.getParam("serial_no", serial_no_); pnh_.param("camera", camera_, (std::string) R200); pnh_.param("mode", mode_, DEFAULT_MODE); pnh_.param("enable_depth", enable_depth_, ENABLE_DEPTH); @@ -535,6 +604,7 @@ namespace realsense_camera std::vector::iterator it; for (camera_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; @@ -566,7 +636,7 @@ namespace realsense_camera opt_val = val; } ROS_INFO_STREAM ("RealSense Camera - Static Options: " << opt_name << " = " << opt_val); - rs_set_device_option (rs_device_, o.opt, opt_val, &rs_error_); + rs_set_device_option(rs_device_, o.opt, opt_val, &rs_error_); checkError(); } } @@ -576,24 +646,24 @@ namespace realsense_camera /* * Copy frame data from realsense to member cv images. */ - void RealsenseNodelet::prepareStreamData (rs_stream rs_strm) + void RealsenseNodelet::prepareStreamData(rs_stream rs_strm) { switch (rs_strm) { case RS_STREAM_COLOR: - image_[(uint32_t) RS_STREAM_COLOR].data = (unsigned char *) (rs_get_frame_data (rs_device_, RS_STREAM_COLOR, 0)); + image_[(uint32_t) RS_STREAM_COLOR].data = (unsigned char *) (rs_get_frame_data(rs_device_, RS_STREAM_COLOR, 0)); break; case RS_STREAM_DEPTH: - image_depth16_ = reinterpret_cast (rs_get_frame_data (rs_device_, RS_STREAM_DEPTH, 0)); + image_depth16_ = reinterpret_cast (rs_get_frame_data(rs_device_, RS_STREAM_DEPTH, 0)); image_[(uint32_t) RS_STREAM_DEPTH].data = (unsigned char *) image_depth16_; break; case RS_STREAM_INFRARED: image_[(uint32_t) RS_STREAM_INFRARED].data = - (unsigned char *) (rs_get_frame_data (rs_device_, RS_STREAM_INFRARED, 0)); + (unsigned char *) (rs_get_frame_data(rs_device_, RS_STREAM_INFRARED, 0)); break; case RS_STREAM_INFRARED2: image_[(uint32_t) RS_STREAM_INFRARED2].data = - (unsigned char *) (rs_get_frame_data (rs_device_, RS_STREAM_INFRARED2, 0)); + (unsigned char *) (rs_get_frame_data(rs_device_, RS_STREAM_INFRARED2, 0)); break; default: // no other streams supported @@ -604,7 +674,7 @@ namespace realsense_camera /* * Populate the encodings for each stream. */ - void RealsenseNodelet::fillStreamEncoding () + void RealsenseNodelet::fillStreamEncoding() { stream_encoding_[(uint32_t) RS_STREAM_COLOR] = "rgb8"; stream_step_[(uint32_t) RS_STREAM_COLOR] = color_width_ * sizeof (unsigned char) * 3; @@ -619,39 +689,45 @@ namespace realsense_camera /* * Publish streams. */ - void RealsenseNodelet::publishStreams () + void RealsenseNodelet::publishStreams() { - if(enable_depth_ == false && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1) + if (enable_depth_ == false && rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 1) { - if(rs_is_device_streaming(rs_device_, 0) == 1) + if (rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); checkError (); + rs_stop_device(rs_device_, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } //disable depth, infrared1 and infrared2 streams - rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); checkError (); + rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Depth Stream Disabled"); - rs_disable_stream(rs_device_, RS_STREAM_INFRARED, &rs_error_); checkError (); + rs_disable_stream(rs_device_, RS_STREAM_INFRARED, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Infrared1 stream Disabled"); - rs_disable_stream(rs_device_, RS_STREAM_INFRARED2, &rs_error_); checkError (); + rs_disable_stream(rs_device_, RS_STREAM_INFRARED2, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Infrared2 stream Disabled"); - if(rs_is_device_streaming(rs_device_, 0) == 0) + if (rs_is_device_streaming(rs_device_, 0) == 0) { - rs_start_device (rs_device_, &rs_error_);checkError (); + rs_start_device(rs_device_, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } - if(enable_depth_ == true && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 0) + if (enable_depth_ == true && rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 0) { - if(rs_is_device_streaming(rs_device_, 0) == 1) + if (rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); checkError (); + rs_stop_device(rs_device_, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } @@ -659,30 +735,31 @@ namespace realsense_camera enableInfraredStream(); enableInfrared2Stream(); - if(rs_is_device_streaming(rs_device_, 0) == 0) + if (rs_is_device_streaming(rs_device_, 0) == 0) { - rs_start_device (rs_device_, &rs_error_); checkError (); + rs_start_device(rs_device_, &rs_error_); + checkError(); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } - if(rs_is_device_streaming(rs_device_, 0) == 1) + if (rs_is_device_streaming(rs_device_, 0) == 1) { - rs_wait_for_frames (rs_device_, &rs_error_); - checkError (); - time_stamp_ = ros::Time::now (); + rs_wait_for_frames(rs_device_, &rs_error_); + checkError(); + time_stamp_ = ros::Time::now(); for (int stream_index = 0; stream_index < STREAM_COUNT; ++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) + if (camera_publisher_[stream_index].getNumSubscribers() > 0 && + rs_is_stream_enabled(rs_device_, (rs_stream) stream_index, 0) == 1) { prepareStreamData ((rs_stream) stream_index); - sensor_msgs::ImagePtr msg = cv_bridge::CvImage (std_msgs::Header (), + sensor_msgs::ImagePtr msg = cv_bridge::CvImage (std_msgs::Header(), stream_encoding_[stream_index], - image_[stream_index]).toImageMsg (); + image_[stream_index]).toImageMsg(); msg->header.frame_id = frame_id_[stream_index]; msg->header.stamp = time_stamp_; // Publish timestamp to synchronize frames. @@ -696,14 +773,14 @@ namespace realsense_camera } } - if (pointcloud_publisher_.getNumSubscribers () > 0 && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1 - && enable_pointcloud_ == true) + if (pointcloud_publisher_.getNumSubscribers() > 0 && + rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 1 && enable_pointcloud_ == true) { - if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers () <= 0) + if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers() <= 0) { prepareStreamData (RS_STREAM_DEPTH); } - if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers () <= 0) + if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers() <= 0) { prepareStreamData (RS_STREAM_COLOR); } @@ -718,19 +795,21 @@ namespace realsense_camera void RealsenseNodelet::publishPointCloud (cv::Mat & image_color) { // Publish pointcloud only if there is at least one subscriber. - if (pointcloud_publisher_.getNumSubscribers () > 0) + if (pointcloud_publisher_.getNumSubscribers() > 0) { rs_intrinsics color_intrinsic; rs_extrinsics z_extrinsic; rs_intrinsics z_intrinsic; - rs_get_stream_intrinsics (rs_device_, RS_STREAM_DEPTH, &z_intrinsic, &rs_error_); - checkError (); + rs_get_stream_intrinsics(rs_device_, RS_STREAM_DEPTH, &z_intrinsic, &rs_error_); + checkError(); if (enable_color_ == true) { - rs_get_stream_intrinsics (rs_device_, RS_STREAM_COLOR, &color_intrinsic, &rs_error_); checkError (); - rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); checkError (); + rs_get_stream_intrinsics(rs_device_, RS_STREAM_COLOR, &color_intrinsic, &rs_error_); + checkError(); + rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + checkError(); } // Convert pointcloud from the camera to pointcloud object for ROS. @@ -740,28 +819,28 @@ namespace realsense_camera msg_pointcloud.header.stamp = time_stamp_; msg_pointcloud.is_dense = true; - sensor_msgs::PointCloud2Modifier modifier (msg_pointcloud); + sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); - modifier.setPointCloud2Fields (4, "x", 1, + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - modifier.setPointCloud2FieldsByString (2, "xyz", "rgb"); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - sensor_msgs::PointCloud2Iterator < float >iter_x (msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iterator < float >iter_y (msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iterator < float >iter_z (msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); - sensor_msgs::PointCloud2Iterator < uint8_t > iter_r (msg_pointcloud, "r"); - sensor_msgs::PointCloud2Iterator < uint8_t > iter_g (msg_pointcloud, "g"); - sensor_msgs::PointCloud2Iterator < uint8_t > iter_b (msg_pointcloud, "b"); + sensor_msgs::PointCloud2Iteratoriter_r(msg_pointcloud, "r"); + sensor_msgs::PointCloud2Iteratoriter_g(msg_pointcloud, "g"); + sensor_msgs::PointCloud2Iteratoriter_b(msg_pointcloud, "b"); float depth_point[3], color_point[3], color_pixel[2], scaled_depth; unsigned char *color_data = image_color.data; - const float depth_scale = rs_get_device_depth_scale (rs_device_, &rs_error_); - checkError ();// Default value is 0.001 + const float depth_scale = rs_get_device_depth_scale(rs_device_, &rs_error_); + checkError();// Default value is 0.001 // Fill the PointCloud2 fields. for (int y = 0; y < z_intrinsic.height; y++) @@ -771,7 +850,7 @@ namespace realsense_camera scaled_depth = ((float) *image_depth16_) * depth_scale; float depth_pixel[2] = { (float) x, (float) y}; - rs_deproject_pixel_to_point (depth_point, &z_intrinsic, depth_pixel, scaled_depth); + rs_deproject_pixel_to_point(depth_point, &z_intrinsic, depth_pixel, scaled_depth); if (depth_point[2] <= 0 || depth_point[2] > MAX_Z) { @@ -791,8 +870,8 @@ namespace realsense_camera if (enable_color_ == true) { - rs_transform_point_to_point (color_point, &z_extrinsic, depth_point); - rs_project_point_to_pixel (color_pixel, &color_intrinsic, color_point); + rs_transform_point_to_point(color_point, &z_extrinsic, depth_point); + rs_project_point_to_pixel(color_pixel, &color_intrinsic, color_point); if (color_pixel[1] < 0 || color_pixel[1] > image_color.rows || color_pixel[0] < 0 || color_pixel[0] > image_color.cols) @@ -820,7 +899,7 @@ namespace realsense_camera } msg_pointcloud.header.frame_id = frame_id_[RS_STREAM_DEPTH]; - pointcloud_publisher_.publish (msg_pointcloud); + pointcloud_publisher_.publish(msg_pointcloud); } } /* @@ -836,7 +915,8 @@ namespace realsense_camera rs_extrinsics z_extrinsic; // extrinsics are offsets between the cameras - rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); checkError (); + rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + checkError(); ros::Duration sleeper(0.1); // 100ms diff --git a/camera/src/realsense_camera_nodelet.h b/camera/src/realsense_camera_nodelet.h index 665d37a82e..0354588d83 100644 --- a/camera/src/realsense_camera_nodelet.h +++ b/camera/src/realsense_camera_nodelet.h @@ -76,7 +76,6 @@ class RealsenseNodelet: public nodelet::Nodelet // Default Constants. const int MAX_Z = 8; // in meters - const int DEFAULT_CAMERA = 0; // default camera index const std::string DEFAULT_MODE = "preset"; const int DEPTH_HEIGHT = 360; const int DEPTH_WIDTH = 480; @@ -88,7 +87,6 @@ class RealsenseNodelet: public nodelet::Nodelet const bool ENABLE_COLOR = true; const bool ENABLE_PC = true; const bool ENABLE_TF = true; - const uint32_t SERIAL_NUMBER = 0xFFFFFFFF; const rs_format DEPTH_FORMAT = RS_FORMAT_Z16; const rs_format COLOR_FORMAT = RS_FORMAT_RGB8; const rs_format IR1_FORMAT = RS_FORMAT_Y8; @@ -117,7 +115,10 @@ class RealsenseNodelet: public nodelet::Nodelet rs_error *rs_error_ = 0; rs_context *rs_context_; rs_device *rs_device_; + std::vector rs_detected_devices_; + int num_of_cameras_; + std::string serial_no_; int color_height_; int color_width_; int depth_height_; @@ -181,6 +182,7 @@ class RealsenseNodelet: public nodelet::Nodelet void getCameraOptions(); void allocateResources(); bool connectToCamera(); + rs_device * getCameraBySerialNumber(); void fillStreamEncoding(); void setStreamOptions(); void setStaticCameraOptions(); diff --git a/camera/test/realsense_camera_test_rgbd_node.cpp b/camera/test/realsense_camera_test_rgbd_node.cpp index d3cf52ecca..0ff6d23295 100644 --- a/camera/test/realsense_camera_test_rgbd_node.cpp +++ b/camera/test/realsense_camera_test_rgbd_node.cpp @@ -8,12 +8,12 @@ 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 + 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 + 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" @@ -135,71 +135,98 @@ void topic12Callback(const sensor_msgs::ImageConstPtr msg) } } -TEST (RealsenseTests, RGB_IMAGE_MONO) +TEST (RealsenseTests, rgb_image_mono) { EXPECT_TRUE (topic_0_recv); } -TEST (RealsenseTests, RGB_IMAGE_COLOR) +TEST (RealsenseTests, rgb_image_color) { EXPECT_TRUE (topic_1_recv); } -TEST (RealsenseTests, RGB_IMAGE_RECT_MONO) +TEST (RealsenseTests, rgb_image_rect_mono) { EXPECT_TRUE (topic_2_recv); } -TEST (RealsenseTests, RGB_IMAGE_RECT_COLOR) +TEST (RealsenseTests, rgb_image_rect_color) { EXPECT_TRUE (topic_3_recv); } -TEST (RealsenseTests, DEPTH_IMAGE_RECT_RAW) +TEST (RealsenseTests, depth_image_rect_raw) { EXPECT_TRUE (topic_4_recv); } -TEST (RealsenseTests, DEPTH_IMAGE_RECT) +TEST (RealsenseTests, depth_image_rect) { EXPECT_TRUE (topic_5_recv); } -TEST (RealsenseTests, DEPTH_IMAGE) +TEST (RealsenseTests, depth_image) { EXPECT_TRUE (topic_6_recv); } -TEST (RealsenseTests, DEPTH_POINTS) +TEST (RealsenseTests, depth_points) { EXPECT_TRUE (topic_7_recv); } -TEST (RealsenseTests, IR_IMAGE_RECT_IR) +TEST (RealsenseTests, ir_image_rect_ir) { EXPECT_TRUE (topic_8_recv); } -TEST (RealsenseTests, DEPTH_REG_SW_REG_IMAGE_RECT_RAW) +TEST (RealsenseTests, depth_reg_sw_reg_image_rect_raw) { EXPECT_TRUE (topic_9_recv); } -TEST (RealsenseTests, DEPTH_REG_SW_REG_CAMERA_INFO) +TEST (RealsenseTests, depth_reg_sw_reg_camera_info) { EXPECT_TRUE (topic_10_recv); } -TEST (RealsenseTests, DEPTH_REG_POINTS) +TEST (RealsenseTests, depth_reg_points) { EXPECT_TRUE (topic_11_recv); } -TEST (RealsenseTests, DEPTH_REG_SW_REG_IMAGE_RECT) +TEST (RealsenseTests, depth_reg_sw_reg_image_rect) { EXPECT_TRUE (topic_12_recv); } +void getParams() +{ + ros::NodeHandle pnh; + pnh.param("camera", camera, CAMERA); + pnh.param("rgb", rgb, RGB); + pnh.param("depth", depth, DEPTH); + pnh.param("ir", ir, IR); + pnh.param("depth_registered", depth_registered, DEPTH_REGISTERED); +} + +void setTopics() +{ + rgb_image_mono = "/" + camera + "/" + rgb + "/" + RGB_IMAGE_MONO; + rgb_image_color = "/" + camera + "/" + rgb + "/" + RGB_IMAGE_COLOR; + rgb_image_rect_mono = "/" + camera + "/" + rgb + "/" + RGB_IMAGE_RECT_MONO; + rgb_image_rect_color = "/" + camera + "/" + rgb + "/" + RGB_IMAGE_RECT_COLOR; + depth_image_rect_raw = "/" + camera + "/" + depth + "/" + DEPTH_IMAGE_RECT_RAW; + depth_image_rect = "/" + camera + "/" + depth + "/" + DEPTH_IMAGE_RECT; + depth_image = "/" + camera + "/" + depth + "/" + DEPTH_IMAGE; + depth_points = "/" + camera + "/" + depth + "/" + DEPTH_POINTS; + ir_image_rect_ir = "/" + camera + "/" + ir + "/" + IR_IMAGE_RECT_IR; + depth_reg_sw_reg_image_rect_raw = "/" + camera + "/" + depth_registered + "/" + DEPTH_REG_SW_REG_IMAGE_RECT_RAW; + depth_reg_sw_reg_camera_info = "/" + camera + "/" + depth_registered + "/" + DEPTH_REG_SW_REG_CAMERA_INFO; + depth_reg_points = "/" + camera + "/" + depth_registered + "/" + DEPTH_REG_POINTS; + depth_reg_sw_reg_image_rect = "/" + camera + "/" + depth_registered + "/" + DEPTH_REG_SW_REG_IMAGE_RECT; +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); @@ -208,20 +235,22 @@ int main(int argc, char **argv) ROS_INFO_STREAM("RealSense Camera - Starting rgbd_launch Tests..."); ros::NodeHandle nh; - - subscriber[0] = nh.subscribe < sensor_msgs::Image > (RGB_IMAGE_MONO, 1, topic0Callback, 0); - subscriber[1] = nh.subscribe < sensor_msgs::Image > (RGB_IMAGE_COLOR, 1, topic1Callback, 0); - subscriber[2] = nh.subscribe < sensor_msgs::Image > (RGB_IMAGE_RECT_MONO, 1, topic2Callback, 0); - subscriber[3] = nh.subscribe < sensor_msgs::Image > (RGB_IMAGE_RECT_COLOR, 1, topic3Callback, 0); - subscriber[4] = nh.subscribe < sensor_msgs::Image > (DEPTH_IMAGE_RECT_RAW, 1, topic4Callback, 0); - subscriber[5] = nh.subscribe < sensor_msgs::Image > (DEPTH_IMAGE_RECT, 1, topic5Callback, 0); - subscriber[6] = nh.subscribe < sensor_msgs::Image > (DEPTH_IMAGE, 1, topic6Callback, 0); - subscriber[7] = nh.subscribe < sensor_msgs::PointCloud2 > (DEPTH_POINTS, 1, topic7Callback); - subscriber[8] = nh.subscribe < sensor_msgs::Image > (IR_IMAGE_RECT_IR, 1, topic8Callback, 0); - subscriber[9] = nh.subscribe < sensor_msgs::Image > (DEPTH_REG_SW_REG_IMAGE_RECT_RAW, 1, topic9Callback, 0); - subscriber[10] = nh.subscribe < sensor_msgs::CameraInfo > (DEPTH_REG_SW_REG_CAMERA_INFO, 1, topic10Callback); - subscriber[11] = nh.subscribe < sensor_msgs::PointCloud2 > (DEPTH_REG_POINTS, 1, topic11Callback, 0); - subscriber[12] = nh.subscribe < sensor_msgs::Image > (DEPTH_REG_SW_REG_IMAGE_RECT, 1, topic12Callback, 0); + getParams(); + setTopics(); + + subscriber[0] = nh.subscribe(rgb_image_mono, 1, topic0Callback, 0); + subscriber[1] = nh.subscribe(rgb_image_color, 1, topic1Callback, 0); + subscriber[2] = nh.subscribe(rgb_image_rect_mono, 1, topic2Callback, 0); + subscriber[3] = nh.subscribe(rgb_image_rect_color, 1, topic3Callback, 0); + subscriber[4] = nh.subscribe(depth_image_rect_raw, 1, topic4Callback, 0); + subscriber[5] = nh.subscribe(depth_image_rect, 1, topic5Callback, 0); + subscriber[6] = nh.subscribe(depth_image, 1, topic6Callback, 0); + subscriber[7] = nh.subscribe(depth_points, 1, topic7Callback); + subscriber[8] = nh.subscribe(ir_image_rect_ir, 1, topic8Callback, 0); + subscriber[9] = nh.subscribe(depth_reg_sw_reg_image_rect_raw, 1, topic9Callback, 0); + subscriber[10] = nh.subscribe(depth_reg_sw_reg_camera_info, 1, topic10Callback); + subscriber[11] = nh.subscribe(depth_reg_points, 1, topic11Callback, 0); + subscriber[12] = nh.subscribe(depth_reg_sw_reg_image_rect, 1, topic12Callback, 0); ros::Duration duration; duration.sec = 10; diff --git a/camera/test/realsense_camera_test_rgbd_node.h b/camera/test/realsense_camera_test_rgbd_node.h index 1f4713e491..b78ea68dcd 100644 --- a/camera/test/realsense_camera_test_rgbd_node.h +++ b/camera/test/realsense_camera_test_rgbd_node.h @@ -8,12 +8,12 @@ 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 + 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 + 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" @@ -37,19 +37,45 @@ const static int TOPIC_COUNT = 13; -const char *RGB_IMAGE_MONO = "/camera/rgb/image_mono"; -const char *RGB_IMAGE_COLOR = "/camera/rgb/image_color"; -const char *RGB_IMAGE_RECT_MONO = "/camera/rgb/image_rect_mono"; -const char *RGB_IMAGE_RECT_COLOR = "/camera/rgb/image_rect_color"; -const char *DEPTH_IMAGE_RECT_RAW = "/camera/depth/image_rect_raw"; -const char *DEPTH_IMAGE_RECT = "/camera/depth/image_rect"; -const char *DEPTH_IMAGE = "/camera/depth/image"; -const char *DEPTH_POINTS = "/camera/depth/points"; -const char *IR_IMAGE_RECT_IR = "/camera/ir/image_rect_ir"; -const char *DEPTH_REG_SW_REG_IMAGE_RECT_RAW = "/camera/depth_registered/sw_registered/image_rect_raw"; -const char *DEPTH_REG_SW_REG_CAMERA_INFO = "/camera/depth_registered/sw_registered/camera_info"; -const char *DEPTH_REG_POINTS = "/camera/depth_registered/points"; -const char *DEPTH_REG_SW_REG_IMAGE_RECT = "/camera/depth_registered/sw_registered/image_rect"; +std::string RGB_IMAGE_MONO = "image_mono"; +std::string RGB_IMAGE_COLOR = "image_color"; +std::string RGB_IMAGE_RECT_MONO = "image_rect_mono"; +std::string RGB_IMAGE_RECT_COLOR = "image_rect_color"; +std::string DEPTH_IMAGE_RECT_RAW = "image_rect_raw"; +std::string DEPTH_IMAGE_RECT = "image_rect"; +std::string DEPTH_IMAGE = "image"; +std::string DEPTH_POINTS = "points"; +std::string IR_IMAGE_RECT_IR = "image_rect_ir"; +std::string DEPTH_REG_SW_REG_IMAGE_RECT_RAW = "sw_registered/image_rect_raw"; +std::string DEPTH_REG_SW_REG_CAMERA_INFO = "sw_registered/camera_info"; +std::string DEPTH_REG_POINTS = "points"; +std::string DEPTH_REG_SW_REG_IMAGE_RECT = "sw_registered/image_rect"; + +std::string CAMERA = "camera"; +std::string RGB = "rgb"; +std::string DEPTH = "depth"; +std::string IR = "ir"; +std::string DEPTH_REGISTERED = "depth_registered"; + +std::string camera; +std::string rgb; +std::string depth; +std::string ir; +std::string depth_registered; + +std::string rgb_image_mono; +std::string rgb_image_color; +std::string rgb_image_rect_mono; +std::string rgb_image_rect_color; +std::string depth_image_rect_raw; +std::string depth_image_rect; +std::string depth_image; +std::string depth_points; +std::string ir_image_rect_ir; +std::string depth_reg_sw_reg_image_rect_raw; +std::string depth_reg_sw_reg_camera_info; +std::string depth_reg_points; +std::string depth_reg_sw_reg_image_rect; bool topic_0_recv = false; bool topic_1_recv = false; diff --git a/camera/test/realsense_r200_dynamic_camera_options_params.launch b/camera/test/realsense_r200_dynamic_camera_options_params.launch index b57837cd3a..8529efe0c4 100644 --- a/camera/test/realsense_r200_dynamic_camera_options_params.launch +++ b/camera/test/realsense_r200_dynamic_camera_options_params.launch @@ -2,8 +2,8 @@ Launch file for testing if the dynamic camera options get set correctly using params. Steps: - roslaunch realsense_camera realsense_r200_static_camera_options_params.launch - rosrun realsense_camera realsense_camera_test COLOR_BACKLIGHT_COMPENSATION 3 COLOR_BRIGHTNESS 10 COLOR_CONTRAST 16 COLOR_GAIN 18 COLOR_GAMMA 100 COLOR_HUE 20 COLOR_SATURATION 21 COLOR_SHARPNESS 7 COLOR_ENABLE_AUTO_WHITE_BALANCE 0 COLOR_WHITE_BALANCE 3000 R200_LR_GAIN 200 R200_EMITTER_ENABLED 1 R200_LR_EXPOSURE 27 + roslaunch realsense_camera realsense_r200_dynamic_camera_options_params.launch + rosrun realsense_camera realsense_camera_test color_backlight_compensation 3 color_brightness 10 color_contrast 16 color_gain 18 color_gamma 100 color_hue 20 color_saturation 21 color_sharpness 7 color_enable_auto_white_balance 0 color_white_balance 3000 r200_lr_gain 200 r200_emitter_enabled 1 r200_lr_exposure 27 Verify that the "testCameraOptions" test pass. --> @@ -22,19 +22,19 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + @@ -54,19 +54,19 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + diff --git a/camera/test/realsense_r200_static_camera_options_params.launch b/camera/test/realsense_r200_static_camera_options_params.launch index 9ae48c0b6d..0e18fedac3 100644 --- a/camera/test/realsense_r200_static_camera_options_params.launch +++ b/camera/test/realsense_r200_static_camera_options_params.launch @@ -3,7 +3,7 @@ Steps: roslaunch realsense_camera realsense_r200_static_camera_options_params.launch - rosrun realsense_camera realsense_camera_test R200_DEPTH_UNITS 2 R200_DEPTH_CLAMP_MIN 3 R200_DEPTH_CLAMP_MAX 4 R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT 5 R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT 6 R200_DEPTH_CONTROL_MEDIAN_THRESHOLD 7 R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD 8 R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD 9 R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD 10 R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD 11 R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD 12 R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD 13 R200_DEPTH_CONTROL_LR_THRESHOLD 14 + rosrun realsense_camera realsense_camera_test r200_depth_units 2 r200_depth_clamp_min 3 r200_depth_clamp_max 4 r200_depth_control_estimate_median_decrement 5 r200_depth_control_estimate_median_increment 6 r200_depth_control_median_threshold 7 r200_depth_control_score_minimum_threshold 8 r200_depth_control_score_maximum_threshold 9 r200_depth_control_texture_count_threshold 10 r200_depth_control_texture_difference_threshold 11 r200_depth_control_second_peak_threshold 12 r200_depth_control_neighbor_threshold 13 r200_depth_control_lr_threshold 14 Verify that the "testCameraOptions" test pass. --> @@ -11,19 +11,19 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + @@ -31,19 +31,19 @@ args="load realsense_camera/RealsenseNodelet standalone_nodelet"> - - - - - - - - - - - - - + + + + + + + + + + + + +