Reduce Coverity Scan Issues - Step 1

realsense2_camera/include/base_realsense_node.h

@@ -331,7 +331,6 @@ namespace realsense2_camera
         std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _depth_aligned_info_publisher;
         std::map<stream_index_pair, std::shared_ptr<image_publisher>> _depth_aligned_image_publishers;
         std::map<std::string, rs2::region_of_interest> _auto_exposure_roi;
-        std::map<rs2_stream, bool> _is_first_frame;
 
         std::shared_ptr<std::thread> _monitoring_t;
         std::shared_ptr<std::thread> _monitoring_pc;   //pc = profile changes

realsense2_camera/include/named_filter.h

@@ -60,7 +60,6 @@ namespace realsense2_camera
             void setParameters();
 
         private:
-            bool _is_enabled_pc;
             rclcpp::Node& _node;
             bool _allow_no_texture_points;
             bool _ordered_pc;

realsense2_camera/include/profile_manager.h

@@ -82,9 +82,8 @@ namespace realsense2_camera
         private:
             std::string _module_name;
             std::map<stream_index_pair, rs2_format>  _formats;
-            int      _fps;
+            int _fps;
             int _width, _height;
-            bool _is_profile_exist;
             bool _force_image_default_qos;
     };
 

realsense2_camera/include/ros_sensor.h

@@ -86,7 +86,6 @@ namespace realsense2_camera
             ~RosSensor();
             void registerSensorParameters();
             bool getUpdatedProfiles(std::vector<rs2::stream_profile>& wanted_profiles);
-            void runFirstFrameInitialization();
             virtual bool start(const std::vector<rs2::stream_profile>& profiles);
             void stop();
             rmw_qos_profile_t getQOS(const stream_index_pair& sip) const;
@@ -114,8 +113,6 @@ namespace realsense2_camera
             std::function<void(rs2::frame)> _frame_callback;
             SensorParams _params;
             std::function<void()> _update_sensor_func, _hardware_reset_func;
-            bool _is_first_frame;
-            std::vector<std::function<void()> > _first_frame_functions_stack;
             std::vector<std::shared_ptr<ProfilesManager> > _profile_managers;
             rs2::region_of_interest _auto_exposure_roi;
             std::vector<std::string> _parameters_names;

realsense2_camera/src/base_realsense_node.cpp

@@ -74,10 +74,18 @@ void SyncedImuPublisher::PublishPendingMessages()
     while (!_pending_messages.empty())
     {
         const sensor_msgs::msg::Imu &imu_msg = _pending_messages.front();
-        _publisher->publish(imu_msg);
+        try
+        {
+            _publisher->publish(imu_msg);
+        }
+        catch(const std::exception& e)
+        {
+            std::cerr << e.what() << '\n';
+        }
         _pending_messages.pop();
     }
 }
+
 size_t SyncedImuPublisher::getNumSubscribers()
 { 
     if (!_publisher) return 0;
@@ -146,7 +154,14 @@ BaseRealSenseNode::~BaseRealSenseNode()
     clearParameters();
     for(auto&& sensor : _available_ros_sensors)
     {
-        sensor->stop();
+        try
+        {
+            sensor->stop();
+        }
+        catch(const std::exception& e)
+        {
+            std::cerr << e.what() << '\n';
+        }
     }
 }
 
@@ -537,7 +552,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
         rs2::video_frame original_color_frame = frameset.get_color_frame();
 
         ROS_DEBUG("num_filters: %d", static_cast<int>(_filters.size()));
-        for (auto filter_it : _filters)
+        for (auto& filter_it : _filters)
         {
             frameset = filter_it->Process(frameset);
         }

realsense2_camera/src/dynamic_params.cpp

@@ -124,11 +124,11 @@ namespace realsense2_camera
         catch(const std::exception& e)
         {
             std::stringstream range;
-            for (auto val : descriptor.floating_point_range)
+            for (auto& val : descriptor.floating_point_range)
             {
                 range << val.from_value << ", " << val.to_value;
             }
-            for (auto val : descriptor.integer_range)
+            for (auto& val : descriptor.integer_range)
             {
                 range << val.from_value << ", " << val.to_value;
             }

realsense2_camera/src/parameters.cpp

@@ -115,7 +115,7 @@ void BaseRealSenseNode::setDynamicParams()
     }
     sort(enum_vec.begin(), enum_vec.end(), [](std::pair<std::string, int> e1, std::pair<std::string, int> e2){return (e1.second < e2.second);});
     std::stringstream enum_str_values;
