From d4dec914371be30c37953d6733563f999ff98df4 Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 16:15:14 -0800 Subject: [PATCH 1/8] Prevent double freeing of error data Need to set the error point to NULL to ensure we do not attempt to free a second time in the destructor. This would occur if we were able to create a valid rs_context and then later encounter a realsense runtime error like failing to get an extrinsic. --- realsense_camera/src/base_nodelet.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index d286910c09..17ae604334 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -1040,6 +1040,7 @@ namespace realsense_camera ROS_ERROR_STREAM(nodelet_name_ << " - Error calling " << rs_get_failed_function(rs_error_) << " ( " << rs_get_failed_args(rs_error_) << " ): \n" << rs_get_error_message(rs_error_) << " \n"); rs_free_error(rs_error_); + rs_error_ = NULL; ros::shutdown(); exit (EXIT_FAILURE); } From 013d85bbed7336761147d2cd758a65f9148c8b11 Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 16:41:07 -0800 Subject: [PATCH 2/8] Prevent possible double free of rs_context Make sure the rs_context is not freed twice; a second time in the destructor. --- .../include/realsense_camera/base_nodelet.h | 4 ++-- realsense_camera/src/base_nodelet.cpp | 12 ++++++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/realsense_camera/include/realsense_camera/base_nodelet.h b/realsense_camera/include/realsense_camera/base_nodelet.h index 9e7a962dd0..6c6a3f5c4f 100644 --- a/realsense_camera/include/realsense_camera/base_nodelet.h +++ b/realsense_camera/include/realsense_camera/base_nodelet.h @@ -100,8 +100,8 @@ namespace realsense_camera ros::ServiceServer force_power_service_; ros::ServiceServer is_powered_service_; tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; - rs_error *rs_error_ = 0; - rs_context *rs_context_; + rs_error *rs_error_ = NULL; + rs_context *rs_context_ = NULL; rs_device *rs_device_; std::string nodelet_name_; std::string serial_no_; diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index 17ae604334..f670c13a0e 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -45,8 +45,12 @@ namespace realsense_camera stopCamera(); - rs_delete_context(rs_context_, &rs_error_); - checkError(); + if (rs_context_) + { + rs_delete_context(rs_context_, &rs_error_); + rs_context_ = NULL; + checkError(); + } ROS_INFO_STREAM(nodelet_name_ << " - Stopping..."); ros::shutdown(); @@ -141,6 +145,7 @@ namespace realsense_camera { ROS_ERROR_STREAM(nodelet_name_ << " - No cameras detected!"); rs_delete_context(rs_context_, &rs_error_); + rs_context_ = NULL; checkError(); return false; } @@ -153,6 +158,7 @@ namespace realsense_camera { ROS_ERROR_STREAM(nodelet_name_ << " - No '" << camera_type_ << "' cameras detected!"); rs_delete_context(rs_context_, &rs_error_); + rs_context_ = NULL; checkError(); return false; } @@ -163,6 +169,7 @@ namespace realsense_camera ROS_ERROR_STREAM(nodelet_name_ << " - Multiple cameras of same type detected but no input serial_no or usb_port_id specified"); rs_delete_context(rs_context_, &rs_error_); + rs_context_ = NULL; checkError(); return false; } @@ -197,6 +204,7 @@ namespace realsense_camera ROS_ERROR_STREAM(nodelet_name_ << error_msg); rs_delete_context(rs_context_, &rs_error_); + rs_context_ = NULL; checkError(); return false; } From a566c9d77313d9484975cd95e9e7d7a082524150 Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 16:43:00 -0800 Subject: [PATCH 3/8] Remove unneeded exit() call The code should be allow ros::shutdown() to properly stop the node and call the destructors. --- realsense_camera/src/base_nodelet.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index f670c13a0e..8e98921553 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -1050,7 +1050,6 @@ namespace realsense_camera rs_free_error(rs_error_); rs_error_ = NULL; ros::shutdown(); - exit (EXIT_FAILURE); } } } // end namespace From 4696f7d773aeec576106d3124a5e9b6a5e04fb6e Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 17:06:16 -0800 Subject: [PATCH 4/8] Clearer error message for uvcmodule not loaded Display a 'no camera detected' message when the uvcvideo module is not being loaded. This is generally due to no camera every being connected since last boot. --- realsense_camera/src/base_nodelet.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index 8e98921553..a5cdfb5b18 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -135,6 +135,10 @@ namespace realsense_camera bool BaseNodelet::connectToCamera() { rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - No cameras detected!"); + } checkError(); int num_of_cameras = rs_get_device_count(rs_context_, &rs_error_); From b6b03bef8240a8727a772a27dea5e41258ce920a Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 17:43:48 -0800 Subject: [PATCH 5/8] Improve error messages In general, attribute intrinsic failures to firmware, and attribute eextrinsics failures to calibration issues. --- realsense_camera/src/base_nodelet.cpp | 16 ++++++++++++++++ realsense_camera/src/r200_nodelet.cpp | 4 ++++ realsense_camera/src/zr300_nodelet.cpp | 16 ++++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index a5cdfb5b18..8c6b454cb6 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -581,6 +581,10 @@ namespace realsense_camera { rs_intrinsics intrinsic; rs_get_stream_intrinsics(rs_device_, stream_index, &intrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera firmware version and/or calibration data!"); + } checkError(); sensor_msgs::CameraInfo * camera_info = new sensor_msgs::CameraInfo(); @@ -616,6 +620,10 @@ 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_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); camera_info->P.at(3) = z_extrinsic.translation[0]; // Tx camera_info->P.at(7) = z_extrinsic.translation[1]; // Ty @@ -981,6 +989,10 @@ namespace realsense_camera // Get offset between base frame and depth frame rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); // Transform base frame to depth frame @@ -1012,6 +1024,10 @@ namespace realsense_camera // Get offset between base frame and infrared frame rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); // Transform base frame to infrared frame diff --git a/realsense_camera/src/r200_nodelet.cpp b/realsense_camera/src/r200_nodelet.cpp index f5c4a7e563..57ecfcf365 100644 --- a/realsense_camera/src/r200_nodelet.cpp +++ b/realsense_camera/src/r200_nodelet.cpp @@ -489,6 +489,10 @@ namespace realsense_camera // Get offset between base frame and infrared2 frame rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED2, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); // Transform base frame to infrared2 frame diff --git a/realsense_camera/src/zr300_nodelet.cpp b/realsense_camera/src/zr300_nodelet.cpp index 92bde4d2d5..b51eaca574 100644 --- a/realsense_camera/src/zr300_nodelet.cpp +++ b/realsense_camera/src/zr300_nodelet.cpp @@ -134,6 +134,10 @@ namespace realsense_camera rs_motion_intrinsics imu_intrinsics; rs_get_motion_intrinsics(rs_device_, &imu_intrinsics, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera firmware version!"); + } checkError(); int index = 0; @@ -700,6 +704,10 @@ namespace realsense_camera // Get offset between base frame and infrared2 frame rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED2, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); // Transform base frame to infrared2 frame @@ -731,6 +739,10 @@ namespace realsense_camera // Get offset between base frame and fisheye frame rs_get_device_extrinsics(rs_device_, RS_STREAM_FISHEYE, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); // Transform base frame to fisheye frame @@ -765,6 +777,10 @@ namespace realsense_camera /* Temporarily commenting out the code. Instead, hardcoding the values until fully supported by librealsense API. rs_get_motion_extrinsics_from(rs_device_, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); + if (rs_error_) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + } checkError(); */ z_extrinsic.translation[0] = -0.07; From be7ba9724e517d07ba27a5a8feadd1fad807e4d8 Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Thu, 17 Nov 2016 18:09:40 -0800 Subject: [PATCH 6/8] Added exception handling Attempt to catch any exceptions and handle more gracefully. --- realsense_camera/src/base_nodelet.cpp | 38 +++++++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index 8c6b454cb6..cdd751e514 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -59,7 +59,7 @@ namespace realsense_camera /* * Initialize the nodelet. */ - void BaseNodelet::onInit() + void BaseNodelet::onInit() try { getParameters(); @@ -96,6 +96,23 @@ namespace realsense_camera // Start dynamic reconfigure callback startDynamicReconfCallback(); } + catch(const rs::error & e) + { + ROS_ERROR_STREAM(nodelet_name_ << " - " << "RealSense error calling " + << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " + << e.what()); + ros::shutdown(); + } + catch(const std::exception & e) + { + ROS_ERROR_STREAM(nodelet_name_ << " - " << e.what()); + ros::shutdown(); + } + catch(...) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Caught unknown expection...shutting down!"); + ros::shutdown(); + } /* * Get the nodelet parameters. @@ -723,7 +740,7 @@ namespace realsense_camera /* * Prepare and publish topics. */ - void BaseNodelet::prepareTopics() + void BaseNodelet::prepareTopics() try { while (ros::ok()) { @@ -774,6 +791,23 @@ namespace realsense_camera } } } + catch(const rs::error & e) + { + ROS_ERROR_STREAM(nodelet_name_ << " - " << "RealSense error calling " + << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " + << e.what()); + ros::shutdown(); + } + catch(const std::exception & e) + { + ROS_ERROR_STREAM(nodelet_name_ << " - " << e.what()); + ros::shutdown(); + } + catch(...) + { + ROS_ERROR_STREAM(nodelet_name_ << " - Caught unknown expection...shutting down!"); + ros::shutdown(); + } /* * Call publicTopic() for all streams. From d190dce1f7e0852e0fd87e1da34fc5ef27a80e4a Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Fri, 18 Nov 2016 13:27:31 -0800 Subject: [PATCH 7/8] Check status of shutdown before calling --- realsense_camera/src/base_nodelet.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense_camera/src/base_nodelet.cpp b/realsense_camera/src/base_nodelet.cpp index cdd751e514..3bdd8de9d2 100644 --- a/realsense_camera/src/base_nodelet.cpp +++ b/realsense_camera/src/base_nodelet.cpp @@ -53,7 +53,10 @@ namespace realsense_camera } ROS_INFO_STREAM(nodelet_name_ << " - Stopping..."); - ros::shutdown(); + if (! ros::isShuttingDown()) + { + ros::shutdown(); + } } /* From 1a063236d81ff462d29a516a17a4b7a38c063e13 Mon Sep 17 00:00:00 2001 From: Mark D Horn Date: Fri, 18 Nov 2016 14:08:12 -0800 Subject: [PATCH 8/8] Display warning for hardcoded extrinsic Display a warning message about the use of the hardcoded extrinsic value for the color camera on ZR300 until librealsense supports it. --- realsense_camera/src/zr300_nodelet.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/realsense_camera/src/zr300_nodelet.cpp b/realsense_camera/src/zr300_nodelet.cpp index b51eaca574..74279b50c4 100644 --- a/realsense_camera/src/zr300_nodelet.cpp +++ b/realsense_camera/src/zr300_nodelet.cpp @@ -774,18 +774,21 @@ namespace realsense_camera // Get offset between base frame and imu frame -/* Temporarily commenting out the code. Instead, hardcoding the values until fully supported by librealsense API. rs_get_motion_extrinsics_from(rs_device_, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); if (rs_error_) { - ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); +/* Temporarily hardcoding the values until fully supported by librealsense API. */ + // ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!"); + ROS_WARN_STREAM(nodelet_name_ << " - Using Hardcoded extrinsic for IMU."); + rs_free_error(rs_error_); + rs_error_ = NULL; + + z_extrinsic.translation[0] = -0.07; + z_extrinsic.translation[1] = 0.0; + z_extrinsic.translation[2] = 0.0; } - checkError(); -*/ - z_extrinsic.translation[0] = -0.07; - z_extrinsic.translation[1] = 0.0; - z_extrinsic.translation[2] = 0.0; + //checkError(); // Transform base frame to imu frame b2imu_msg.header.stamp = static_transform_ts_;