i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraRealSense2: refactored for freezing/crash issues on stop

Browse Source
This commit is contained in:
matlabbe
2021-01-18 14:58:33 -05:00
parent 70e9dff7da
commit 5d777469ff
2 changed files with 84 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/include/rtabmap/core/camera/CameraRealSense2.h
View File

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/pcl_config.h> #include <pcl/pcl_config.h>
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_frame.hpp> #include <librealsense2/hpp/rs_frame.hpp>
#endif #endif
@@ -86,6 +87,7 @@ public:
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
private: private:
void close();
void imu_callback(rs2::frame frame); void imu_callback(rs2::frame frame);
void pose_callback(rs2::frame frame); void pose_callback(rs2::frame frame);
void frame_callback(rs2::frame frame); void frame_callback(rs2::frame frame);
@@ -103,14 +105,14 @@ protected:
private: private:
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
rs2::context * ctx_; rs2::context ctx_;
std::vector<rs2::device *> dev_; std::vector<rs2::device> dev_;
std::string deviceId_; std::string deviceId_;
rs2::syncer * syncer_; rs2::syncer syncer_;
float depth_scale_meters_; float depth_scale_meters_;
rs2_intrinsics * depthIntrinsics_; rs2_intrinsics depthIntrinsics_;
rs2_intrinsics * rgbIntrinsics_; rs2_intrinsics rgbIntrinsics_;
rs2_extrinsics * depthToRGBExtrinsics_; rs2_extrinsics depthToRGBExtrinsics_;
cv::Mat depthBuffer_; cv::Mat depthBuffer_;
cv::Mat rgbBuffer_; cv::Mat rgbBuffer_;
CameraModel model_; CameraModel model_;