-    for (auto vec_iter : enum_vec)
+    for (auto& vec_iter : enum_vec)
     {
         enum_str_values << std::setw(longest_desc+6) << std::left << vec_iter.first << " : " << vec_iter.second << std::endl;
     }

realsense2_camera/src/profile_manager.cpp

@@ -37,9 +37,7 @@ void ProfilesManager::clearParameters()
 std::string applyTemplateName(std::string template_name, stream_index_pair sip)
 {
     const std::string stream_name(create_graph_resource_name(STREAM_NAME(sip)));
-    char* param_name = new char[template_name.size() + stream_name.size()];
-    sprintf(param_name, template_name.c_str(), stream_name.c_str());
-    return std::string(param_name);
+    return template_name + stream_name;
 }
 
 void ProfilesManager::registerSensorQOSParam(std::string template_name, 
@@ -120,7 +118,7 @@ std::map<stream_index_pair, rs2::stream_profile> ProfilesManager::getDefaultProf
     if (_all_profiles.empty()) 
         throw std::runtime_error("Wrong commands sequence. No profiles set.");
 
-    for (auto profile : _all_profiles)
+    for (auto& profile : _all_profiles)
     {
         stream_index_pair sip(profile.stream_type(), profile.stream_index());
         if (profile.is_default() && (sip_default_profiles.find(sip) == sip_default_profiles.end()))
@@ -142,7 +140,7 @@ std::map<stream_index_pair, rs2::stream_profile> ProfilesManager::getDefaultProf
 void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& wanted_profiles)
 {    
     std::map<stream_index_pair, bool> found_sips;
-    for (auto profile : _all_profiles)
+    for (auto const & profile : _all_profiles)
     {
         stream_index_pair sip(profile.stream_type(), profile.stream_index());
         if (!(*_enabled_profiles[sip])) continue;
@@ -219,6 +217,9 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr<Parameters> parameter
                                            const std::string& module_name, rclcpp::Logger logger, bool force_image_default_qos):
     ProfilesManager(parameters, logger),
     _module_name(module_name),
+    _fps(0),
+    _width(0),
+    _height(0),
     _force_image_default_qos(force_image_default_qos)
 {
 
@@ -354,13 +355,13 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
     _fps = video_profile.fps();
 
     // Set the stream formats
-    for (auto sip : sips)
+    for (auto& sip : sips)
     {
         registerVideoSensorProfileFormat(sip);
     }
 
     // Overwrite the _formats with default values queried from LibRealsense
-    for (auto sip_default_profile : sip_default_profiles)
+    for (auto& sip_default_profile : sip_default_profiles)
     {
         stream_index_pair sip = sip_default_profile.first;
 
@@ -431,7 +432,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
             }, crnt_descriptor);
     _parameters_names.push_back(param_name);
 
-    for (auto sip : sips)
+    for (auto& sip : sips)
     {
         std::string param_name(_module_name + "." + STREAM_NAME(sip) + "_format");
         std::string param_value = rs2_format_to_string(_formats[sip]);
@@ -528,7 +529,7 @@ void MotionProfilesManager::registerFPSParams()
 
     // Overwrite with default values:
     std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();
-    for (auto sip_default_profile : sip_default_profiles)
+    for (auto& sip_default_profile : sip_default_profiles)
     {
         stream_index_pair sip = sip_default_profile.first;
         *(_fps[sip]) = sip_default_profile.second.as<rs2::motion_stream_profile>().fps();

realsense2_camera/src/ros_sensor.cpp

@@ -57,7 +57,6 @@ RosSensor::RosSensor(rs2::sensor sensor,
 {
     _frame_callback = [this](rs2::frame frame)
         {
-            runFirstFrameInitialization();
             auto stream_type = frame.get_profile().stream_type();
             auto stream_index = frame.get_profile().stream_index();
             stream_index_pair sip{stream_type, stream_index};
@@ -204,41 +203,6 @@ void RosSensor::registerSensorParameters()
     }
 }
 
-void RosSensor::runFirstFrameInitialization()
-{
-    if (_is_first_frame)
-    {
-        ROS_DEBUG_STREAM("runFirstFrameInitialization: " << _first_frame_functions_stack.size());
-        _is_first_frame = false;
-        if (!_first_frame_functions_stack.empty())
-        {
-            std::thread t = std::thread([=]()
-            {
-                try
-                {
-                    while (!_first_frame_functions_stack.empty())
-                    {
-                        _first_frame_functions_stack.back()();
-                        _first_frame_functions_stack.pop_back();
-                    }
-                }
-                catch(const std::exception& e)
-                {
-                    std::cerr << "runFirstFrameInitialization(): " << e.what() << '\n';
-                    throw e;
-                }
-                catch(...)
-                {
-                    std::cerr << "runFirstFrameInitialization()!!!" << std::endl;
-                    throw;
-                }                
-
-            });
-            t.detach();
-        }
-    }
-}
-
 bool RosSensor::start(const std::vector<stream_profile>& profiles)
 {
     if (get_active_streams().size() > 0)
@@ -262,13 +226,12 @@ bool RosSensor::start(const std::vector<stream_profile>& profiles)
 
 void RosSensor::stop()
 {
-    if (get_active_streams().size() == 0)
-        return;
-    ROS_INFO_STREAM("Stop Sensor: " << rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
-    _frequency_diagnostics.clear();
-
     try
     {
+        if (get_active_streams().size() == 0)
+            return;
+        ROS_INFO_STREAM("Stop Sensor: " << rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
+        _frequency_diagnostics.clear();
         rs2::sensor::stop();
     }
     catch (const std::exception& e)
@@ -352,7 +315,7 @@ bool RosSensor::getUpdatedProfiles(std::vector<stream_profile>& wanted_profiles)
 {
     wanted_profiles.clear();
     std::vector<stream_profile> active_profiles = get_active_streams();
-    for (auto profile_manager : _profile_managers)
+    for (auto& profile_manager : _profile_managers)
     {
         profile_manager->addWantedProfiles(wanted_profiles);        
     }
@@ -441,7 +404,7 @@ void RosSensor::registerAutoExposureROIOptions()
 
 void RosSensor::clearParameters()
 {
-    for (auto profile_manager : _profile_managers)
+    for (auto& profile_manager : _profile_managers)
     {
         profile_manager->clearParameters();
     }

realsense2_camera/src/sensor_params.cpp

@@ -277,14 +277,14 @@ void SensorParams::registerDynamicOptions(rs2::options sensor, const std::string
         {
             std::vector<std::pair<std::string, int> > enum_vec;
             size_t longest_desc(0);
-            for (auto enum_iter : enum_dict)
+            for (auto& enum_iter : enum_dict)
             {
                 enum_vec.push_back(std::make_pair(enum_iter.first, enum_iter.second));
                 longest_desc = std::max(longest_desc, enum_iter.first.size());
             }
             sort(enum_vec.begin(), enum_vec.end(), [](std::pair<std::string, int> e1, std::pair<std::string, int> e2){return (e1.second < e2.second);});
             std::stringstream description;
-            for (auto vec_iter : enum_vec)
+            for (auto& vec_iter : enum_vec)
             {
                 description << std::setw(longest_desc+6) << std::left << vec_iter.first << " : " << vec_iter.second << std::endl;
             }

realsense2_camera/test/gtest_template.cpp

@@ -27,7 +27,14 @@ TEST(realsense2_camera, test2)
 
 int main(int argc, char** argv)
 {
-    testing::InitGoogleTest(&argc, argv);
+    try
+    {
+        testing::InitGoogleTest(&argc, argv);
+    }
+    catch(const std::exception& e)
+    {
+        std::cerr << e.what() << '\n';
+    }
     return RUN_ALL_TESTS();
 }