diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index d0017b03..40e1de04 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -136,8 +136,8 @@ void Stereo::setupRectQueue(std::shared_ptr device, auto offset = static_cast(ph->getParam(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset")); conv->addExposureOffset(offset); } - im = std::make_shared( - getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorName).get(), "/rect"); + im = std::make_shared(getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorName).get(), + "/rect"); if(ph->getParam("i_reverse_stereo_socket_order")) { conv->reverseStereoSocketOrder(); } @@ -336,12 +336,15 @@ void Stereo::closeQueues() { if(ph->getParam("i_publish_topic")) { stereoQ->close(); } - if(ph->getParam("i_publish_left_rect") || ph->getParam("i_publish_synced_rect_pair")) { - syncTimer->reset(); + if(ph->getParam("i_publish_left_rect")) { leftRectQ->close(); } - if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { - syncTimer->reset(); + if(ph->getParam("i_publish_right_rect")) { + rightRectQ->close(); + } + if(ph->getParam("i_publish_synced_rect_pair")) { + syncTimer->cancel(); + leftRectQ->close(); rightRectQ->close(); } if(ph->getParam("i_left_rect_enable_feature_tracker")) { diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index c7231614..d6362822 100644 --- a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -64,8 +64,9 @@ std::string SysLogger::sysInfoToString(const dai::SystemInformation& sysInfo) { void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) { try { - auto logData = loggerQ->tryGet(); - if(logData) { + bool timeout; + auto logData = loggerQ->get(std::chrono::seconds(5), timeout); + if(!timeout) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information"); stat.add("System Information", sysInfoToString(*logData)); } else { diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index ba533270..0109fec1 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -70,7 +70,7 @@ void SensorParamHandler::declareParams(std::shared_ptr mo monoCam->setIsp3aFps(declareAndLogParam("i_isp3a_fps", 10)); } monoCam->setImageOrientation( - utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "NORMAL"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); + utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); } void SensorParamHandler::declareParams(std::shared_ptr colorCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish) { declareAndLogParam("i_publish_topic", publish); @@ -158,7 +158,7 @@ void SensorParamHandler::declareParams(std::shared_ptr c colorCam->setIsp3aFps(declareAndLogParam("i_isp3a_fps", 10)); } colorCam->setImageOrientation( - utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "NORMAL"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); + utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); } dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector& params) { dai::CameraControl ctrl;