diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0202e4e70b..617acce5a9 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -34,7 +34,11 @@ SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher: SyncedImuPublisher::~SyncedImuPublisher() { - PublishPendingMessages(); + try + { + PublishPendingMessages(); + } + catch(...){} // Not allowed to throw from Dtor } void SyncedImuPublisher::Publish(sensor_msgs::msg::Imu imu_msg) @@ -149,10 +153,14 @@ BaseRealSenseNode::~BaseRealSenseNode() _monitoring_pc->join(); } clearParameters(); - for(auto&& sensor : _available_ros_sensors) + try { - sensor->stop(); + for(auto&& sensor : _available_ros_sensors) + { + sensor->stop(); + } } + catch(...){} // Not allowed to throw from Dtor } void BaseRealSenseNode::hardwareResetRequest() @@ -643,7 +651,11 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain) { - ROS_WARN_ONCE(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME ? "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)" : ""); + if (time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME) + { + ROS_WARN_ONCE("Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)"); + } + if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 95dadbbad6..25b11d110e 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -39,7 +39,9 @@ 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); + std::string full_name(param_name); + delete [] param_name; + return full_name; } void ProfilesManager::registerSensorQOSParam(std::string template_name, diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 35a4ef8ca3..0a26f85937 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -78,8 +78,12 @@ RosSensor::RosSensor(rs2::sensor sensor, RosSensor::~RosSensor() { - clearParameters(); - stop(); + try + { + clearParameters(); + stop(); + } + catch(...){} // Not allowed to throw from Dtor } void RosSensor::setParameters(bool is_rosbag_file)