169
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/types_c.h> #include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h> #include <librealsense2/rsutil.h>
#include <librealsense2/hpp/rs_processing.hpp> #include <librealsense2/hpp/rs_processing.hpp>
#include <librealsense2/rs_advanced_mode.hpp> #include <librealsense2/rs_advanced_mode.hpp>
@@ -59,14 +58,8 @@ CameraRealSense2::CameraRealSense2(
Camera(imageRate, localTransform) Camera(imageRate, localTransform)
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
, ,
ctx_(new rs2::context),
dev_(2, 0),
deviceId_(device), deviceId_(device),
syncer_(new rs2::syncer),
depth_scale_meters_(1.0f), depth_scale_meters_(1.0f),
depthIntrinsics_(new rs2_intrinsics),
rgbIntrinsics_(new rs2_intrinsics),
depthToRGBExtrinsics_(new rs2_extrinsics),
lastImuStamp_(0.0), lastImuStamp_(0.0),
clockSyncWarningShown_(false), clockSyncWarningShown_(false),
imuGlobalSyncWarningShown_(false), imuGlobalSyncWarningShown_(false),
@@ -93,63 +86,51 @@ CameraRealSense2::CameraRealSense2(
CameraRealSense2::~CameraRealSense2() CameraRealSense2::~CameraRealSense2()
{ {
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
close();
#endif
}
#ifdef RTABMAP_REALSENSE2
void CameraRealSense2::close()
{
closing_ = true; closing_ = true;
try try
{ {
UDEBUG("Closing device(s)..."); UDEBUG("Closing device(s)...");
for(size_t i=0; i<dev_.size(); ++i) for(size_t i=0; i<dev_.size(); ++i)
{ {
if(dev_[i]) UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i].query_sensors().size(), (int)i);
for(rs2::sensor _sensor : dev_[i].query_sensors())
{ {
UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i]->query_sensors().size(), (int)i); if(!_sensor.get_active_streams().empty())
for(rs2::sensor _sensor : dev_[i]->query_sensors())
{ {
if(!_sensor.get_active_streams().empty()) try
{ {
try _sensor.stop();
{ _sensor.close();
_sensor.stop(); }
_sensor.close(); catch(const rs2::error & error)
} {
catch(const rs2::error & error) UWARN("%s", error.what());
{
UWARN("%s", error.what());
}
} }
} }
#ifdef WIN32
dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
// Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted
#endif
delete dev_[i];
} }
#ifdef WIN32
dev_[i].hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
// Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted
#endif
} }
UDEBUG("Clearing devices...");
dev_.clear();
} }
catch(const rs2::error & error) catch(const rs2::error & error)
{ {
UINFO("%s", error.what()); UINFO("%s", error.what());
} }
try {
delete ctx_; closing_ = false;
}
catch(const rs2::error & error)
{
UWARN("%s", error.what());
}
try {
delete syncer_;
}
catch(const rs2::error & error)
{
UWARN("%s", error.what());
}
delete depthIntrinsics_;
delete rgbIntrinsics_;
delete depthToRGBExtrinsics_;
#endif
} }
#ifdef RTABMAP_REALSENSE2
void CameraRealSense2::imu_callback(rs2::frame frame) void CameraRealSense2::imu_callback(rs2::frame frame)
{ {
auto stream = frame.get_profile().stream_type(); auto stream = frame.get_profile().stream_type();
@@ -211,10 +192,14 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
void CameraRealSense2::frame_callback(rs2::frame frame) void CameraRealSense2::frame_callback(rs2::frame frame)
{ {
UDEBUG("Frame callback! %f", frame.get_timestamp()); UDEBUG("Frame callback! %f", frame.get_timestamp());
(*syncer_)(frame); syncer_(frame);
} }
void CameraRealSense2::multiple_message_callback(rs2::frame frame) void CameraRealSense2::multiple_message_callback(rs2::frame frame)
{ {
if(closing_)
{
return;
}
auto stream = frame.get_profile().stream_type(); auto stream = frame.get_profile().stream_type();
switch (stream) switch (stream)
{ {
@@ -500,15 +485,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
UINFO("setupDevice..."); UINFO("setupDevice...");
for(size_t i=0; i<dev_.size(); ++i) close();
{
delete dev_[i];
dev_[i] = 0;
}
clockSyncWarningShown_ = false; clockSyncWarningShown_ = false;
imuGlobalSyncWarningShown_ = false; imuGlobalSyncWarningShown_ = false;
auto list = ctx_->query_devices(); rs2::device_list list = ctx_.query_devices();
if (0 == list.size()) if (0 == list.size())
{ {
UERROR("No RealSense2 devices were found!"); UERROR("No RealSense2 devices were found!");
@@ -516,7 +498,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
bool found=false; bool found=false;
for (auto&& dev : list) for (rs2::device dev : list)
{ {
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
@@ -529,24 +511,26 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{ {
// Dual setup: device[0] = D400, device[1] = T265 // Dual setup: device[0] = D400, device[1] = T265
// T265 // T265
dev_[1] = new rs2::device(); dev_.resize(2);
*dev_[1] = dev; dev_[1] = dev;
} }
else if (!found && (deviceId_.empty() || deviceId_ == sn)) else if (!found && (deviceId_.empty() || deviceId_ == sn))
{ {
dev_[0] = new rs2::device(); if(dev_.empty())
*dev_[0] = dev; {
dev_.resize(1);
}
dev_[0] = dev;
found=true; found=true;
} }
} }
if (!found) if (!found)
{ {
if(dualMode_ && dev_[1]!=0) if(dualMode_ && dev_.size()==2)
{ {
UERROR("Dual setup is enabled, but a D400 camera is not detected!"); UERROR("Dual setup is enabled, but a D400 camera is not detected!");
delete dev_[1]; dev_.clear();
dev_[1] = 0;
} }
else else
{ {
@@ -554,17 +538,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
return false; return false;
} }
else if(dualMode_ && dev_[1] == 0) else if(dualMode_ && dev_.size()!=2)
{ {
UERROR("Dual setup is enabled, but a T265 camera is not detected!"); UERROR("Dual setup is enabled, but a T265 camera is not detected!");
delete dev_[0]; dev_.clear();
dev_[0] = 0;
return false; return false;
} }
UASSERT(!dev_.empty());
if (!jsonConfig_.empty()) if (!jsonConfig_.empty())
{ {
if (dev_[0]->is<rs400::advanced_mode>()) if (dev_[0].is<rs400::advanced_mode>())
{ {
std::stringstream ss; std::stringstream ss;
std::ifstream in(jsonConfig_); std::ifstream in(jsonConfig_);
@@ -573,7 +558,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
ss << in.rdbuf(); ss << in.rdbuf();
std::string json_file_content = ss.str(); std::string json_file_content = ss.str();
auto adv = dev_[0]->as<rs400::advanced_mode>(); auto adv = dev_[0].as<rs400::advanced_mode>();
adv.load_json(json_file_content); adv.load_json(json_file_content);
UINFO("JSON file is loaded! (%s)", jsonConfig_.c_str()); UINFO("JSON file is loaded! (%s)", jsonConfig_.c_str());
} }
@@ -588,44 +573,42 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
} }
ctx_->set_devices_changed_callback([this](rs2::event_information& info) ctx_.set_devices_changed_callback([this](rs2::event_information& info)
{ {
for(size_t i=0; i<dev_.size(); ++i) for(size_t i=0; i<dev_.size(); ++i)
{ {
if(dev_[i]) if (info.was_removed(dev_[i]))
{ {
if (info.was_removed(*dev_[i])) if (closing_)
{ {
if (closing_) UDEBUG("The device %d has been disconnected!", i);
{ }
UDEBUG("The device %d has been disconnected!", i); else
} {
else UERROR("The device %d has been disconnected!", i);
{
UERROR("The device %d has been disconnected!", i);
}
} }
} }
} }
}); });
auto camera_name = dev_[0]->get_info(RS2_CAMERA_INFO_NAME); auto camera_name = dev_[0].get_info(RS2_CAMERA_INFO_NAME);
UINFO("Device Name: %s", camera_name); UINFO("Device Name: %s", camera_name);
auto sn = dev_[0]->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); auto sn = dev_[0].get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
UINFO("Device Serial No: %s", sn); UINFO("Device Serial No: %s", sn);
auto fw_ver = dev_[0]->get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION); auto fw_ver = dev_[0].get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
UINFO("Device FW version: %s", fw_ver); UINFO("Device FW version: %s", fw_ver);
auto pid = dev_[0]->get_info(RS2_CAMERA_INFO_PRODUCT_ID); auto pid = dev_[0].get_info(RS2_CAMERA_INFO_PRODUCT_ID);
UINFO("Device Product ID: 0x%s", pid); UINFO("Device Product ID: 0x%s", pid);
auto dev_sensors = dev_[0]->query_sensors(); auto dev_sensors = dev_[0].query_sensors();
if(dualMode_) if(dualMode_)
{ {
auto dev_sensors2 = dev_[1]->query_sensors(); UASSERT(dev_.size()>1);
auto dev_sensors2 = dev_[1].query_sensors();
dev_sensors.insert(dev_sensors.end(), dev_sensors2.begin(), dev_sensors2.end()); dev_sensors.insert(dev_sensors.end(), dev_sensors2.begin(), dev_sensors2.end());
} }
@@ -766,7 +749,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
intrinsic.model, intrinsic.model,
intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]); intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
rgbStreamProfile = profile; rgbStreamProfile = profile;
*rgbIntrinsics_ = intrinsic; rgbIntrinsics_ = intrinsic;
added = true; added = true;
if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2) if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
{ {
@@ -780,7 +763,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
profilesPerSensor[i].push_back(profile); profilesPerSensor[i].push_back(profile);
depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0)); depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
depthStreamProfile = profile; depthStreamProfile = profile;
*depthIntrinsics_ = intrinsic; depthIntrinsics_ = intrinsic;
added = true; added = true;
if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2) if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
{ {
@@ -840,14 +823,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
// LEFT FISHEYE // LEFT FISHEYE
rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
rgbStreamProfile = profile; rgbStreamProfile = profile;
*rgbIntrinsics_ = intrinsic; rgbIntrinsics_ = intrinsic;
} }
else else
{ {
// RIGHT FISHEYE // RIGHT FISHEYE
depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
depthStreamProfile = profile; depthStreamProfile = profile;
*depthIntrinsics_ = intrinsic; depthIntrinsics_ = intrinsic;
} }
added = true; added = true;
} }
@@ -891,7 +874,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
std::cout<< model_ << std::endl; std::cout<< model_ << std::endl;
return false; return false;
} }
*depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile); depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
if(dualMode_) if(dualMode_)
{ {
@@ -903,9 +886,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
if(!ir_) if(!ir_)
{ {
Transform leftIRToRGB( Transform leftIRToRGB(
depthToRGBExtrinsics_->rotation[0], depthToRGBExtrinsics_->rotation[1], depthToRGBExtrinsics_->rotation[2], depthToRGBExtrinsics_->translation[0], depthToRGBExtrinsics_.rotation[0], depthToRGBExtrinsics_.rotation[1], depthToRGBExtrinsics_.rotation[2], depthToRGBExtrinsics_.translation[0],
depthToRGBExtrinsics_->rotation[3], depthToRGBExtrinsics_->rotation[4], depthToRGBExtrinsics_->rotation[5], depthToRGBExtrinsics_->translation[1], depthToRGBExtrinsics_.rotation[3], depthToRGBExtrinsics_.rotation[4], depthToRGBExtrinsics_.rotation[5], depthToRGBExtrinsics_.translation[1],
depthToRGBExtrinsics_->rotation[6], depthToRGBExtrinsics_->rotation[7], depthToRGBExtrinsics_->rotation[8], depthToRGBExtrinsics_->translation[2]); depthToRGBExtrinsics_.rotation[6], depthToRGBExtrinsics_.rotation[7], depthToRGBExtrinsics_.rotation[8], depthToRGBExtrinsics_.translation[2]);
leftIRToRGB = leftIRToRGB.inverse(); leftIRToRGB = leftIRToRGB.inverse();
UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str()); UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str());
baseToCam *= leftIRToRGB; baseToCam *= leftIRToRGB;
@@ -1106,9 +1089,9 @@ bool CameraRealSense2::isCalibrated() const
std::string CameraRealSense2::getSerial() const std::string CameraRealSense2::getSerial() const
{ {
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
if(dev_[0]) if(!dev_.empty())
{ {
return dev_[0]->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); return dev_[0].get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
} }
#endif #endif
return "NA"; return "NA";
@@ -1215,13 +1198,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
try{ try{
auto frameset = syncer_->wait_for_frames(5000); auto frameset = syncer_.wait_for_frames(5000);
UTimer timer; UTimer timer;
int desiredFramesetSize = 2; int desiredFramesetSize = 2;
while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0) while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0)
{ {
// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds) // maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
frameset = syncer_->wait_for_frames(100); frameset = syncer_.wait_for_frames(100);
} }
if ((int)frameset.size() == desiredFramesetSize) if ((int)frameset.size() == desiredFramesetSize)
{ {