From 2e34d3b4796dc0c2a4be7ce71b8d2c10ec00ab03 Mon Sep 17 00:00:00 2001 From: rjingar Date: Fri, 4 Mar 2016 16:16:34 -0800 Subject: [PATCH 01/15] Add dynamically enable/disable depth stream code --- camera/cfg/camera_params.cfg | 1 + camera/src/realsense_camera_nodelet.cpp | 277 ++++++++++++++++-------- camera/src/realsense_camera_nodelet.h | 4 + 3 files changed, 189 insertions(+), 93 deletions(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index ce1d2376f7..9d60718b21 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,6 +7,7 @@ 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", 56, 0, 255) gen.add("COLOR_CONTRAST", int_t, 0, "Contrast", 32, 16, 64) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index 361163cdac..cd475861ce 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -92,6 +92,9 @@ namespace realsense_camera frame_id_[RS_STREAM_INFRARED] = IR1_DEF_FRAME; frame_id_[RS_STREAM_INFRARED2] = IR2_DEF_FRAME; + for (int i = 0; i < STREAM_COUNT; ++i) + camera_info_[i] = NULL; + ros::NodeHandle & nh = getNodeHandle (); // Set up the topics. @@ -101,28 +104,18 @@ namespace realsense_camera getConfigValues (camera_configuration_); // Advertise the various topics and services. + // Advertise color stream only if color parameter is set. + camera_publisher_[RS_STREAM_COLOR] = it.advertiseCamera (COLOR_TOPIC, 1); // Advertise depth and infrared streams only if depth parameter is set. - if (enable_depth_ == true) + 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_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); - } - - // Advertise pointcloud only if pointcloud parameter is set. - if (enable_pointcloud_ == true) - { - pointcloud_publisher_ = nh.advertise < sensor_msgs::PointCloud2 > (PC_TOPIC, 1); - } + camera_publisher_[RS_STREAM_INFRARED2] = it.advertiseCamera (IR2_TOPIC, 1); } - // Advertise color stream only if color parameter is set. - if (enable_color_ == true) - { - camera_publisher_[RS_STREAM_COLOR] = it.advertiseCamera (COLOR_TOPIC, 1); - } + pointcloud_publisher_ = nh.advertise < sensor_msgs::PointCloud2 > (PC_TOPIC, 1); + get_options_service_ = nh.advertiseService (SETTINGS_SERVICE, &RealsenseNodelet::getCameraSettings, this); @@ -156,6 +149,90 @@ namespace realsense_camera /* *Private Methods. */ + void RealsenseNodelet::enable_color_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_COLOR; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_COLOR); + } + } + + void RealsenseNodelet::enable_depth_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_DEPTH; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_DEPTH); + } + } + + void RealsenseNodelet::enable_infrared_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_INFRARED); + } + } + + void RealsenseNodelet::enable_infrared2_stream() + { + // Enable streams. + 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); + } + 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); + } + uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED2; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_INFRARED2); + } + } + + + 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); @@ -203,7 +280,15 @@ namespace realsense_camera 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) { + enable_depth_ = false; + } + else { + enable_depth_ = true; + } + } void RealsenseNodelet::check_error () @@ -254,7 +339,7 @@ namespace realsense_camera 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_)); + rs_get_device_firmware_version (rs_device_, &rs_error_)); check_error (); ROS_INFO_STREAM ("RealSense Camera - Name: " << rs_get_device_name (rs_device_, &rs_error_)); check_error (); @@ -262,36 +347,16 @@ namespace realsense_camera check_error (); // Enable streams. - if (enable_depth_ == true) + if (enable_color_ == true) { - 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_); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED, depth_width_, depth_height_, IR1_FORMAT, depth_fps_, &rs_error_); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED2, depth_width_, depth_height_, IR2_FORMAT, depth_fps_, 0); - } - 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_); check_error (); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &rs_error_); check_error (); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED2, RS_PRESET_BEST_QUALITY, 0); - } + enable_color_stream(); } - if (enable_color_ == true) + if (enable_depth_ == true) { - 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_); - } - 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_); check_error (); - } + enable_depth_stream(); + enable_infrared_stream(); + enable_infrared2_stream(); } for (int i = 0; i < RS_OPTION_COUNT; ++i) @@ -299,7 +364,7 @@ namespace realsense_camera option_str o = { (rs_option) i}; - if (!rs_device_supports_option (rs_device_, o.opt, &rs_error_)) + if (!rs_device_supports_option (rs_device_, o.opt, 0)) { continue; } @@ -318,6 +383,7 @@ namespace realsense_camera // Start device. rs_start_device (rs_device_, &rs_error_); + check_error (); is_device_started_ = true; @@ -334,26 +400,12 @@ namespace realsense_camera // Prepare camera for enabled streams (color/depth/infrared/infrared2). fillStreamEncoding (); - if (rs_is_stream_enabled (rs_device_, RS_STREAM_COLOR, 0)) - { - image_[(uint32_t) RS_STREAM_COLOR] = cv::Mat (color_height_, color_width_, CV_8UC3, cv::Scalar (0, 0, 0)); - prepareStreamCalibData (RS_STREAM_COLOR); - } - - if (rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0)) + 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) { - // Define message. - image_[(uint32_t) RS_STREAM_DEPTH] = cv::Mat (depth_height_, depth_width_, CV_16UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_DEPTH); - - image_[(uint32_t) RS_STREAM_INFRARED] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_INFRARED); - - if (rs_is_stream_enabled (rs_device_, RS_STREAM_INFRARED2, 0)) - { image_[(uint32_t) RS_STREAM_INFRARED2] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_INFRARED2); - } } } @@ -425,7 +477,7 @@ namespace realsense_camera { option_str o = { (rs_option) i}; - if (!rs_device_supports_option (rs_device_, o.opt, &rs_error_)) + if (!rs_device_supports_option (rs_device_, o.opt, 0)) { continue; } @@ -636,45 +688,84 @@ namespace realsense_camera */ void RealsenseNodelet::publishStreams () { - rs_wait_for_frames (rs_device_, &rs_error_); - check_error (); - time_stamp_ = ros::Time::now (); - - for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) + if(enable_depth_ == false && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1) { - // Publish image stream only if there is at least one subscriber. - if (camera_publisher_[stream_index].getNumSubscribers () > 0) + if(rs_is_device_streaming(rs_device_, 0) == 1) { - prepareStreamData ((rs_stream) stream_index); - - sensor_msgs::ImagePtr msg = cv_bridge::CvImage (std_msgs::Header (), - stream_encoding_[stream_index], - image_[stream_index]).toImageMsg (); + rs_stop_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); + } - 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]; + rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Depth Stream Disabled"); - camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; - camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); + if(rs_is_device_streaming(rs_device_, 0) == 0) + { + rs_start_device (rs_device_, &rs_error_);check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Started"); + } + } + + if(enable_depth_ == true && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 0) + { + if(rs_is_device_streaming(rs_device_, 0) == 1) + { + rs_stop_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); + } + + enable_depth_stream(); + + if(rs_is_device_streaming(rs_device_, 0) == 0) + { + rs_start_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } - if (pointcloud_publisher_.getNumSubscribers () > 0 && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) - && enable_pointcloud_ == true) + if(rs_is_device_streaming(rs_device_, 0) == 1) { - if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers () <= 0) + rs_wait_for_frames (rs_device_, &rs_error_); + check_error (); + time_stamp_ = ros::Time::now (); + + for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) { - prepareStreamData (RS_STREAM_DEPTH); + // 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) + { + 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]; + + camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; + camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); + } } - if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers () <= 0) + + if (pointcloud_publisher_.getNumSubscribers () > 0 && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1 + && enable_pointcloud_ == true) { - prepareStreamData (RS_STREAM_COLOR); + if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers () <= 0) + { + prepareStreamData (RS_STREAM_DEPTH); + } + if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers () <= 0) + { + prepareStreamData (RS_STREAM_COLOR); + } + publishPointCloud (image_[(uint32_t) RS_STREAM_COLOR]); } - publishPointCloud (image_[(uint32_t) RS_STREAM_COLOR]); } } diff --git a/camera/src/realsense_camera_nodelet.h b/camera/src/realsense_camera_nodelet.h index dd27b3f8a1..645c2433e0 100644 --- a/camera/src/realsense_camera_nodelet.h +++ b/camera/src/realsense_camera_nodelet.h @@ -165,6 +165,10 @@ class RealsenseNodelet: public nodelet::Nodelet boost::shared_ptr> dynamic_reconf_server_; // Member Functions. + void enable_color_stream(); + void enable_depth_stream(); + void enable_infrared_stream(); + void enable_infrared2_stream(); void check_error(); void fetchCalibData(); void prepareStreamCalibData(rs_stream calib_data); From dc9881ac425d5996d37376f50cc7bc33bbc5fde8 Mon Sep 17 00:00:00 2001 From: rjingar Date: Thu, 10 Mar 2016 17:53:40 -0800 Subject: [PATCH 02/15] Add unit tests for Enable/disable depth stream --- camera/test/realsense_camera_test_node.cpp | 24 ++++++++++++++++------ camera/test/realsense_camera_test_node.h | 2 ++ 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 7cfccd9e01..1daeb29b0b 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -262,6 +262,18 @@ TEST (RealsenseTests, testColorCameraInfo) } } +TEST (RealsenseTests, testIsDepthStreamEnabled) +{ + if (enable_depth) + { + EXPECT_TRUE (depth_recv); + } + else + { + EXPECT_FALSE (depth_recv); + } +} + TEST (RealsenseTests, testDepthStream) { if (enable_depth) @@ -317,7 +329,7 @@ TEST (RealsenseTests, testDepthCameraInfo) TEST (RealsenseTests, testInfrared1Stream) { - if (enable_depth) + if (enable_infrared1) { EXPECT_TRUE (infrared1_avg > 0); EXPECT_TRUE (infrared1_recv); @@ -338,7 +350,7 @@ TEST (RealsenseTests, testInfrared1Stream) TEST (RealsenseTests, testInfrared1Resolution) { - if (enable_depth) + if (enable_infrared1) { if (depth_width_exp > 0) { @@ -353,7 +365,7 @@ TEST (RealsenseTests, testInfrared1Resolution) TEST (RealsenseTests, testInfrared1CameraInfo) { - if (enable_depth) + if (enable_infrared1) { EXPECT_EQ (infrared1_width_recv, inf1_caminfo_width_recv); EXPECT_EQ (infrared1_height_recv, inf1_caminfo_height_recv); @@ -369,7 +381,7 @@ TEST (RealsenseTests, testInfrared1CameraInfo) TEST (RealsenseTests, testInfrared2Stream) { - if (enable_depth) + if (enable_infrared2) { EXPECT_TRUE (infrared2_avg > 0); EXPECT_TRUE (infrared2_recv); @@ -382,7 +394,7 @@ TEST (RealsenseTests, testInfrared2Stream) TEST (RealsenseTests, testInfrared2Resolution) { - if (enable_depth) + if (enable_infrared2) { if (depth_width_exp > 0) { @@ -397,7 +409,7 @@ TEST (RealsenseTests, testInfrared2Resolution) TEST (RealsenseTests, testInfrared2CameraInfo) { - if (enable_depth) + if (enable_infrared2) { EXPECT_EQ (infrared2_width_recv, inf2_caminfo_width_recv); EXPECT_EQ (infrared2_height_recv, inf2_caminfo_height_recv); diff --git a/camera/test/realsense_camera_test_node.h b/camera/test/realsense_camera_test_node.h index 5e9dc5c6aa..ab06dbe8fc 100644 --- a/camera/test/realsense_camera_test_node.h +++ b/camera/test/realsense_camera_test_node.h @@ -80,6 +80,8 @@ uint32_t infrared2_step_exp; // Expected infrared2 step. bool enable_color = true; bool enable_depth = true; +bool enable_infrared1 = true; +bool enable_infrared2 = true; bool enable_pointcloud = true; std::string depth_encoding_exp; // Expected depth encoding. From 7ff2eeba7f47000bce910ad0f6b51e1cc18c5795 Mon Sep 17 00:00:00 2001 From: rjingar Date: Mon, 14 Mar 2016 08:39:58 -0700 Subject: [PATCH 03/15] Added Enable/disable infrared streams along with depth stream --- camera/src/realsense_camera_nodelet.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index cd475861ce..4b8847626f 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -699,6 +699,12 @@ namespace realsense_camera rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); ROS_INFO_STREAM ("RealSense Camera - Depth Stream Disabled"); + rs_disable_stream(rs_device_, RS_STREAM_INFRARED, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Infrared1 stream Disabled"); + + rs_disable_stream(rs_device_, RS_STREAM_INFRARED2, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Infrared2 stream Disabled"); + if(rs_is_device_streaming(rs_device_, 0) == 0) { rs_start_device (rs_device_, &rs_error_);check_error (); @@ -715,7 +721,9 @@ namespace realsense_camera } enable_depth_stream(); - + enable_infrared_stream(); + enable_infrared2_stream(); + if(rs_is_device_streaming(rs_device_, 0) == 0) { rs_start_device (rs_device_, &rs_error_); check_error (); From 589eeeaf9dfdb82bab6b37030feeecb2e37f9646 Mon Sep 17 00:00:00 2001 From: rjingar Date: Mon, 14 Mar 2016 14:58:42 -0700 Subject: [PATCH 04/15] Added Enable/disable infrared streams along with depth stream. --- camera/README.md | 7 +- camera/src/realsense_camera_nodelet.cpp | 109 ++++++++++++--------- camera/src/realsense_camera_nodelet.h | 10 +- camera/test/realsense_camera_test_node.cpp | 16 +-- 4 files changed, 83 insertions(+), 59 deletions(-) diff --git a/camera/README.md b/camera/README.md index 46959655a1..f804bfd891 100644 --- a/camera/README.md +++ b/camera/README.md @@ -88,7 +88,7 @@ Infrared2 camera depth_fps (int, default: 60) Specify the depth camera FPS enable_depth (bool, default: true) - Specify if to enable or not the depth camera. + Specify if to enable or not the depth and infrared camera. enable_color (bool, default: true) Specify if to enable or not the color camera. enable_pointcloud (bool, default: true) @@ -141,6 +141,11 @@ To get supported camera options with current value set. It returns string in opt R200_AUTO_EXPOSURE_LEFT_EDGE R200_AUTO_EXPOSURE_RIGHT_EDGE + Enable/Disable Stream: + ENABLE_DEPTH + Check or uncheck the option to disable/enable depth and infrared streams dynamically. + Note: Infrared streams will be enabled or disabled along with depth stream. + Use rqt_reconfigure GUI to view and edit the parameters that are accessible via dynamic_reconfigure. Command to launch GUI: diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index 4b8847626f..37fc057ed3 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -58,7 +58,7 @@ namespace realsense_camera { rs_stop_device (rs_device_, 0); rs_delete_context (rs_context_, &rs_error_); - check_error (); + checkError (); } ROS_INFO_STREAM ("RealSense Camera - Stopping camera nodelet."); @@ -149,19 +149,19 @@ namespace realsense_camera /* *Private Methods. */ - void RealsenseNodelet::enable_color_stream() + void RealsenseNodelet::enableColorStream() { // Enable streams. 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_); - check_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_); check_error (); + 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; @@ -170,49 +170,51 @@ namespace realsense_camera } } - void RealsenseNodelet::enable_depth_stream() + void RealsenseNodelet::enableDepthStream() { // Enable streams. 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_); - check_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_); check_error (); + 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); } } - void RealsenseNodelet::enable_infrared_stream() + void RealsenseNodelet::enableInfraredStream() { // Enable streams. 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_); - check_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_); check_error (); + 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); } } - void RealsenseNodelet::enable_infrared2_stream() + void RealsenseNodelet::enableInfrared2Stream() { // Enable streams. if (mode_.compare ("manual") == 0) @@ -225,8 +227,10 @@ namespace realsense_camera ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared2 Stream: preset mode"); 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); } } @@ -282,16 +286,25 @@ namespace realsense_camera rs_set_device_options(rs_device_, edge_options_, 4, edge_values_, 0); } - if(config.ENABLE_DEPTH == false) { - enable_depth_ = false; + if(config.ENABLE_DEPTH == false) + { + if(enable_color_ == false) + { + ROS_INFO_STREAM ("RealSense Camera - Color stream is also disabled. Cannot disable depth stream"); + config.ENABLE_DEPTH = true; + } + else + { + enable_depth_ = false; + } } - else { + else + { enable_depth_ = true; } - } - void RealsenseNodelet::check_error () + void RealsenseNodelet::checkError () { if (rs_error_) { @@ -325,7 +338,7 @@ namespace realsense_camera } rs_context_ = rs_create_context (RS_API_VERSION, &rs_error_); - check_error (); + checkError (); int num_of_cameras = rs_get_device_count (rs_context_, NULL); if (num_of_cameras < 1) @@ -335,28 +348,28 @@ namespace realsense_camera } rs_device_ = rs_get_device (rs_context_, 0, &rs_error_); - check_error (); + checkError (); 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_)); - check_error (); + checkError (); ROS_INFO_STREAM ("RealSense Camera - Name: " << rs_get_device_name (rs_device_, &rs_error_)); - check_error (); + checkError (); ROS_INFO_STREAM ("RealSense Camera - Serial no: " << rs_get_device_serial (rs_device_, &rs_error_)); - check_error (); + checkError (); // Enable streams. if (enable_color_ == true) { - enable_color_stream(); + enableColorStream(); } if (enable_depth_ == true) { - enable_depth_stream(); - enable_infrared_stream(); - enable_infrared2_stream(); + enableDepthStream(); + enableInfraredStream(); + enableInfrared2Stream(); } for (int i = 0; i < RS_OPTION_COUNT; ++i) @@ -383,7 +396,7 @@ namespace realsense_camera // Start device. rs_start_device (rs_device_, &rs_error_); - check_error (); + checkError (); is_device_started_ = true; @@ -417,7 +430,7 @@ namespace realsense_camera uint32_t stream_index = (uint32_t) rs_strm; rs_intrinsics intrinsic; rs_get_stream_intrinsics (rs_device_, rs_strm, &intrinsic, &rs_error_); - check_error (); + checkError (); camera_info_[stream_index] = new sensor_msgs::CameraInfo (); camera_info_ptr_[stream_index] = sensor_msgs::CameraInfoPtr (camera_info_[stream_index]); @@ -634,7 +647,7 @@ namespace realsense_camera { ROS_INFO_STREAM ("RealSense Camera - Setting" << opt_name.c_str () << " to " << config_.at (opt_name).c_str ()); rs_set_device_option (rs_device_, o.opt, atoi (config_.at (opt_name).c_str ()), &rs_error_); - check_error (); + checkError (); } cam_options.pop_back (); } @@ -692,22 +705,24 @@ namespace realsense_camera { if(rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); check_error (); + rs_stop_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } - rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); + + //disable depth, infrared1 and infrared2 streams + 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_); check_error (); + 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_); check_error (); + 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) { - rs_start_device (rs_device_, &rs_error_);check_error (); + rs_start_device (rs_device_, &rs_error_);checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } @@ -716,17 +731,17 @@ namespace realsense_camera { if(rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); check_error (); + rs_stop_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } - enable_depth_stream(); - enable_infrared_stream(); - enable_infrared2_stream(); + enableDepthStream(); + enableInfraredStream(); + enableInfrared2Stream(); if(rs_is_device_streaming(rs_device_, 0) == 0) { - rs_start_device (rs_device_, &rs_error_); check_error (); + rs_start_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } @@ -734,7 +749,7 @@ namespace realsense_camera if(rs_is_device_streaming(rs_device_, 0) == 1) { rs_wait_for_frames (rs_device_, &rs_error_); - check_error (); + checkError (); time_stamp_ = ros::Time::now (); for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) @@ -790,12 +805,12 @@ namespace realsense_camera rs_intrinsics z_intrinsic; rs_get_stream_intrinsics (rs_device_, RS_STREAM_DEPTH, &z_intrinsic, &rs_error_); - check_error (); + checkError (); if (enable_color_ == true) { - rs_get_stream_intrinsics (rs_device_, RS_STREAM_COLOR, &color_intrinsic, &rs_error_); check_error (); - rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); check_error (); + 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. @@ -826,7 +841,7 @@ namespace realsense_camera 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_); - check_error ();// Default value is 0.001 + checkError ();// Default value is 0.001 // Fill the PointCloud2 fields. for (int y = 0; y < z_intrinsic.height; y++) @@ -901,7 +916,7 @@ 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_); check_error (); + 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 645c2433e0..df105b6fbf 100644 --- a/camera/src/realsense_camera_nodelet.h +++ b/camera/src/realsense_camera_nodelet.h @@ -165,11 +165,11 @@ class RealsenseNodelet: public nodelet::Nodelet boost::shared_ptr> dynamic_reconf_server_; // Member Functions. - void enable_color_stream(); - void enable_depth_stream(); - void enable_infrared_stream(); - void enable_infrared2_stream(); - void check_error(); + void enableColorStream(); + void enableDepthStream(); + void enableInfraredStream(); + void enableInfrared2Stream(); + void checkError(); void fetchCalibData(); void prepareStreamCalibData(rs_stream calib_data); void prepareStreamData(rs_stream rs_strm); diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 1daeb29b0b..06c5522c6b 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -267,10 +267,14 @@ TEST (RealsenseTests, testIsDepthStreamEnabled) if (enable_depth) { EXPECT_TRUE (depth_recv); + EXPECT_TRUE (infrared1_recv); + EXPECT_TRUE (infrared2_recv); } else { EXPECT_FALSE (depth_recv); + EXPECT_FALSE (infrared1_recv); + EXPECT_FALSE (infrared2_recv); } } @@ -329,7 +333,7 @@ TEST (RealsenseTests, testDepthCameraInfo) TEST (RealsenseTests, testInfrared1Stream) { - if (enable_infrared1) + if (enable_depth) { EXPECT_TRUE (infrared1_avg > 0); EXPECT_TRUE (infrared1_recv); @@ -350,7 +354,7 @@ TEST (RealsenseTests, testInfrared1Stream) TEST (RealsenseTests, testInfrared1Resolution) { - if (enable_infrared1) + if (enable_depth) { if (depth_width_exp > 0) { @@ -365,7 +369,7 @@ TEST (RealsenseTests, testInfrared1Resolution) TEST (RealsenseTests, testInfrared1CameraInfo) { - if (enable_infrared1) + if (enable_depth) { EXPECT_EQ (infrared1_width_recv, inf1_caminfo_width_recv); EXPECT_EQ (infrared1_height_recv, inf1_caminfo_height_recv); @@ -381,7 +385,7 @@ TEST (RealsenseTests, testInfrared1CameraInfo) TEST (RealsenseTests, testInfrared2Stream) { - if (enable_infrared2) + if (enable_depth) { EXPECT_TRUE (infrared2_avg > 0); EXPECT_TRUE (infrared2_recv); @@ -394,7 +398,7 @@ TEST (RealsenseTests, testInfrared2Stream) TEST (RealsenseTests, testInfrared2Resolution) { - if (enable_infrared2) + if (enable_depth) { if (depth_width_exp > 0) { @@ -409,7 +413,7 @@ TEST (RealsenseTests, testInfrared2Resolution) TEST (RealsenseTests, testInfrared2CameraInfo) { - if (enable_infrared2) + if (enable_depth) { EXPECT_EQ (infrared2_width_recv, inf2_caminfo_width_recv); EXPECT_EQ (infrared2_height_recv, inf2_caminfo_height_recv); From b9a139d5008d9d527eea2b98a86060447d73129d Mon Sep 17 00:00:00 2001 From: Rajvi Jingar Date: Mon, 14 Mar 2016 17:07:03 -0500 Subject: [PATCH 05/15] Update realsense_camera_test_node.h removed enable_infrared1 and enable_infrared2 flags --- camera/test/realsense_camera_test_node.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/camera/test/realsense_camera_test_node.h b/camera/test/realsense_camera_test_node.h index ab06dbe8fc..5e9dc5c6aa 100644 --- a/camera/test/realsense_camera_test_node.h +++ b/camera/test/realsense_camera_test_node.h @@ -80,8 +80,6 @@ uint32_t infrared2_step_exp; // Expected infrared2 step. bool enable_color = true; bool enable_depth = true; -bool enable_infrared1 = true; -bool enable_infrared2 = true; bool enable_pointcloud = true; std::string depth_encoding_exp; // Expected depth encoding. From b2946e01f5f7279f24ab976892ba2705b19b4334 Mon Sep 17 00:00:00 2001 From: Rajvi Jingar Date: Tue, 15 Mar 2016 14:31:55 -0500 Subject: [PATCH 06/15] Update camera_params.cfg --- camera/cfg/camera_params.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index 9d60718b21..0dca975f71 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,7 +7,7 @@ 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("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) From 56061ee208968b1e357eb54b4e2fd65a7a198824 Mon Sep 17 00:00:00 2001 From: rjingar Date: Tue, 15 Mar 2016 12:55:10 -0700 Subject: [PATCH 07/15] Fixec cfg formatting --- camera/cfg/camera_params.cfg | 40 ++++++++++++++++++------------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index 0dca975f71..abebab2fe0 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,25 +7,25 @@ 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", 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("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) exit(gen.generate(PACKAGE, "realsense_camera", "camera_params")) From d4f9e56d0ba091965b9fef72a9872dec5708959f Mon Sep 17 00:00:00 2001 From: rjingar Date: Fri, 4 Mar 2016 16:16:34 -0800 Subject: [PATCH 08/15] Add dynamically enable/disable depth stream code --- camera/cfg/camera_params.cfg | 1 + camera/src/realsense_camera_nodelet.cpp | 277 ++++++++++++++++-------- camera/src/realsense_camera_nodelet.h | 4 + 3 files changed, 189 insertions(+), 93 deletions(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index ce1d2376f7..9d60718b21 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,6 +7,7 @@ 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", 56, 0, 255) gen.add("COLOR_CONTRAST", int_t, 0, "Contrast", 32, 16, 64) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index f5a64d4997..61d9db400e 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -92,6 +92,9 @@ namespace realsense_camera frame_id_[RS_STREAM_INFRARED] = IR1_DEF_FRAME; frame_id_[RS_STREAM_INFRARED2] = IR2_DEF_FRAME; + for (int i = 0; i < STREAM_COUNT; ++i) + camera_info_[i] = NULL; + ros::NodeHandle & nh = getNodeHandle (); // Set up the topics. @@ -101,28 +104,18 @@ namespace realsense_camera getConfigValues (camera_configuration_); // Advertise the various topics and services. + // Advertise color stream only if color parameter is set. + camera_publisher_[RS_STREAM_COLOR] = it.advertiseCamera (COLOR_TOPIC, 1); // Advertise depth and infrared streams only if depth parameter is set. - if (enable_depth_ == true) + 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_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); - } - - // Advertise pointcloud only if pointcloud parameter is set. - if (enable_pointcloud_ == true) - { - pointcloud_publisher_ = nh.advertise < sensor_msgs::PointCloud2 > (PC_TOPIC, 1); - } + camera_publisher_[RS_STREAM_INFRARED2] = it.advertiseCamera (IR2_TOPIC, 1); } - // Advertise color stream only if color parameter is set. - if (enable_color_ == true) - { - camera_publisher_[RS_STREAM_COLOR] = it.advertiseCamera (COLOR_TOPIC, 1); - } + pointcloud_publisher_ = nh.advertise < sensor_msgs::PointCloud2 > (PC_TOPIC, 1); + get_options_service_ = nh.advertiseService (SETTINGS_SERVICE, &RealsenseNodelet::getCameraSettings, this); @@ -156,6 +149,90 @@ namespace realsense_camera /* *Private Methods. */ + void RealsenseNodelet::enable_color_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_COLOR; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_COLOR); + } + } + + void RealsenseNodelet::enable_depth_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_DEPTH; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_DEPTH); + } + } + + void RealsenseNodelet::enable_infrared_stream() + { + // Enable streams. + 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_); + check_error (); + } + 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_); check_error (); + } + + uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_INFRARED); + } + } + + void RealsenseNodelet::enable_infrared2_stream() + { + // Enable streams. + 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); + } + 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); + } + uint32_t stream_index = (uint32_t) RS_STREAM_INFRARED2; + if(camera_info_[stream_index] == NULL) { + prepareStreamCalibData (RS_STREAM_INFRARED2); + } + } + + + 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); @@ -203,7 +280,15 @@ namespace realsense_camera 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) { + enable_depth_ = false; + } + else { + enable_depth_ = true; + } + } void RealsenseNodelet::check_error () @@ -254,7 +339,7 @@ namespace realsense_camera 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_)); + rs_get_device_firmware_version (rs_device_, &rs_error_)); check_error (); ROS_INFO_STREAM ("RealSense Camera - Name: " << rs_get_device_name (rs_device_, &rs_error_)); check_error (); @@ -262,36 +347,16 @@ namespace realsense_camera check_error (); // Enable streams. - if (enable_depth_ == true) + if (enable_color_ == true) { - 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_); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED, depth_width_, depth_height_, IR1_FORMAT, depth_fps_, &rs_error_); - rs_enable_stream (rs_device_, RS_STREAM_INFRARED2, depth_width_, depth_height_, IR2_FORMAT, depth_fps_, 0); - } - 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_); check_error (); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &rs_error_); check_error (); - rs_enable_stream_preset (rs_device_, RS_STREAM_INFRARED2, RS_PRESET_BEST_QUALITY, 0); - } + enable_color_stream(); } - if (enable_color_ == true) + if (enable_depth_ == true) { - 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_); - } - 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_); check_error (); - } + enable_depth_stream(); + enable_infrared_stream(); + enable_infrared2_stream(); } for (int i = 0; i < RS_OPTION_COUNT; ++i) @@ -299,7 +364,7 @@ namespace realsense_camera option_str o = { (rs_option) i}; - if (!rs_device_supports_option (rs_device_, o.opt, &rs_error_)) + if (!rs_device_supports_option (rs_device_, o.opt, 0)) { continue; } @@ -318,6 +383,7 @@ namespace realsense_camera // Start device. rs_start_device (rs_device_, &rs_error_); + check_error (); is_device_started_ = true; @@ -334,26 +400,12 @@ namespace realsense_camera // Prepare camera for enabled streams (color/depth/infrared/infrared2). fillStreamEncoding (); - if (rs_is_stream_enabled (rs_device_, RS_STREAM_COLOR, 0)) - { - image_[(uint32_t) RS_STREAM_COLOR] = cv::Mat (color_height_, color_width_, CV_8UC3, cv::Scalar (0, 0, 0)); - prepareStreamCalibData (RS_STREAM_COLOR); - } - - if (rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0)) + 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) { - // Define message. - image_[(uint32_t) RS_STREAM_DEPTH] = cv::Mat (depth_height_, depth_width_, CV_16UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_DEPTH); - - image_[(uint32_t) RS_STREAM_INFRARED] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_INFRARED); - - if (rs_is_stream_enabled (rs_device_, RS_STREAM_INFRARED2, 0)) - { image_[(uint32_t) RS_STREAM_INFRARED2] = cv::Mat (depth_height_, depth_width_, CV_8UC1, cv::Scalar (0)); - prepareStreamCalibData (RS_STREAM_INFRARED2); - } } } @@ -435,7 +487,7 @@ namespace realsense_camera { option_str o = { (rs_option) i}; - if (!rs_device_supports_option (rs_device_, o.opt, &rs_error_)) + if (!rs_device_supports_option (rs_device_, o.opt, 0)) { continue; } @@ -646,45 +698,84 @@ namespace realsense_camera */ void RealsenseNodelet::publishStreams () { - rs_wait_for_frames (rs_device_, &rs_error_); - check_error (); - time_stamp_ = ros::Time::now (); - - for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) + if(enable_depth_ == false && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1) { - // Publish image stream only if there is at least one subscriber. - if (camera_publisher_[stream_index].getNumSubscribers () > 0) + if(rs_is_device_streaming(rs_device_, 0) == 1) { - prepareStreamData ((rs_stream) stream_index); - - sensor_msgs::ImagePtr msg = cv_bridge::CvImage (std_msgs::Header (), - stream_encoding_[stream_index], - image_[stream_index]).toImageMsg (); + rs_stop_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); + } - 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]; + rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Depth Stream Disabled"); - camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; - camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); + if(rs_is_device_streaming(rs_device_, 0) == 0) + { + rs_start_device (rs_device_, &rs_error_);check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Started"); + } + } + + if(enable_depth_ == true && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 0) + { + if(rs_is_device_streaming(rs_device_, 0) == 1) + { + rs_stop_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); + } + + enable_depth_stream(); + + if(rs_is_device_streaming(rs_device_, 0) == 0) + { + rs_start_device (rs_device_, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } - if (pointcloud_publisher_.getNumSubscribers () > 0 && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) - && enable_pointcloud_ == true) + if(rs_is_device_streaming(rs_device_, 0) == 1) { - if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers () <= 0) + rs_wait_for_frames (rs_device_, &rs_error_); + check_error (); + time_stamp_ = ros::Time::now (); + + for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) { - prepareStreamData (RS_STREAM_DEPTH); + // 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) + { + 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]; + + camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp; + camera_publisher_[stream_index].publish (msg, camera_info_ptr_[stream_index]); + } } - if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers () <= 0) + + if (pointcloud_publisher_.getNumSubscribers () > 0 && rs_is_stream_enabled (rs_device_, RS_STREAM_DEPTH, 0) == 1 + && enable_pointcloud_ == true) { - prepareStreamData (RS_STREAM_COLOR); + if (camera_publisher_[(uint32_t) RS_STREAM_DEPTH].getNumSubscribers () <= 0) + { + prepareStreamData (RS_STREAM_DEPTH); + } + if (camera_publisher_[(uint32_t) RS_STREAM_COLOR].getNumSubscribers () <= 0) + { + prepareStreamData (RS_STREAM_COLOR); + } + publishPointCloud (image_[(uint32_t) RS_STREAM_COLOR]); } - publishPointCloud (image_[(uint32_t) RS_STREAM_COLOR]); } } diff --git a/camera/src/realsense_camera_nodelet.h b/camera/src/realsense_camera_nodelet.h index dd27b3f8a1..645c2433e0 100644 --- a/camera/src/realsense_camera_nodelet.h +++ b/camera/src/realsense_camera_nodelet.h @@ -165,6 +165,10 @@ class RealsenseNodelet: public nodelet::Nodelet boost::shared_ptr> dynamic_reconf_server_; // Member Functions. + void enable_color_stream(); + void enable_depth_stream(); + void enable_infrared_stream(); + void enable_infrared2_stream(); void check_error(); void fetchCalibData(); void prepareStreamCalibData(rs_stream calib_data); From 9886939bab9f1226ed7201600ae241a340cf3630 Mon Sep 17 00:00:00 2001 From: rjingar Date: Thu, 10 Mar 2016 17:53:40 -0800 Subject: [PATCH 09/15] Add unit tests for Enable/disable depth stream --- camera/test/realsense_camera_test_node.cpp | 24 ++++++++++++++++------ camera/test/realsense_camera_test_node.h | 2 ++ 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 2515ab1bee..7938aca0f5 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -300,6 +300,18 @@ TEST (RealsenseTests, testColorCameraInfo) } } +TEST (RealsenseTests, testIsDepthStreamEnabled) +{ + if (enable_depth) + { + EXPECT_TRUE (depth_recv); + } + else + { + EXPECT_FALSE (depth_recv); + } +} + TEST (RealsenseTests, testDepthStream) { if (enable_depth) @@ -370,7 +382,7 @@ TEST (RealsenseTests, testDepthCameraInfo) TEST (RealsenseTests, testInfrared1Stream) { - if (enable_depth) + if (enable_infrared1) { EXPECT_TRUE (infrared1_avg > 0); EXPECT_TRUE (infrared1_recv); @@ -391,7 +403,7 @@ TEST (RealsenseTests, testInfrared1Stream) TEST (RealsenseTests, testInfrared1Resolution) { - if (enable_depth) + if (enable_infrared1) { if (depth_width_exp > 0) { @@ -406,7 +418,7 @@ TEST (RealsenseTests, testInfrared1Resolution) TEST (RealsenseTests, testInfrared1CameraInfo) { - if (enable_depth) + if (enable_infrared1) { EXPECT_EQ (infrared1_width_recv, inf1_caminfo_width_recv); EXPECT_EQ (infrared1_height_recv, inf1_caminfo_height_recv); @@ -437,7 +449,7 @@ TEST (RealsenseTests, testInfrared1CameraInfo) TEST (RealsenseTests, testInfrared2Stream) { - if (enable_depth) + if (enable_infrared2) { EXPECT_TRUE (infrared2_avg > 0); EXPECT_TRUE (infrared2_recv); @@ -450,7 +462,7 @@ TEST (RealsenseTests, testInfrared2Stream) TEST (RealsenseTests, testInfrared2Resolution) { - if (enable_depth) + if (enable_infrared2) { if (depth_width_exp > 0) { @@ -465,7 +477,7 @@ TEST (RealsenseTests, testInfrared2Resolution) TEST (RealsenseTests, testInfrared2CameraInfo) { - if (enable_depth) + if (enable_infrared2) { EXPECT_EQ (infrared2_width_recv, inf2_caminfo_width_recv); EXPECT_EQ (infrared2_height_recv, inf2_caminfo_height_recv); diff --git a/camera/test/realsense_camera_test_node.h b/camera/test/realsense_camera_test_node.h index df3cc4049a..395f196d34 100644 --- a/camera/test/realsense_camera_test_node.h +++ b/camera/test/realsense_camera_test_node.h @@ -80,6 +80,8 @@ uint32_t infrared2_step_exp; // Expected infrared2 step. bool enable_color = true; bool enable_depth = true; +bool enable_infrared1 = true; +bool enable_infrared2 = true; bool enable_pointcloud = true; std::string depth_encoding_exp; // Expected depth encoding. From 3394f4057d39468a857a8e775f1a59dcecf9cbff Mon Sep 17 00:00:00 2001 From: rjingar Date: Mon, 14 Mar 2016 08:39:58 -0700 Subject: [PATCH 10/15] Added Enable/disable infrared streams along with depth stream --- camera/src/realsense_camera_nodelet.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index 61d9db400e..d778f23c4e 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -709,6 +709,12 @@ namespace realsense_camera rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); ROS_INFO_STREAM ("RealSense Camera - Depth Stream Disabled"); + rs_disable_stream(rs_device_, RS_STREAM_INFRARED, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Infrared1 stream Disabled"); + + rs_disable_stream(rs_device_, RS_STREAM_INFRARED2, &rs_error_); check_error (); + ROS_INFO_STREAM ("RealSense Camera - Infrared2 stream Disabled"); + if(rs_is_device_streaming(rs_device_, 0) == 0) { rs_start_device (rs_device_, &rs_error_);check_error (); @@ -725,7 +731,9 @@ namespace realsense_camera } enable_depth_stream(); - + enable_infrared_stream(); + enable_infrared2_stream(); + if(rs_is_device_streaming(rs_device_, 0) == 0) { rs_start_device (rs_device_, &rs_error_); check_error (); From 3202a2fd49194894efff176b4b37ce39df77fdef Mon Sep 17 00:00:00 2001 From: rjingar Date: Mon, 14 Mar 2016 14:58:42 -0700 Subject: [PATCH 11/15] Added Enable/disable infrared streams along with depth stream. --- camera/README.md | 7 +- camera/src/realsense_camera_nodelet.cpp | 109 ++++++++++++--------- camera/src/realsense_camera_nodelet.h | 10 +- camera/test/realsense_camera_test_node.cpp | 16 +-- 4 files changed, 83 insertions(+), 59 deletions(-) diff --git a/camera/README.md b/camera/README.md index 46959655a1..f804bfd891 100644 --- a/camera/README.md +++ b/camera/README.md @@ -88,7 +88,7 @@ Infrared2 camera depth_fps (int, default: 60) Specify the depth camera FPS enable_depth (bool, default: true) - Specify if to enable or not the depth camera. + Specify if to enable or not the depth and infrared camera. enable_color (bool, default: true) Specify if to enable or not the color camera. enable_pointcloud (bool, default: true) @@ -141,6 +141,11 @@ To get supported camera options with current value set. It returns string in opt R200_AUTO_EXPOSURE_LEFT_EDGE R200_AUTO_EXPOSURE_RIGHT_EDGE + Enable/Disable Stream: + ENABLE_DEPTH + Check or uncheck the option to disable/enable depth and infrared streams dynamically. + Note: Infrared streams will be enabled or disabled along with depth stream. + Use rqt_reconfigure GUI to view and edit the parameters that are accessible via dynamic_reconfigure. Command to launch GUI: diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index d778f23c4e..13a799ab24 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -58,7 +58,7 @@ namespace realsense_camera { rs_stop_device (rs_device_, 0); rs_delete_context (rs_context_, &rs_error_); - check_error (); + checkError (); } ROS_INFO_STREAM ("RealSense Camera - Stopping camera nodelet."); @@ -149,19 +149,19 @@ namespace realsense_camera /* *Private Methods. */ - void RealsenseNodelet::enable_color_stream() + void RealsenseNodelet::enableColorStream() { // Enable streams. 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_); - check_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_); check_error (); + 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; @@ -170,49 +170,51 @@ namespace realsense_camera } } - void RealsenseNodelet::enable_depth_stream() + void RealsenseNodelet::enableDepthStream() { // Enable streams. 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_); - check_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_); check_error (); + 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); } } - void RealsenseNodelet::enable_infrared_stream() + void RealsenseNodelet::enableInfraredStream() { // Enable streams. 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_); - check_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_); check_error (); + 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); } } - void RealsenseNodelet::enable_infrared2_stream() + void RealsenseNodelet::enableInfrared2Stream() { // Enable streams. if (mode_.compare ("manual") == 0) @@ -225,8 +227,10 @@ namespace realsense_camera ROS_INFO_STREAM ("RealSense Camera - Enabling Infrared2 Stream: preset mode"); 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); } } @@ -282,16 +286,25 @@ namespace realsense_camera rs_set_device_options(rs_device_, edge_options_, 4, edge_values_, 0); } - if(config.ENABLE_DEPTH == false) { - enable_depth_ = false; + if(config.ENABLE_DEPTH == false) + { + if(enable_color_ == false) + { + ROS_INFO_STREAM ("RealSense Camera - Color stream is also disabled. Cannot disable depth stream"); + config.ENABLE_DEPTH = true; + } + else + { + enable_depth_ = false; + } } - else { + else + { enable_depth_ = true; } - } - void RealsenseNodelet::check_error () + void RealsenseNodelet::checkError () { if (rs_error_) { @@ -325,7 +338,7 @@ namespace realsense_camera } rs_context_ = rs_create_context (RS_API_VERSION, &rs_error_); - check_error (); + checkError (); int num_of_cameras = rs_get_device_count (rs_context_, NULL); if (num_of_cameras < 1) @@ -335,28 +348,28 @@ namespace realsense_camera } rs_device_ = rs_get_device (rs_context_, 0, &rs_error_); - check_error (); + checkError (); 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_)); - check_error (); + checkError (); ROS_INFO_STREAM ("RealSense Camera - Name: " << rs_get_device_name (rs_device_, &rs_error_)); - check_error (); + checkError (); ROS_INFO_STREAM ("RealSense Camera - Serial no: " << rs_get_device_serial (rs_device_, &rs_error_)); - check_error (); + checkError (); // Enable streams. if (enable_color_ == true) { - enable_color_stream(); + enableColorStream(); } if (enable_depth_ == true) { - enable_depth_stream(); - enable_infrared_stream(); - enable_infrared2_stream(); + enableDepthStream(); + enableInfraredStream(); + enableInfrared2Stream(); } for (int i = 0; i < RS_OPTION_COUNT; ++i) @@ -383,7 +396,7 @@ namespace realsense_camera // Start device. rs_start_device (rs_device_, &rs_error_); - check_error (); + checkError (); is_device_started_ = true; @@ -417,7 +430,7 @@ namespace realsense_camera uint32_t stream_index = (uint32_t) rs_strm; rs_intrinsics intrinsic; rs_get_stream_intrinsics (rs_device_, rs_strm, &intrinsic, &rs_error_); - check_error (); + checkError (); camera_info_[stream_index] = new sensor_msgs::CameraInfo (); camera_info_ptr_[stream_index] = sensor_msgs::CameraInfoPtr (camera_info_[stream_index]); @@ -644,7 +657,7 @@ namespace realsense_camera { ROS_INFO_STREAM ("RealSense Camera - Setting" << opt_name.c_str () << " to " << config_.at (opt_name).c_str ()); rs_set_device_option (rs_device_, o.opt, atoi (config_.at (opt_name).c_str ()), &rs_error_); - check_error (); + checkError (); } cam_options.pop_back (); } @@ -702,22 +715,24 @@ namespace realsense_camera { if(rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); check_error (); + rs_stop_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } - rs_disable_stream(rs_device_, RS_STREAM_DEPTH, &rs_error_); check_error (); + + //disable depth, infrared1 and infrared2 streams + 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_); check_error (); + 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_); check_error (); + 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) { - rs_start_device (rs_device_, &rs_error_);check_error (); + rs_start_device (rs_device_, &rs_error_);checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } @@ -726,17 +741,17 @@ namespace realsense_camera { if(rs_is_device_streaming(rs_device_, 0) == 1) { - rs_stop_device (rs_device_, &rs_error_); check_error (); + rs_stop_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Stopped"); } - enable_depth_stream(); - enable_infrared_stream(); - enable_infrared2_stream(); + enableDepthStream(); + enableInfraredStream(); + enableInfrared2Stream(); if(rs_is_device_streaming(rs_device_, 0) == 0) { - rs_start_device (rs_device_, &rs_error_); check_error (); + rs_start_device (rs_device_, &rs_error_); checkError (); ROS_INFO_STREAM ("RealSense Camera - Device Started"); } } @@ -744,7 +759,7 @@ namespace realsense_camera if(rs_is_device_streaming(rs_device_, 0) == 1) { rs_wait_for_frames (rs_device_, &rs_error_); - check_error (); + checkError (); time_stamp_ = ros::Time::now (); for (int stream_index = 0; stream_index < STREAM_COUNT; ++stream_index) @@ -800,12 +815,12 @@ namespace realsense_camera rs_intrinsics z_intrinsic; rs_get_stream_intrinsics (rs_device_, RS_STREAM_DEPTH, &z_intrinsic, &rs_error_); - check_error (); + checkError (); if (enable_color_ == true) { - rs_get_stream_intrinsics (rs_device_, RS_STREAM_COLOR, &color_intrinsic, &rs_error_); check_error (); - rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); check_error (); + 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. @@ -836,7 +851,7 @@ namespace realsense_camera 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_); - check_error ();// Default value is 0.001 + checkError ();// Default value is 0.001 // Fill the PointCloud2 fields. for (int y = 0; y < z_intrinsic.height; y++) @@ -911,7 +926,7 @@ 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_); check_error (); + 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 645c2433e0..df105b6fbf 100644 --- a/camera/src/realsense_camera_nodelet.h +++ b/camera/src/realsense_camera_nodelet.h @@ -165,11 +165,11 @@ class RealsenseNodelet: public nodelet::Nodelet boost::shared_ptr> dynamic_reconf_server_; // Member Functions. - void enable_color_stream(); - void enable_depth_stream(); - void enable_infrared_stream(); - void enable_infrared2_stream(); - void check_error(); + void enableColorStream(); + void enableDepthStream(); + void enableInfraredStream(); + void enableInfrared2Stream(); + void checkError(); void fetchCalibData(); void prepareStreamCalibData(rs_stream calib_data); void prepareStreamData(rs_stream rs_strm); diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 7938aca0f5..9e92b11a80 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -305,10 +305,14 @@ TEST (RealsenseTests, testIsDepthStreamEnabled) if (enable_depth) { EXPECT_TRUE (depth_recv); + EXPECT_TRUE (infrared1_recv); + EXPECT_TRUE (infrared2_recv); } else { EXPECT_FALSE (depth_recv); + EXPECT_FALSE (infrared1_recv); + EXPECT_FALSE (infrared2_recv); } } @@ -382,7 +386,7 @@ TEST (RealsenseTests, testDepthCameraInfo) TEST (RealsenseTests, testInfrared1Stream) { - if (enable_infrared1) + if (enable_depth) { EXPECT_TRUE (infrared1_avg > 0); EXPECT_TRUE (infrared1_recv); @@ -403,7 +407,7 @@ TEST (RealsenseTests, testInfrared1Stream) TEST (RealsenseTests, testInfrared1Resolution) { - if (enable_infrared1) + if (enable_depth) { if (depth_width_exp > 0) { @@ -418,7 +422,7 @@ TEST (RealsenseTests, testInfrared1Resolution) TEST (RealsenseTests, testInfrared1CameraInfo) { - if (enable_infrared1) + if (enable_depth) { EXPECT_EQ (infrared1_width_recv, inf1_caminfo_width_recv); EXPECT_EQ (infrared1_height_recv, inf1_caminfo_height_recv); @@ -449,7 +453,7 @@ TEST (RealsenseTests, testInfrared1CameraInfo) TEST (RealsenseTests, testInfrared2Stream) { - if (enable_infrared2) + if (enable_depth) { EXPECT_TRUE (infrared2_avg > 0); EXPECT_TRUE (infrared2_recv); @@ -462,7 +466,7 @@ TEST (RealsenseTests, testInfrared2Stream) TEST (RealsenseTests, testInfrared2Resolution) { - if (enable_infrared2) + if (enable_depth) { if (depth_width_exp > 0) { @@ -477,7 +481,7 @@ TEST (RealsenseTests, testInfrared2Resolution) TEST (RealsenseTests, testInfrared2CameraInfo) { - if (enable_infrared2) + if (enable_depth) { EXPECT_EQ (infrared2_width_recv, inf2_caminfo_width_recv); EXPECT_EQ (infrared2_height_recv, inf2_caminfo_height_recv); From b68dd8856bbea815a8a329b18bf506b3e0fb46be Mon Sep 17 00:00:00 2001 From: Rajvi Jingar Date: Mon, 14 Mar 2016 17:07:03 -0500 Subject: [PATCH 12/15] Update realsense_camera_test_node.h removed enable_infrared1 and enable_infrared2 flags --- camera/test/realsense_camera_test_node.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/camera/test/realsense_camera_test_node.h b/camera/test/realsense_camera_test_node.h index 395f196d34..df3cc4049a 100644 --- a/camera/test/realsense_camera_test_node.h +++ b/camera/test/realsense_camera_test_node.h @@ -80,8 +80,6 @@ uint32_t infrared2_step_exp; // Expected infrared2 step. bool enable_color = true; bool enable_depth = true; -bool enable_infrared1 = true; -bool enable_infrared2 = true; bool enable_pointcloud = true; std::string depth_encoding_exp; // Expected depth encoding. From 444fe4c64dc0c26e243a4b55a214a1c4459c2608 Mon Sep 17 00:00:00 2001 From: Rajvi Jingar Date: Tue, 15 Mar 2016 14:31:55 -0500 Subject: [PATCH 13/15] Update camera_params.cfg --- camera/cfg/camera_params.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index 9d60718b21..0dca975f71 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,7 +7,7 @@ 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("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) From 627296d669175c9060545c19a1777be4f2c806f5 Mon Sep 17 00:00:00 2001 From: rjingar Date: Tue, 15 Mar 2016 12:55:10 -0700 Subject: [PATCH 14/15] Fixec cfg formatting --- camera/cfg/camera_params.cfg | 40 ++++++++++++++++++------------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/camera/cfg/camera_params.cfg b/camera/cfg/camera_params.cfg index 0dca975f71..abebab2fe0 100755 --- a/camera/cfg/camera_params.cfg +++ b/camera/cfg/camera_params.cfg @@ -7,25 +7,25 @@ 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", 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("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) exit(gen.generate(PACKAGE, "realsense_camera", "camera_params")) From 233190fec2d52d2d1ccaa82a1bbd1e092fa2d21f Mon Sep 17 00:00:00 2001 From: rjingar Date: Tue, 15 Mar 2016 13:06:50 -0700 Subject: [PATCH 15/15] Fixed testfile testcase duplication after merge --- camera/test/realsense_camera_test_node.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 0695ba7ca5..9e92b11a80 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -316,22 +316,6 @@ TEST (RealsenseTests, testIsDepthStreamEnabled) } } -TEST (RealsenseTests, testIsDepthStreamEnabled) -{ - if (enable_depth) - { - EXPECT_TRUE (depth_recv); - EXPECT_TRUE (infrared1_recv); - EXPECT_TRUE (infrared2_recv); - } - else - { - EXPECT_FALSE (depth_recv); - EXPECT_FALSE (infrared1_recv); - EXPECT_FALSE (infrared2_recv); - } -} - TEST (RealsenseTests, testDepthStream) { if (enable_depth)