From d8180d79469c3d3bdac75010ff5392e4cab9c136 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Fri, 12 May 2023 16:45:59 +0200 Subject: [PATCH] Fix: Avoids crashing if camera can not be found Previously if a camera with the given name could not be found, it would still call a function on it. This prevents this and adds some user-friendly output. --- src/RealSensePlugin.cpp | 57 +++++++++++++++++++++++++++++------------ 1 file changed, 40 insertions(+), 17 deletions(-) diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 53c6e65..b62f3a3 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -28,6 +28,21 @@ using namespace gazebo; +template +std::shared_ptr GetAndCheckSensor(sensors::SensorManager *smanager, std::string name) { + const gazebo::sensors::SensorPtr s = smanager->GetSensor(name); + if (s == NULL) { + std::cerr << "RealSensePlugin: Sensor '" << name << "' not found. Available sensor are:" << std::endl; + const auto sensors = smanager->GetSensors(); + for (const auto& sensor : sensors) { + std::cerr << "\t" << sensor->Name() << std::endl; + } + return NULL; + } + return std::dynamic_pointer_cast(s); +} + + ///////////////////////////////////////////////// RealSensePlugin::RealSensePlugin() { this->depthCam = nullptr; @@ -117,7 +132,7 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { else if (name == "robotNamespace") break; else - throw std::runtime_error("Ivalid parameter for ReakSensePlugin"); + throw std::runtime_error("Invalid parameter for RealSensePlugin"); _sdf = _sdf->GetNextElement(); } while (_sdf); @@ -132,39 +147,47 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { sensors::SensorManager *smanager = sensors::SensorManager::Instance(); // Get Cameras Renderers - this->depthCam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + DEPTH_CAMERA_NAME)) - ->DepthCamera(); - - this->ired1Cam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + IRED1_CAMERA_NAME)) - ->Camera(); - this->ired2Cam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + IRED2_CAMERA_NAME)) - ->Camera(); - this->colorCam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + COLOR_CAMERA_NAME)) - ->Camera(); + sensors::DepthCameraSensorPtr depth_cs = GetAndCheckSensor(smanager, std::string(prefix + DEPTH_CAMERA_NAME)); + if (depth_cs) { + this->depthCam = depth_cs->DepthCamera(); + } + sensors::CameraSensorPtr ired1_cs = GetAndCheckSensor(smanager, std::string(prefix + IRED1_CAMERA_NAME)); + if (ired1_cs) { + this->ired1Cam = ired1_cs->Camera(); + } + sensors::CameraSensorPtr ired2_cs = GetAndCheckSensor(smanager, std::string(prefix + IRED2_CAMERA_NAME)); + if (ired2_cs) { + this->ired2Cam = ired2_cs->Camera(); + } + sensors::CameraSensorPtr color_cs = GetAndCheckSensor(smanager, std::string(prefix + COLOR_CAMERA_NAME)); + if (color_cs) { + this->colorCam = color_cs->Camera(); + } + bool abort{false}; // Check if camera renderers have been found successfuly if (!this->depthCam) { std::cerr << "RealSensePlugin: Depth Camera has not been found" << std::endl; - return; + abort = true; } if (!this->ired1Cam) { std::cerr << "RealSensePlugin: InfraRed Camera 1 has not been found" << std::endl; - return; + abort = true; } if (!this->ired2Cam) { std::cerr << "RealSensePlugin: InfraRed Camera 2 has not been found" << std::endl; - return; + abort = true; } if (!this->colorCam) { std::cerr << "RealSensePlugin: Color Camera has not been found" << std::endl; + abort = true; + } + if (abort) { + std:cerr << "RealSensePlugin: Aborting loading" return; }