From 3df30815281655c217deef8612c692df9af785d3 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Fri, 21 Sep 2018 19:05:39 +0200 Subject: [PATCH 01/41] First somewhat working version. Refactored out timesync into header-only library mavros_timestamp. Adjusted realsense node accordingly, and added configuration parameters for sync mode (hardware config) and mavros timestamping --- .../include/base_realsense_node.h | 16 +- realsense2_camera/include/constants.h | 2 + realsense2_camera/include/mavros_timestamp.h | 281 ++++++++++++++++++ .../launch/includes/nodelet.launch.xml | 4 + realsense2_camera/package.xml | 3 + realsense2_camera/src/base_realsense_node.cpp | 129 +++++++- 6 files changed, 420 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/include/mavros_timestamp.h diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index a3831d546b..e916289303 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -3,6 +3,7 @@ #pragma once +#include "../include/mavros_timestamp.h" #include "../include/realsense_node_factory.h" #include #include @@ -54,10 +55,10 @@ namespace realsense2_camera class PipelineSyncer : public rs2::asynchronous_syncer { public: - void operator()(rs2::frame f) const - { - invoke(std::move(f)); - } + void operator()(rs2::frame f) const + { + invoke(std::move(f)); + } }; class BaseRealSenseNode : public InterfaceRealSenseNode @@ -162,6 +163,13 @@ namespace realsense2_camera std::map _camera_info; bool _intialize_time_base; double _camera_time_base; + int _inter_cam_sync_mode; + bool _force_mavros_triggering; + typedef struct{ + sensor_msgs::ImagePtr img; + sensor_msgs::CameraInfo info; + }cache_type; + mavros_trigger::MavrosTrigger _trigger; std::map> _enabled_profiles; ros::Publisher _pointcloud_publisher; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 3b98ac4d68..82c325cd70 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -35,6 +35,7 @@ namespace realsense2_camera const bool ALIGN_DEPTH = false; const bool POINTCLOUD = false; const bool SYNC_FRAMES = false; + const bool FORCE_MAVROS_TRIGGERING = false; const int DEPTH_WIDTH = 640; const int DEPTH_HEIGHT = 480; @@ -68,6 +69,7 @@ namespace realsense2_camera const bool ENABLE_FISHEYE = true; const bool ENABLE_IMU = true; + const std::string INTER_CAM_SYNC_MODE = "none"; const std::string DEFAULT_BASE_FRAME_ID = "camera_link"; const std::string DEFAULT_DEPTH_FRAME_ID = "camera_depth_frame"; diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h new file mode 100644 index 0000000000..4a81109009 --- /dev/null +++ b/realsense2_camera/include/mavros_timestamp.h @@ -0,0 +1,281 @@ +// +// Created by mpantic on 19.09.18. +// + +#ifndef REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H +#define REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +namespace mavros_trigger +{ + +template +class MavrosTrigger{ + typedef boost::function& cal)> caching_callback; + + typedef struct { + uint32_t seq; + ros::Time old_stamp; + std::shared_ptr frame; + double exposure; + } cache_queue_type; + + public: + MavrosTrigger(const std::set& channel_set) : + channel_set_(channel_set){ + ROS_WARN_STREAM("CHANNEL SIZE" << channel_set_.size()); + for(t_chanel_id id : channel_set_){ + sequence_time_map_[id].clear(); + ROS_WARN("CHANNEL loop"); + + logging_name_[id] = "A"; + } + } + + caching_callback callback; + + void setup(){ + // Set these for now... + first_image_ = false; + trigger_sequence_offset_ = 0; + triggering_started_ = false; + + cam_imu_sub_ = + nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, + &MavrosTrigger::camImuStampCallback, this); + } + + void start() { + if(triggering_started_){ + //already started, ignore + return; + } + start_lock.lock(); + ROS_WARN("TRIGG"); + // First subscribe to the messages so we don't miss any.' + + for(t_chanel_id id : channel_set_){ + ROS_WARN("CHANNEL INIT"); + + //sequence_time_map_[id].clear(); + ROS_WARN("CHANNEL INIT 2"); + + } + ROS_WARN("CHANNEL INIT 3"); + + trigger_sequence_offset_ = 0; + + const std::string mavros_trigger_service = "/mavros/cmd/trigger_control"; + if (ros::service::exists(mavros_trigger_service, false)) { + mavros_msgs::CommandTriggerControl req; + req.request.trigger_enable = true; + // This is NOT integration time, this is actually the sequence reset. + req.request.integration_time = 1.0; + + ros::service::call(mavros_trigger_service, req); + + ROS_INFO("Called mavros trigger service! Success? %d Result? %d", + req.response.success, req.response.result); + } else { + ROS_WARN("Mavros service not available!"); + } + + first_image_ = true; + triggering_started_ = true; + start_lock.unlock(); + ROS_WARN("CHANNEL INIT done"); + + } + + bool channelValid(const t_chanel_id& channel){ + return channel_set_.count(channel) == 1; + } + + void cacheFrame(const t_chanel_id& channel, const uint32_t seq, const ros::Time& original_stamp, double exposure, + const std::shared_ptr frame){ + if(!channelValid(channel)){ + ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for unsynchronized channel."); + return; + } + + if(cache_queue_[channel].frame) { + ROS_WARN_STREAM_THROTTLE(60, log_prefix_ << " Overwriting image queue! Make sure you're getting " + "Timestamps from mavros. This message will only print " + "once a minute."); + } + cache_queue_[channel].frame = frame; + cache_queue_[channel].old_stamp = original_stamp; + cache_queue_[channel].seq = seq; + cache_queue_[channel].exposure = exposure; + + ROS_WARN_STREAM(log_prefix_ << "Cached frame w seq " << seq); + } + + bool lookupSequenceStamp(const t_chanel_id& channel, const uint32_t seq, const ros::Time& old_stamp, double exposure, ros::Time* new_stamp, + bool from_image_queue = false ) { + if(!channelValid(channel)){ + ROS_WARN_STREAM_ONCE(log_prefix_ << "lookupSequenceStamp called for unsynchronized channel."); + return false; + } + + if (sequence_time_map_[channel].empty()) { + ROS_DEBUG_STREAM(log_prefix_ << " Sequence time map empty"); + return false; + } + + // Handle case of first image where offset is not determined yet + if (first_image_) { + + // Get the first from the sequence time map. + auto it = sequence_time_map_[channel].begin(); + int32_t mavros_sequence = it->first; + + // Get offset between first frame sequence and mavros + trigger_sequence_offset_ = + mavros_sequence - static_cast(seq); + + ROS_INFO( + "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " + "correction: %f seconds.", + log_prefix_, channel, + trigger_sequence_offset_, it->first, seq, + it->second.toSec() - old_stamp.toSec()); + + *new_stamp = it->second; + *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); + + first_image_ = false; + sequence_time_map_[channel].erase(it); + return true; + } + + // Search correct sequence number and adjust + auto it = sequence_time_map_[channel].find(seq + trigger_sequence_offset_); + if (it == sequence_time_map_[channel].end()) { + ROS_WARN_STREAM("Sequence not found " << seq+trigger_sequence_offset_ << " original seq# " << seq); + + if(seqsecond.toSec()); + + // Check for divergence (too large delay) + const double kMinExpectedDelay = 0.0; + const double kMaxExpectedDelay = 34.0 * 1e-3; + + double delay = old_stamp.toSec() - it->second.toSec(); + if (delay < kMinExpectedDelay || delay > kMaxExpectedDelay) { + ROS_ERROR( + "%s Delay out of bounds! Actual delay: %f s, min: " + "%f s max: %f s. Resetting triggering on next image.", + log_prefix_, + delay, kMinExpectedDelay, kMaxExpectedDelay); + triggering_started_ = false; + first_image_ = true; + } + + *new_stamp = it->second; + *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); + sequence_time_map_[channel].erase(it); + + return true; + } + + ros::Time shiftTimestampToMidExposure(const ros::Time& stamp, + double exposure_us) { + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0); + return new_stamp; + } + + + void camImuStampCallback(const mavros_msgs::CamIMUStamp& cam_imu_stamp) { + start_lock.lock(); + if (!triggering_started_) { + // Ignore stuff from before we *officially* start the triggering. + // The triggering is techncially always running but... + start_lock.unlock(); + return; + } + + //ignore messages until they wrap back - we should receive a 0 eventually. + if(first_image_ && cam_imu_stamp.frame_seq_id != 0){ + ROS_WARN("[Cam Imu Sync] Ignoring old messages"); + start_lock.unlock(); + return; + + } + + // insert stamps into sequence map for all channels + // each channel can use each time stamp once. + for(auto channel : channel_set_){ + sequence_time_map_[channel][cam_imu_stamp.frame_seq_id] = cam_imu_stamp.frame_stamp; + } + + ROS_INFO( + "[Cam Imu Sync] Received a new stamp for sequence number: %ld with " + "stamp: %f", + cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_stamp.toSec()); + + + // release potentially cached frames + constexpr bool kFromImageQueue = true; + + for(auto channel : channel_set_) { + + if (!cache_queue_[channel].frame) { + continue; + } + + ros::Time new_stamp; + + // even if we do have to discard cached image due to a missing callback fct, + // we want them to pass through the lookup logic, in order to not miss any sequence offsets etc. + if (lookupSequenceStamp(channel, cache_queue_[channel].seq, + cache_queue_[channel].old_stamp, + cache_queue_[channel].exposure, + &new_stamp, kFromImageQueue)) { + if (callback) { + callback(channel, new_stamp, cache_queue_[channel].frame); + } else { + ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " No callback set - discarding cached images."); + } + cache_queue_[channel].frame.reset(); + } + } + + start_lock.unlock(); + } + + private: + ros::NodeHandle nh_; + bool first_image_; + bool triggering_started_; + int trigger_sequence_offset_ = 0; + + const std::set channel_set_; + ros::Subscriber cam_imu_sub_; + std::map> sequence_time_map_; + std::mutex start_lock; + + std::map logging_name_; + std::map cache_queue_; + + const std::string log_prefix_ = "[Mavros Triggering] "; +}; + + +} + +#endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 0121aca3a6..f15ae533a9 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -43,6 +43,8 @@ + + @@ -53,6 +55,8 @@ + + diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index e55cfd8ad4..3de1011b62 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -24,6 +24,8 @@ tf dynamic_reconfigure diagnostic_updater + mavros_msgs + image_transport cv_bridge nodelet @@ -35,6 +37,7 @@ tf dynamic_reconfigure diagnostic_updater + mavros_msgs diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index e697cde38b..0e5c48bad2 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -18,7 +18,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _pnh(privateNodeHandle), _json_file_path(""), _serial_no(serial_no), _base_frame_id(""), _intialize_time_base(false), - _namespace(getNamespaceStr()) + _namespace(getNamespaceStr()), + _trigger(std::set({INFRA1})) { // Types for depth stream _is_frame_arrived[DEPTH] = false; @@ -100,6 +101,39 @@ void BaseRealSenseNode::getParameters() { ROS_INFO("getParameters..."); + std::string inter_cam_sync_mode_param; + _pnh.param("inter_cam_sync_mode", inter_cam_sync_mode_param, INTER_CAM_SYNC_MODE); + std::transform(inter_cam_sync_mode_param.begin(), inter_cam_sync_mode_param.end(), + inter_cam_sync_mode_param.begin(), ::tolower); + + // note: added a "none" mode, as not all sensor types/firmware versions allow setting of the sync mode. + // Use "none" if nothing is specified or an error occurs. + // Default (mode = 0) here refers to the default sync mode as per Intel whitepaper, + // which corresponds to master mode but no trigger output on Pin 5. + // Master (mode = 1) activates trigger signal output on Pin 5. + // Slave (mode = 2) causes the realsense to listen to a trigger signal on pin 5. + + if(inter_cam_sync_mode_param == "default"){ _inter_cam_sync_mode = 0; } + else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = 1; } + else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = 2; } + else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = -1; } + else { + _inter_cam_sync_mode = -1; + ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); + } + + _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); + if(_force_mavros_triggering && _inter_cam_sync_mode != 2){ + ROS_WARN_STREAM("Force mavros triggering enabled but device not set to slave triggering mode!"); + } + + // set up mavros trigger if enabled + if(_force_mavros_triggering){ + _trigger.setup(); + //ros::Duration(1.0).sleep(); + } + + _pnh.param("align_depth", _align_depth, ALIGN_DEPTH); _pnh.param("enable_pointcloud", _pointcloud, POINTCLOUD); _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES); @@ -264,6 +298,13 @@ void BaseRealSenseNode::setupDevice() } } } + + // set cam sync mode + if(_inter_cam_sync_mode != -1) + { + _sensors[DEPTH].set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, _inter_cam_sync_mode); + ROS_INFO_STREAM("Inter cam sync mode set to " << _inter_cam_sync_mode); + } } catch(const std::exception& ex) { @@ -532,10 +573,12 @@ void BaseRealSenseNode::enable_devices() void BaseRealSenseNode::setupStreams() { + ROS_INFO("setupStreams..."); enable_devices(); + try{ - // Publish image stream info + // Publish image stream info for (auto& profiles : _enabled_profiles) { for (auto& profile : profiles.second) @@ -547,7 +590,36 @@ void BaseRealSenseNode::setupStreams() auto frame_callback = [this](rs2::frame frame) { - try{ + if(_force_mavros_triggering && frame.get_profile().stream_type() == RS2_STREAM_DEPTH) { + ros::spinOnce(); + + // create callback for cachecd images + std::function&)> f1 = [this](const stream_index_pair& channel, const ros::Time& new_stamp, const std::shared_ptr& cal){ + // fix stamps + cal->img->header.stamp = new_stamp; + cal->info.header.stamp = new_stamp; + + //publish + + auto& info_publisher = this->_info_publisher.at(channel); + auto& image_publisher = this->_image_publishers.at(channel); + info_publisher.publish(cal->info); + + image_publisher.first.publish(cal->img); + image_publisher.second->update(); + + + }; + _trigger.callback = f1; + + _trigger.start(); + ros::spinOnce(); + } + + ros::spinOnce(); + + try{ + // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, // and the incremental timestamp from the camera. // In sync mode the timestamp is based on ROS time @@ -563,15 +635,21 @@ void BaseRealSenseNode::setupStreams() ros::Time t; if (_sync_frames) + { t = ros::Time::now(); + } else - t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000); + { + t = ros::Time(_ros_time_base.toSec() + (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) + / /*ms to seconds*/ 1000); + } + std::map is_frame_arrived(_is_frame_arrived); std::vector frames; if (frame.is()) { - ROS_DEBUG("Frameset arrived."); + ROS_INFO("Frameset arrived."); bool is_depth_arrived = false; rs2::frame depth_frame; auto frameset = frame.as(); @@ -582,7 +660,7 @@ void BaseRealSenseNode::setupStreams() auto stream_index = f.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_DEBUG("Frameset contain (%s, %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + ROS_INFO("Frameset contain (%s, %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); stream_index_pair sip{stream_type,stream_index}; @@ -615,10 +693,11 @@ void BaseRealSenseNode::setupStreams() auto stream_type = frame.get_profile().stream_type(); auto stream_index = frame.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + ROS_INFO("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); stream_index_pair sip{stream_type,stream_index}; + publishFrame(frame, t, sip, _image, @@ -1215,11 +1294,17 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, if (copy_data_from_frame) image.data = (uint8_t*)f.get_data(); + + + + + + ++(seq[stream]); auto& info_publisher = info_publishers.at(stream); auto& image_publisher = image_publishers.at(stream); - if(0 != info_publisher.getNumSubscribers() || - 0 != image_publisher.first.getNumSubscribers()) + /* if(0 != info_publisher.getNumSubscribers() || + 0 != image_publisher.first.getNumSubscribers())*/ { auto width = 0; auto height = 0; @@ -1242,9 +1327,31 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, img->header.stamp = t; img->header.seq = seq[stream]; + auto& cam_info = camera_info.at(stream); - cam_info.header.stamp = t; - cam_info.header.seq = seq[stream]; + cam_info.header.stamp = img->header.stamp; + cam_info.header.seq = img->header.seq; + + // correct timestamp if needed + ros::Time hw_synced_stamp; + if(_force_mavros_triggering) { + + double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? + static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; + if(!_trigger.lookupSequenceStamp(stream, _seq[stream], t, exposure, &hw_synced_stamp)){ + auto cache = std::make_shared(); + + cache->img = img; + cache->info = cam_info; + _trigger.cacheFrame(stream, _seq[stream], t, exposure, cache); + return; + }else{ + img->header.stamp = hw_synced_stamp; + cam_info.header.stamp =hw_synced_stamp; + } + + } + info_publisher.publish(cam_info); image_publisher.first.publish(img); From 2a0d0a0fa069d7d333335a7b69fb1f4675d49f0c Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Fri, 21 Sep 2018 19:45:02 +0200 Subject: [PATCH 02/41] Minor comments / fixes --- realsense2_camera/src/base_realsense_node.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0e5c48bad2..39bc211f85 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -590,25 +590,27 @@ void BaseRealSenseNode::setupStreams() auto frame_callback = [this](rs2::frame frame) { - if(_force_mavros_triggering && frame.get_profile().stream_type() == RS2_STREAM_DEPTH) { + //todo(mpantic): find it where this code belongs ideally (during or before first callback?) + if(_force_mavros_triggering) { ros::spinOnce(); - // create callback for cachecd images - std::function&)> f1 = [this](const stream_index_pair& channel, const ros::Time& new_stamp, const std::shared_ptr& cal){ + // create callback for cached images + std::function&)> f1 = [this](const stream_index_pair& channel, + const ros::Time& new_stamp, + const std::shared_ptr& cal){ // fix stamps cal->img->header.stamp = new_stamp; cal->info.header.stamp = new_stamp; //publish - auto& info_publisher = this->_info_publisher.at(channel); auto& image_publisher = this->_image_publishers.at(channel); info_publisher.publish(cal->info); image_publisher.first.publish(cal->img); image_publisher.second->update(); - - }; _trigger.callback = f1; @@ -1294,12 +1296,6 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, if (copy_data_from_frame) image.data = (uint8_t*)f.get_data(); - - - - - - ++(seq[stream]); auto& info_publisher = info_publishers.at(stream); auto& image_publisher = image_publishers.at(stream); @@ -1336,13 +1332,20 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, ros::Time hw_synced_stamp; if(_force_mavros_triggering) { + // todo(mpantic): investigate why exposure is not available in the beginning. double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; + + // allow clearing of IMU queue before we lookup the timestamp + ros::spinOnce(); + if(!_trigger.lookupSequenceStamp(stream, _seq[stream], t, exposure, &hw_synced_stamp)){ - auto cache = std::make_shared(); + auto cache = std::make_shared(); cache->img = img; cache->info = cam_info; + + // cache frame and return if timestamp not available _trigger.cacheFrame(stream, _seq[stream], t, exposure, cache); return; }else{ From 716118f21a1c0e5992d27fff494f42a4a0c5399b Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Fri, 21 Sep 2018 19:53:49 +0200 Subject: [PATCH 03/41] Removed some warning comments used for debugging --- realsense2_camera/include/mavros_timestamp.h | 22 ++++++++------------ 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 4a81109009..a37c17c237 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -17,6 +17,7 @@ namespace mavros_trigger template class MavrosTrigger{ + typedef boost::function& cal)> caching_callback; typedef struct { @@ -29,12 +30,10 @@ class MavrosTrigger{ public: MavrosTrigger(const std::set& channel_set) : channel_set_(channel_set){ - ROS_WARN_STREAM("CHANNEL SIZE" << channel_set_.size()); + + ROS_WARN_STREAM("CHANNEL NUMBER" << channel_set_.size()); for(t_chanel_id id : channel_set_){ sequence_time_map_[id].clear(); - ROS_WARN("CHANNEL loop"); - - logging_name_[id] = "A"; } } @@ -61,13 +60,8 @@ class MavrosTrigger{ // First subscribe to the messages so we don't miss any.' for(t_chanel_id id : channel_set_){ - ROS_WARN("CHANNEL INIT"); - - //sequence_time_map_[id].clear(); - ROS_WARN("CHANNEL INIT 2"); - + sequence_time_map_[id].clear(); } - ROS_WARN("CHANNEL INIT 3"); trigger_sequence_offset_ = 0; @@ -83,14 +77,12 @@ class MavrosTrigger{ ROS_INFO("Called mavros trigger service! Success? %d Result? %d", req.response.success, req.response.result); } else { - ROS_WARN("Mavros service not available!"); + ROS_ERROR("Mavros service not available!"); } first_image_ = true; triggering_started_ = true; start_lock.unlock(); - ROS_WARN("CHANNEL INIT done"); - } bool channelValid(const t_chanel_id& channel){ @@ -99,6 +91,7 @@ class MavrosTrigger{ void cacheFrame(const t_chanel_id& channel, const uint32_t seq, const ros::Time& original_stamp, double exposure, const std::shared_ptr frame){ + if(!channelValid(channel)){ ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for unsynchronized channel."); return; @@ -109,6 +102,7 @@ class MavrosTrigger{ "Timestamps from mavros. This message will only print " "once a minute."); } + cache_queue_[channel].frame = frame; cache_queue_[channel].old_stamp = original_stamp; cache_queue_[channel].seq = seq; @@ -119,6 +113,7 @@ class MavrosTrigger{ bool lookupSequenceStamp(const t_chanel_id& channel, const uint32_t seq, const ros::Time& old_stamp, double exposure, ros::Time* new_stamp, bool from_image_queue = false ) { + if(!channelValid(channel)){ ROS_WARN_STREAM_ONCE(log_prefix_ << "lookupSequenceStamp called for unsynchronized channel."); return false; @@ -164,6 +159,7 @@ class MavrosTrigger{ ROS_WARN_STREAM("REMOVED CACHE"); return true; } + return false; } From 8e0fbc7cce9d6753bcdcc63fc2057d6af3577da3 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Thu, 27 Sep 2018 19:51:32 +0200 Subject: [PATCH 04/41] Ugly hacks to fix race conditions on startup --- realsense2_camera/include/mavros_timestamp.h | 79 ++++++++++++++----- realsense2_camera/src/base_realsense_node.cpp | 6 +- 2 files changed, 63 insertions(+), 22 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index a37c17c237..8dde40f0b0 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,7 @@ class MavrosTrigger{ caching_callback callback; void setup(){ + // Set these for now... first_image_ = false; trigger_sequence_offset_ = 0; @@ -48,14 +50,17 @@ class MavrosTrigger{ cam_imu_sub_ = nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); + + delay_pub_ = nh_.advertise("delay", 100); } void start() { + std::lock_guard lg(start_lock); // This now locked your mutex + if(triggering_started_){ //already started, ignore return; } - start_lock.lock(); ROS_WARN("TRIGG"); // First subscribe to the messages so we don't miss any.' @@ -82,7 +87,7 @@ class MavrosTrigger{ first_image_ = true; triggering_started_ = true; - start_lock.unlock(); + ROS_WARN("TRIGG END"); } bool channelValid(const t_chanel_id& channel){ @@ -91,6 +96,11 @@ class MavrosTrigger{ void cacheFrame(const t_chanel_id& channel, const uint32_t seq, const ros::Time& original_stamp, double exposure, const std::shared_ptr frame){ + std::lock_guard lg(start_lock); // This now locked your mutex + + if(first_image_){ + return; + } if(!channelValid(channel)){ ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for unsynchronized channel."); @@ -98,9 +108,9 @@ class MavrosTrigger{ } if(cache_queue_[channel].frame) { - ROS_WARN_STREAM_THROTTLE(60, log_prefix_ << " Overwriting image queue! Make sure you're getting " + ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " Overwriting image queue! Make sure you're getting " "Timestamps from mavros. This message will only print " - "once a minute."); + "every 10 sec."); } cache_queue_[channel].frame = frame; @@ -111,11 +121,29 @@ class MavrosTrigger{ ROS_WARN_STREAM(log_prefix_ << "Cached frame w seq " << seq); } - bool lookupSequenceStamp(const t_chanel_id& channel, const uint32_t seq, const ros::Time& old_stamp, double exposure, ros::Time* new_stamp, + void reset(){ + + first_image_ = true; + triggering_started_ = false; + + for(auto channel : channel_set_){ + sequence_time_map_[channel].clear(); + //if(cache_queue_[channel].frame) cache_queue_[channel].frame.reset(); + } + + + } + + + bool lookupSequenceStamp(const t_chanel_id& channel, const uint32_t seq, + const ros::Time& old_stamp, double exposure, ros::Time* new_stamp, bool from_image_queue = false ) { + std::lock_guard lg(start_lock); // This now locked your mutex + if(!channelValid(channel)){ ROS_WARN_STREAM_ONCE(log_prefix_ << "lookupSequenceStamp called for unsynchronized channel."); + return false; } @@ -125,7 +153,7 @@ class MavrosTrigger{ } // Handle case of first image where offset is not determined yet - if (first_image_) { + if (first_image_ && !from_image_queue) { // Get the first from the sequence time map. auto it = sequence_time_map_[channel].begin(); @@ -135,6 +163,7 @@ class MavrosTrigger{ trigger_sequence_offset_ = mavros_sequence - static_cast(seq); + ROS_INFO( "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " "correction: %f seconds.", @@ -142,8 +171,12 @@ class MavrosTrigger{ trigger_sequence_offset_, it->first, seq, it->second.toSec() - old_stamp.toSec()); - *new_stamp = it->second; - *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); + if( (it->second.toSec() - old_stamp.toSec()) >0.0) { + ROS_ERROR("Ignoring negative offset"); + + return false; + } + first_image_ = false; sequence_time_map_[channel].erase(it); @@ -157,29 +190,33 @@ class MavrosTrigger{ if(seqsecond.toSec(); + delay_pub_.publish(delay); + ROS_INFO("[Mavros Triggering] Remapped seq %d to %d, %f to %f: %f", seq, seq + trigger_sequence_offset_, old_stamp.toSec(), - it->second.toSec()); + it->second.toSec(), delay); // Check for divergence (too large delay) const double kMinExpectedDelay = 0.0; const double kMaxExpectedDelay = 34.0 * 1e-3; - double delay = old_stamp.toSec() - it->second.toSec(); - if (delay < kMinExpectedDelay || delay > kMaxExpectedDelay) { + + if ( (delay < kMinExpectedDelay || delay > kMaxExpectedDelay)) { ROS_ERROR( "%s Delay out of bounds! Actual delay: %f s, min: " "%f s max: %f s. Resetting triggering on next image.", log_prefix_, delay, kMinExpectedDelay, kMaxExpectedDelay); - triggering_started_ = false; - first_image_ = true; + reset(); + return true; } *new_stamp = it->second; @@ -191,7 +228,8 @@ class MavrosTrigger{ ros::Time shiftTimestampToMidExposure(const ros::Time& stamp, double exposure_us) { - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0); + ROS_WARN_STREAM(".5 Exposure =" << exposure_us*1e-6/2.0); + ros::Time new_stamp = stamp ;//+ ros::Duration(exposure_us * 1e-6 / 2.0); (debug)c return new_stamp; } @@ -220,11 +258,12 @@ class MavrosTrigger{ } ROS_INFO( - "[Cam Imu Sync] Received a new stamp for sequence number: %ld with " + "[Cam Imu Sync] Received a new stamp for sequence number: %ld (%ld) with " "stamp: %f", - cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_stamp.toSec()); + cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, cam_imu_stamp.frame_stamp.toSec()); + start_lock.unlock(); // release potentially cached frames constexpr bool kFromImageQueue = true; @@ -243,6 +282,8 @@ class MavrosTrigger{ cache_queue_[channel].exposure, &new_stamp, kFromImageQueue)) { if (callback) { + ROS_WARN_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); + callback(channel, new_stamp, cache_queue_[channel].frame); } else { ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " No callback set - discarding cached images."); @@ -250,8 +291,7 @@ class MavrosTrigger{ cache_queue_[channel].frame.reset(); } } - - start_lock.unlock(); + ROS_INFO("DONE"); } private: @@ -262,6 +302,7 @@ class MavrosTrigger{ const std::set channel_set_; ros::Subscriber cam_imu_sub_; + ros::Publisher delay_pub_; std::map> sequence_time_map_; std::mutex start_lock; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 39bc211f85..5bf1542ea5 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1321,7 +1321,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, img->step = width * bpp; img->header.frame_id = optical_frame_id.at(stream); img->header.stamp = t; - img->header.seq = seq[stream]; + img->header.seq = f.get_frame_number(); auto& cam_info = camera_info.at(stream); @@ -1339,14 +1339,14 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, // allow clearing of IMU queue before we lookup the timestamp ros::spinOnce(); - if(!_trigger.lookupSequenceStamp(stream, _seq[stream], t, exposure, &hw_synced_stamp)){ + if(!_trigger.lookupSequenceStamp(stream, cam_info.header.seq, t, exposure, &hw_synced_stamp)){ auto cache = std::make_shared(); cache->img = img; cache->info = cam_info; // cache frame and return if timestamp not available - _trigger.cacheFrame(stream, _seq[stream], t, exposure, cache); + _trigger.cacheFrame(stream, cam_info.header.seq, t, exposure, cache); return; }else{ img->header.stamp = hw_synced_stamp; From 8e816f07054bf263497fe70010f5508109bbb397 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Thu, 27 Sep 2018 20:06:10 +0200 Subject: [PATCH 05/41] Enum instead of multiple bools for internal state --- realsense2_camera/include/mavros_timestamp.h | 48 ++++++++++---------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 8dde40f0b0..9ac3445b57 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -15,6 +15,13 @@ namespace mavros_trigger { +enum trigger_state{ + ts_synced = 1, + ts_not_initalized, + ts_wait_for_sync, + ts_disabled +}; + template class MavrosTrigger{ @@ -30,7 +37,8 @@ class MavrosTrigger{ public: MavrosTrigger(const std::set& channel_set) : - channel_set_(channel_set){ + channel_set_(channel_set), + state_(ts_not_initalized){ ROS_WARN_STREAM("CHANNEL NUMBER" << channel_set_.size()); for(t_chanel_id id : channel_set_){ @@ -43,24 +51,24 @@ class MavrosTrigger{ void setup(){ // Set these for now... - first_image_ = false; trigger_sequence_offset_ = 0; - triggering_started_ = false; - cam_imu_sub_ = nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); delay_pub_ = nh_.advertise("delay", 100); + + state_ = ts_not_initalized; } void start() { std::lock_guard lg(start_lock); // This now locked your mutex - if(triggering_started_){ + if(state_ != ts_not_initalized){ //already started, ignore return; } + ROS_WARN("TRIGG"); // First subscribe to the messages so we don't miss any.' @@ -81,13 +89,11 @@ class MavrosTrigger{ ROS_INFO("Called mavros trigger service! Success? %d Result? %d", req.response.success, req.response.result); + state_ = ts_wait_for_sync; } else { ROS_ERROR("Mavros service not available!"); + state_ = ts_disabled; } - - first_image_ = true; - triggering_started_ = true; - ROS_WARN("TRIGG END"); } bool channelValid(const t_chanel_id& channel){ @@ -98,7 +104,7 @@ class MavrosTrigger{ const std::shared_ptr frame){ std::lock_guard lg(start_lock); // This now locked your mutex - if(first_image_){ + if(state_ != ts_synced){ return; } @@ -122,10 +128,7 @@ class MavrosTrigger{ } void reset(){ - - first_image_ = true; - triggering_started_ = false; - + state_ = ts_not_initalized; for(auto channel : channel_set_){ sequence_time_map_[channel].clear(); //if(cache_queue_[channel].frame) cache_queue_[channel].frame.reset(); @@ -153,7 +156,7 @@ class MavrosTrigger{ } // Handle case of first image where offset is not determined yet - if (first_image_ && !from_image_queue) { + if (state_ == ts_wait_for_sync && !from_image_queue) { // Get the first from the sequence time map. auto it = sequence_time_map_[channel].begin(); @@ -173,13 +176,11 @@ class MavrosTrigger{ if( (it->second.toSec() - old_stamp.toSec()) >0.0) { ROS_ERROR("Ignoring negative offset"); - return false; } - - first_image_ = false; sequence_time_map_[channel].erase(it); + state_ = ts_synced; return true; } @@ -236,7 +237,7 @@ class MavrosTrigger{ void camImuStampCallback(const mavros_msgs::CamIMUStamp& cam_imu_stamp) { start_lock.lock(); - if (!triggering_started_) { + if (state_ == ts_not_initalized) { // Ignore stuff from before we *officially* start the triggering. // The triggering is techncially always running but... start_lock.unlock(); @@ -244,11 +245,10 @@ class MavrosTrigger{ } //ignore messages until they wrap back - we should receive a 0 eventually. - if(first_image_ && cam_imu_stamp.frame_seq_id != 0){ + if(state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0){ ROS_WARN("[Cam Imu Sync] Ignoring old messages"); start_lock.unlock(); return; - } // insert stamps into sequence map for all channels @@ -296,8 +296,10 @@ class MavrosTrigger{ private: ros::NodeHandle nh_; - bool first_image_; - bool triggering_started_; + + + trigger_state state_; + int trigger_sequence_offset_ = 0; const std::set channel_set_; From 88f527a6e30fc8c185494d71dae915638e6a0480 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Mon, 1 Oct 2018 18:00:03 +0200 Subject: [PATCH 06/41] Bugfixes and hacks to make it work properly --- realsense2_camera/include/mavros_timestamp.h | 32 ++++++++++---- realsense2_camera/src/base_realsense_node.cpp | 42 ++++++++++--------- 2 files changed, 46 insertions(+), 28 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 9ac3445b57..ccaafb16ef 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include @@ -46,9 +46,9 @@ class MavrosTrigger{ } } - caching_callback callback; + caching_callback callback_; - void setup(){ + void setup(const caching_callback& callback){ // Set these for now... trigger_sequence_offset_ = 0; @@ -56,9 +56,12 @@ class MavrosTrigger{ nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); - delay_pub_ = nh_.advertise("delay", 100); + delay_pub_ = nh_.advertise("delay", 100); state_ = ts_not_initalized; + + callback_ = callback; + ROS_ERROR("CALLBACK SET"); } void start() { @@ -113,6 +116,8 @@ class MavrosTrigger{ return; } + + if(cache_queue_[channel].frame) { ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " Overwriting image queue! Make sure you're getting " "Timestamps from mavros. This message will only print " @@ -179,6 +184,13 @@ class MavrosTrigger{ return false; } + double delay = old_stamp.toSec() - it->second.toSec(); + geometry_msgs::PointStamped msg; + msg.header.stamp = it->second; + msg.point.x = delay; + msg.point.y = 1.0; + delay_pub_.publish(msg); + sequence_time_map_[channel].erase(it); state_ = ts_synced; return true; @@ -200,7 +212,10 @@ class MavrosTrigger{ } double delay = old_stamp.toSec() - it->second.toSec(); - delay_pub_.publish(delay); + geometry_msgs::PointStamped msg; + msg.header.stamp = it->second; + msg.point.x = delay; + delay_pub_.publish(msg); ROS_INFO("[Mavros Triggering] Remapped seq %d to %d, %f to %f: %f", seq, seq + trigger_sequence_offset_, old_stamp.toSec(), it->second.toSec(), delay); @@ -230,7 +245,8 @@ class MavrosTrigger{ ros::Time shiftTimestampToMidExposure(const ros::Time& stamp, double exposure_us) { ROS_WARN_STREAM(".5 Exposure =" << exposure_us*1e-6/2.0); - ros::Time new_stamp = stamp ;//+ ros::Duration(exposure_us * 1e-6 / 2.0); (debug)c + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(8.75 * 1e-3); + return new_stamp; } @@ -281,10 +297,10 @@ class MavrosTrigger{ cache_queue_[channel].old_stamp, cache_queue_[channel].exposure, &new_stamp, kFromImageQueue)) { - if (callback) { + if (callback_) { ROS_WARN_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); - callback(channel, new_stamp, cache_queue_[channel].frame); + callback_(channel, new_stamp, cache_queue_[channel].frame); } else { ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " No callback set - discarding cached images."); } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 5bf1542ea5..f069fa9a62 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -129,7 +129,26 @@ void BaseRealSenseNode::getParameters() // set up mavros trigger if enabled if(_force_mavros_triggering){ - _trigger.setup(); + + // create callback for cached images + std::function&)> f1 = [this](const stream_index_pair& channel, + const ros::Time& new_stamp, + const std::shared_ptr& cal){ + // fix stamps + cal->img->header.stamp = new_stamp; + cal->info.header.stamp = new_stamp; + + //publish + auto& info_publisher = this->_info_publisher.at(channel); + auto& image_publisher = this->_image_publishers.at(channel); + info_publisher.publish(cal->info); + + image_publisher.first.publish(cal->img); + image_publisher.second->update(); + }; + _trigger.setup(f1); //ros::Duration(1.0).sleep(); } @@ -594,25 +613,6 @@ void BaseRealSenseNode::setupStreams() if(_force_mavros_triggering) { ros::spinOnce(); - // create callback for cached images - std::function&)> f1 = [this](const stream_index_pair& channel, - const ros::Time& new_stamp, - const std::shared_ptr& cal){ - // fix stamps - cal->img->header.stamp = new_stamp; - cal->info.header.stamp = new_stamp; - - //publish - auto& info_publisher = this->_info_publisher.at(channel); - auto& image_publisher = this->_image_publishers.at(channel); - info_publisher.publish(cal->info); - - image_publisher.first.publish(cal->img); - image_publisher.second->update(); - }; - _trigger.callback = f1; _trigger.start(); ros::spinOnce(); @@ -700,6 +700,7 @@ void BaseRealSenseNode::setupStreams() stream_index_pair sip{stream_type,stream_index}; + ros::spinOnce(); publishFrame(frame, t, sip, _image, @@ -1314,6 +1315,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, } sensor_msgs::ImagePtr img; + img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); img->width = width; img->height = height; From 988004be4716369564c941847ab07c17bd3aa7af Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Tue, 2 Oct 2018 10:50:13 +0200 Subject: [PATCH 07/41] Timestamp offset --- realsense2_camera/include/mavros_timestamp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index ccaafb16ef..2dcafa8080 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -245,7 +245,7 @@ class MavrosTrigger{ ros::Time shiftTimestampToMidExposure(const ros::Time& stamp, double exposure_us) { ROS_WARN_STREAM(".5 Exposure =" << exposure_us*1e-6/2.0); - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(8.75 * 1e-3); + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(9.25 * 1e-3); return new_stamp; } From 488d8b128aafb8d15d552115c17ab3ec6ec9618d Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Tue, 2 Oct 2018 13:58:15 +0200 Subject: [PATCH 08/41] CleanuP --- realsense2_camera/include/mavros_timestamp.h | 282 ++++++++++--------- 1 file changed, 144 insertions(+), 138 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 2dcafa8080..3ede0379ab 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -13,21 +13,30 @@ #include #include -namespace mavros_trigger -{ -enum trigger_state{ +// Note on multi threading: +// To avoid any confusion and non-defined state, this class locks a mutex for every function call +// that is not const. This is due to the fact that many callbacks can happen simultaneously, especially +// on callback based drivers such as the realsense. If not locked properly, this can lead to weird states. + + +namespace mavros_trigger { +enum trigger_state { ts_synced = 1, ts_not_initalized, - ts_wait_for_sync, + ts_wait_for_sync, // MAVROS reset sent, but no image/timestamps correlated yet. ts_disabled }; - template -class MavrosTrigger{ +class MavrosTrigger { - typedef boost::function& cal)> caching_callback; + // callback definition for processing cached frames (with type t_cache). + typedef boost::function &cal)> caching_callback; + // internal representation of a cached frame + // t_cache is the external representation typedef struct { uint32_t seq; ros::Time old_stamp; @@ -36,46 +45,41 @@ class MavrosTrigger{ } cache_queue_type; public: - MavrosTrigger(const std::set& channel_set) : - channel_set_(channel_set), - state_(ts_not_initalized){ - ROS_WARN_STREAM("CHANNEL NUMBER" << channel_set_.size()); - for(t_chanel_id id : channel_set_){ + MavrosTrigger(const std::set &channel_set) : + channel_set_(channel_set), + state_(ts_not_initalized) { + ROS_DEBUG_STREAM(log_prefix_ << " Initialized with " << channel_set_.size() << " channels."); + for (t_chanel_id id : channel_set_) { sequence_time_map_[id].clear(); } } - caching_callback callback_; - - void setup(const caching_callback& callback){ + void setup(const caching_callback &callback) { + std::lock_guard lg(mutex_); - // Set these for now... trigger_sequence_offset_ = 0; - cam_imu_sub_ = - nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, - &MavrosTrigger::camImuStampCallback, this); - delay_pub_ = nh_.advertise("delay", 100); - state_ = ts_not_initalized; - callback_ = callback; - ROS_ERROR("CALLBACK SET"); + cam_imu_sub_ = + nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, + &MavrosTrigger::camImuStampCallback, this); + + ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); } void start() { - std::lock_guard lg(start_lock); // This now locked your mutex + std::lock_guard lg(mutex_); - if(state_ != ts_not_initalized){ + if (state_ != ts_not_initalized) { //already started, ignore return; } - ROS_WARN("TRIGG"); - // First subscribe to the messages so we don't miss any.' + ROS_INFO_STREAM(log_prefix_ << " Started triggering."); - for(t_chanel_id id : channel_set_){ + for (t_chanel_id id : channel_set_) { sequence_time_map_[id].clear(); } @@ -99,59 +103,90 @@ class MavrosTrigger{ } } - bool channelValid(const t_chanel_id& channel){ + bool channelValid(const t_chanel_id &channel) const { return channel_set_.count(channel) == 1; } - void cacheFrame(const t_chanel_id& channel, const uint32_t seq, const ros::Time& original_stamp, double exposure, - const std::shared_ptr frame){ - std::lock_guard lg(start_lock); // This now locked your mutex + void cacheFrame(const t_chanel_id &channel, const uint32_t seq, const ros::Time &original_stamp, double exposure, + const std::shared_ptr frame) { + std::lock_guard lg(mutex_); - if(state_ != ts_synced){ + if (state_ != ts_synced) { return; } - if(!channelValid(channel)){ - ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for unsynchronized channel."); + if (!channelValid(channel)) { + ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for invalid channel."); return; } - - - if(cache_queue_[channel].frame) { + if (cache_queue_[channel].frame) { ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " Overwriting image queue! Make sure you're getting " "Timestamps from mavros. This message will only print " "every 10 sec."); } + // set chached frame cache_queue_[channel].frame = frame; cache_queue_[channel].old_stamp = original_stamp; cache_queue_[channel].seq = seq; cache_queue_[channel].exposure = exposure; - - ROS_WARN_STREAM(log_prefix_ << "Cached frame w seq " << seq); + ROS_DEBUG_STREAM(log_prefix_ << "Cached frame w seq " << seq); } - void reset(){ - state_ = ts_not_initalized; - for(auto channel : channel_set_){ - sequence_time_map_[channel].clear(); - //if(cache_queue_[channel].frame) cache_queue_[channel].frame.reset(); + bool syncOffset(const t_chanel_id &channel, const uint32_t seq, const ros::Time &old_stamp) { + // Get the first from the sequence time map. + auto it = sequence_time_map_[channel].begin(); + int32_t mavros_sequence = it->first; + + // Get offset between first frame sequence and mavros + trigger_sequence_offset_ = + mavros_sequence - static_cast(seq); + + double delay = old_stamp.toSec() - it->second.toSec(); + + + ROS_INFO( + "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " + "correction: %f seconds.", + log_prefix_, channel, + trigger_sequence_offset_, it->first, seq, + delay); + + // Check for divergence (too large delay) + const double kMinExpectedDelay = 0.0; + const double kMaxExpectedDelay = 34.0 * 1e-3; + + if ((delay < kMinExpectedDelay || delay > kMaxExpectedDelay)) { + ROS_ERROR( + "%s Delay out of bounds! Actual delay: %f s, min: " + "%f s max: %f s. Resetting triggering on next image.", + log_prefix_, + delay, kMinExpectedDelay, kMaxExpectedDelay); + return false; + } + + if (delay < 0.0) { + ROS_ERROR_STREAM(log_prefix_ << " Ignoring negative time offset"); + // this forces synchronization to always be "forward" - the camera + // frame received after the trigger signal is assigned the triggering timestamp. + // Note that this could, depending on camera/hardware be wrong. + return false; } + state_ = ts_synced; + return true; } + bool lookupSequenceStamp(const t_chanel_id &channel, const uint32_t seq, + const ros::Time &old_stamp, double exposure, ros::Time *new_stamp, + bool from_image_queue = false) { - bool lookupSequenceStamp(const t_chanel_id& channel, const uint32_t seq, - const ros::Time& old_stamp, double exposure, ros::Time* new_stamp, - bool from_image_queue = false ) { + std::lock_guard lg(mutex_); - std::lock_guard lg(start_lock); // This now locked your mutex - - if(!channelValid(channel)){ + if (!channelValid(channel)) { ROS_WARN_STREAM_ONCE(log_prefix_ << "lookupSequenceStamp called for unsynchronized channel."); - return false; } @@ -160,130 +195,102 @@ class MavrosTrigger{ return false; } - // Handle case of first image where offset is not determined yet - if (state_ == ts_wait_for_sync && !from_image_queue) { - - // Get the first from the sequence time map. - auto it = sequence_time_map_[channel].begin(); - int32_t mavros_sequence = it->first; - - // Get offset between first frame sequence and mavros - trigger_sequence_offset_ = - mavros_sequence - static_cast(seq); + // Unsynced state and cached images => cannot determine offset. + if (state_ == ts_wait_for_sync && from_image_queue) { + ROS_ERROR_STREAM(log_prefix_ << " Received cached image without sync, potentially wrong timestamps published"); + return true; // return true to process the frame nevertheless. So it doesn't go to cache again + } - ROS_INFO( - "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " - "correction: %f seconds.", - log_prefix_, channel, - trigger_sequence_offset_, it->first, seq, - it->second.toSec() - old_stamp.toSec()); - - if( (it->second.toSec() - old_stamp.toSec()) >0.0) { - ROS_ERROR("Ignoring negative offset"); + // Unsynced state and "fresh" image => try determining offset. + bool newly_synchronized = false; + if (state_ == ts_wait_for_sync && !from_image_queue) { + if (!(newly_synchronized =syncOffset(channel, seq, old_stamp))) { return false; } - - double delay = old_stamp.toSec() - it->second.toSec(); - geometry_msgs::PointStamped msg; - msg.header.stamp = it->second; - msg.point.x = delay; - msg.point.y = 1.0; - delay_pub_.publish(msg); - - sequence_time_map_[channel].erase(it); - state_ = ts_synced; - return true; } - // Search correct sequence number and adjust - auto it = sequence_time_map_[channel].find(seq + trigger_sequence_offset_); - if (it == sequence_time_map_[channel].end()) { - ROS_WARN_STREAM("Sequence not found " << seq+trigger_sequence_offset_ << " original seq# " << seq); + // Synchronized state - we also hit this if we synced successfully during the same function call + if (state_ == ts_synced) { + // Search correct sequence number and adjust + auto it = sequence_time_map_[channel].find(seq + trigger_sequence_offset_); + + // if we haven't found the sequence + if (it == sequence_time_map_[channel].end()) { + ROS_WARN_STREAM( + log_prefix_ << "Sequence not found " << seq + trigger_sequence_offset_ << " original seq# " << seq); - if(seqsecond; + *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); + sequence_time_map_[channel].erase(it); } - double delay = old_stamp.toSec() - it->second.toSec(); + double delay = old_stamp.toSec() - new_stamp->toSec(); + + // if we made it until here, it's a successful match. geometry_msgs::PointStamped msg; - msg.header.stamp = it->second; + msg.header.stamp = *new_stamp; msg.point.x = delay; + msg.point.y = newly_synchronized ? 1.0 : 0.0; // Mark new synchronization by a 1 delay_pub_.publish(msg); - ROS_INFO("[Mavros Triggering] Remapped seq %d to %d, %f to %f: %f", seq, - seq + trigger_sequence_offset_, old_stamp.toSec(), - it->second.toSec(), delay); - - // Check for divergence (too large delay) - const double kMinExpectedDelay = 0.0; - const double kMaxExpectedDelay = 34.0 * 1e-3; - - if ( (delay < kMinExpectedDelay || delay > kMaxExpectedDelay)) { - ROS_ERROR( - "%s Delay out of bounds! Actual delay: %f s, min: " - "%f s max: %f s. Resetting triggering on next image.", - log_prefix_, - delay, kMinExpectedDelay, kMaxExpectedDelay); - reset(); - return true; - } - - *new_stamp = it->second; - *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); - sequence_time_map_[channel].erase(it); + ROS_INFO_STREAM(log_prefix_ << " Remapped sequence " << seq << " -> " << seq + trigger_sequence_offset_ << + " and time " << old_stamp.toSec() << " -> " << new_stamp->toSec() << " ~ " << delay); return true; } - ros::Time shiftTimestampToMidExposure(const ros::Time& stamp, - double exposure_us) { - ROS_WARN_STREAM(".5 Exposure =" << exposure_us*1e-6/2.0); - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(9.25 * 1e-3); + ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, + double exposure_us) const { + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(9.25 * 1e-3); return new_stamp; } - - void camImuStampCallback(const mavros_msgs::CamIMUStamp& cam_imu_stamp) { - start_lock.lock(); + void camImuStampCallback(const mavros_msgs::CamIMUStamp &cam_imu_stamp) { + mutex_.lock(); if (state_ == ts_not_initalized) { // Ignore stuff from before we *officially* start the triggering. // The triggering is techncially always running but... - start_lock.unlock(); + mutex_.unlock(); return; } //ignore messages until they wrap back - we should receive a 0 eventually. - if(state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0){ + if (state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0) { ROS_WARN("[Cam Imu Sync] Ignoring old messages"); - start_lock.unlock(); + mutex_.unlock(); return; } // insert stamps into sequence map for all channels // each channel can use each time stamp once. - for(auto channel : channel_set_){ + for (auto channel : channel_set_) { sequence_time_map_[channel][cam_imu_stamp.frame_seq_id] = cam_imu_stamp.frame_stamp; } ROS_INFO( "[Cam Imu Sync] Received a new stamp for sequence number: %ld (%ld) with " "stamp: %f", - cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, cam_imu_stamp.frame_stamp.toSec()); + cam_imu_stamp.frame_seq_id, + cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, + cam_imu_stamp.frame_stamp.toSec()); - - start_lock.unlock(); + mutex_.unlock(); // release potentially cached frames constexpr bool kFromImageQueue = true; - for(auto channel : channel_set_) { + for (auto channel : channel_set_) { if (!cache_queue_[channel].frame) { continue; @@ -294,9 +301,9 @@ class MavrosTrigger{ // even if we do have to discard cached image due to a missing callback fct, // we want them to pass through the lookup logic, in order to not miss any sequence offsets etc. if (lookupSequenceStamp(channel, cache_queue_[channel].seq, - cache_queue_[channel].old_stamp, - cache_queue_[channel].exposure, - &new_stamp, kFromImageQueue)) { + cache_queue_[channel].old_stamp, + cache_queue_[channel].exposure, + &new_stamp, kFromImageQueue)) { if (callback_) { ROS_WARN_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); @@ -313,16 +320,16 @@ class MavrosTrigger{ private: ros::NodeHandle nh_; - - trigger_state state_; - + const std::set channel_set_; int trigger_sequence_offset_ = 0; - const std::set channel_set_; ros::Subscriber cam_imu_sub_; ros::Publisher delay_pub_; std::map> sequence_time_map_; - std::mutex start_lock; + std::mutex mutex_; + + caching_callback callback_; + trigger_state state_; std::map logging_name_; std::map cache_queue_; @@ -330,7 +337,6 @@ class MavrosTrigger{ const std::string log_prefix_ = "[Mavros Triggering] "; }; - } #endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H From 2d2226ededbad79ada65152d0530e68dba8c45f9 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Mon, 8 Oct 2018 15:30:14 +0200 Subject: [PATCH 09/41] Fixed some race-conditions with caching of images --- realsense2_camera/include/mavros_timestamp.h | 25 +++++++++++++------ realsense2_camera/src/base_realsense_node.cpp | 8 +++--- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 3ede0379ab..fd607ca39c 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -109,18 +109,26 @@ class MavrosTrigger { void cacheFrame(const t_chanel_id &channel, const uint32_t seq, const ros::Time &original_stamp, double exposure, const std::shared_ptr frame) { - std::lock_guard lg(mutex_); - + mutex_.lock(); if (state_ != ts_synced) { + mutex_.unlock(); return; } if (!channelValid(channel)) { + mutex_.unlock(); ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for invalid channel."); return; } + if(cache_queue_[channel].frame){ + mutex_.unlock(); + ros::spinOnce(); + mutex_.lock(); + } + if (cache_queue_[channel].frame) { + ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " Overwriting image queue! Make sure you're getting " "Timestamps from mavros. This message will only print " "every 10 sec."); @@ -132,6 +140,7 @@ class MavrosTrigger { cache_queue_[channel].seq = seq; cache_queue_[channel].exposure = exposure; ROS_DEBUG_STREAM(log_prefix_ << "Cached frame w seq " << seq); + mutex_.unlock(); } bool syncOffset(const t_chanel_id &channel, const uint32_t seq, const ros::Time &old_stamp) { @@ -157,12 +166,13 @@ class MavrosTrigger { const double kMinExpectedDelay = 0.0; const double kMaxExpectedDelay = 34.0 * 1e-3; - if ((delay < kMinExpectedDelay || delay > kMaxExpectedDelay)) { + if (delay > kMaxExpectedDelay) { ROS_ERROR( "%s Delay out of bounds! Actual delay: %f s, min: " "%f s max: %f s. Resetting triggering on next image.", log_prefix_, delay, kMinExpectedDelay, kMaxExpectedDelay); + state_ = ts_not_initalized; return false; } @@ -258,7 +268,8 @@ class MavrosTrigger { } void camImuStampCallback(const mavros_msgs::CamIMUStamp &cam_imu_stamp) { - mutex_.lock(); + mutex_.lock(); // we don't use a lock guard here because we want to release before function ends. + if (state_ == ts_not_initalized) { // Ignore stuff from before we *officially* start the triggering. // The triggering is techncially always running but... @@ -286,7 +297,8 @@ class MavrosTrigger { cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, cam_imu_stamp.frame_stamp.toSec()); - mutex_.unlock(); + mutex_.unlock(); // unlock here to prevent cascading locks. for now.. + // release potentially cached frames constexpr bool kFromImageQueue = true; @@ -305,7 +317,7 @@ class MavrosTrigger { cache_queue_[channel].exposure, &new_stamp, kFromImageQueue)) { if (callback_) { - ROS_WARN_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); + ROS_DEBUG_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); callback_(channel, new_stamp, cache_queue_[channel].frame); } else { @@ -314,7 +326,6 @@ class MavrosTrigger { cache_queue_[channel].frame.reset(); } } - ROS_INFO("DONE"); } private: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index f069fa9a62..7aef106454 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -612,8 +612,6 @@ void BaseRealSenseNode::setupStreams() //todo(mpantic): find it where this code belongs ideally (during or before first callback?) if(_force_mavros_triggering) { ros::spinOnce(); - - _trigger.start(); ros::spinOnce(); } @@ -651,7 +649,7 @@ void BaseRealSenseNode::setupStreams() std::vector frames; if (frame.is()) { - ROS_INFO("Frameset arrived."); + ROS_DEBUG("Frameset arrived."); bool is_depth_arrived = false; rs2::frame depth_frame; auto frameset = frame.as(); @@ -662,7 +660,7 @@ void BaseRealSenseNode::setupStreams() auto stream_index = f.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_INFO("Frameset contain (%s, %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + ROS_DEBUG("Frameset contain (%s, %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); stream_index_pair sip{stream_type,stream_index}; @@ -695,7 +693,7 @@ void BaseRealSenseNode::setupStreams() auto stream_type = frame.get_profile().stream_type(); auto stream_index = frame.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_INFO("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); stream_index_pair sip{stream_type,stream_index}; From f02f6bb9ee2304e6b30d51225fabea63f47ceb7e Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Thu, 11 Oct 2018 13:41:11 +0200 Subject: [PATCH 10/41] Rewrote caching code --- realsense2_camera/include/mavros_timestamp.h | 291 +++++++++--------- realsense2_camera/src/base_realsense_node.cpp | 17 +- 2 files changed, 152 insertions(+), 156 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index fd607ca39c..702573aa82 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include #include // Note on multi threading: @@ -27,31 +29,38 @@ enum trigger_state { ts_disabled }; -template +template class MavrosTrigger { // callback definition for processing cached frames (with type t_cache). - typedef boost::function &cal)> caching_callback; // internal representation of a cached frame // t_cache is the external representation typedef struct { - uint32_t seq; - ros::Time old_stamp; + bool trigger_info; + bool camera_info; + + uint32_t seq_camera; + + ros::Time stamp_camera; + ros::Time stamp_trigger; std::shared_ptr frame; double exposure; } cache_queue_type; + + public: - MavrosTrigger(const std::set &channel_set) : + MavrosTrigger(const std::set &channel_set) : channel_set_(channel_set), state_(ts_not_initalized) { ROS_DEBUG_STREAM(log_prefix_ << " Initialized with " << channel_set_.size() << " channels."); - for (t_chanel_id id : channel_set_) { - sequence_time_map_[id].clear(); + for (t_channel_id id : channel_set_) { + cache_queue_[id].clear(); } } @@ -79,8 +88,8 @@ class MavrosTrigger { ROS_INFO_STREAM(log_prefix_ << " Started triggering."); - for (t_chanel_id id : channel_set_) { - sequence_time_map_[id].clear(); + for (t_channel_id id : channel_set_) { + cache_queue_[id].clear(); } trigger_sequence_offset_ = 0; @@ -103,70 +112,41 @@ class MavrosTrigger { } } - bool channelValid(const t_chanel_id &channel) const { + bool channelValid(const t_channel_id &channel) const { return channel_set_.count(channel) == 1; } - void cacheFrame(const t_chanel_id &channel, const uint32_t seq, const ros::Time &original_stamp, double exposure, - const std::shared_ptr frame) { - mutex_.lock(); - if (state_ != ts_synced) { - mutex_.unlock(); - return; - } - if (!channelValid(channel)) { - mutex_.unlock(); - ROS_WARN_STREAM_ONCE(log_prefix_ << "cacheFrame called for invalid channel."); - return; - } - - if(cache_queue_[channel].frame){ - mutex_.unlock(); - ros::spinOnce(); - mutex_.lock(); + bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { + // Get the first from the sequence time map. + auto it = cache_queue_[channel].begin(); + if(!it->second.trigger_info){ + ROS_ERROR_STREAM(log_prefix_ << "First queue entry without trigger info - should never happen"); + return false; } - if (cache_queue_[channel].frame) { - - ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " Overwriting image queue! Make sure you're getting " - "Timestamps from mavros. This message will only print " - "every 10 sec."); - } - - // set chached frame - cache_queue_[channel].frame = frame; - cache_queue_[channel].old_stamp = original_stamp; - cache_queue_[channel].seq = seq; - cache_queue_[channel].exposure = exposure; - ROS_DEBUG_STREAM(log_prefix_ << "Cached frame w seq " << seq); - mutex_.unlock(); - } - - bool syncOffset(const t_chanel_id &channel, const uint32_t seq, const ros::Time &old_stamp) { - // Get the first from the sequence time map. - auto it = sequence_time_map_[channel].begin(); - int32_t mavros_sequence = it->first; + // key is sequence of trigger + uint32_t seq_trigger = it->first; // Get offset between first frame sequence and mavros trigger_sequence_offset_ = - mavros_sequence - static_cast(seq); + static_cast(seq_trigger) - static_cast(seq_camera); - double delay = old_stamp.toSec() - it->second.toSec(); + double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); ROS_INFO( "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " "correction: %f seconds.", log_prefix_, channel, - trigger_sequence_offset_, it->first, seq, + trigger_sequence_offset_, it->first, seq_camera, delay); // Check for divergence (too large delay) - const double kMinExpectedDelay = 0.0; - const double kMaxExpectedDelay = 34.0 * 1e-3; + const double kMinExpectedDelay = -17.0 * 1e-3; + const double kMaxExpectedDelay = 17.0 * 1e-3; - if (delay > kMaxExpectedDelay) { + if (delay > kMaxExpectedDelay || delay < kMinExpectedDelay) { ROS_ERROR( "%s Delay out of bounds! Actual delay: %f s, min: " "%f s max: %f s. Resetting triggering on next image.", @@ -176,92 +156,61 @@ class MavrosTrigger { return false; } - if (delay < 0.0) { - ROS_ERROR_STREAM(log_prefix_ << " Ignoring negative time offset"); - // this forces synchronization to always be "forward" - the camera - // frame received after the trigger signal is assigned the triggering timestamp. - // Note that this could, depending on camera/hardware be wrong. - return false; + state_ = ts_synced; + + for (auto ch_id : channel_set_) { + last_published_trigger_seq_[ch_id] = seq_trigger - 1; } - state_ = ts_synced; return true; - } - bool lookupSequenceStamp(const t_chanel_id &channel, const uint32_t seq, - const ros::Time &old_stamp, double exposure, ros::Time *new_stamp, - bool from_image_queue = false) { + void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, + const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ std::lock_guard lg(mutex_); if (!channelValid(channel)) { - ROS_WARN_STREAM_ONCE(log_prefix_ << "lookupSequenceStamp called for unsynchronized channel."); - return false; + ROS_WARN_STREAM_ONCE(log_prefix_ << "Unsynchronized channe - ignoring frame."); } - if (sequence_time_map_[channel].empty()) { - ROS_DEBUG_STREAM(log_prefix_ << " Sequence time map empty"); - return false; - } - - - // Unsynced state and cached images => cannot determine offset. - if (state_ == ts_wait_for_sync && from_image_queue) { - ROS_ERROR_STREAM(log_prefix_ << " Received cached image without sync, potentially wrong timestamps published"); - return true; // return true to process the frame nevertheless. So it doesn't go to cache again - } - - // Unsynced state and "fresh" image => try determining offset. - bool newly_synchronized = false; - if (state_ == ts_wait_for_sync && !from_image_queue) { - if (!(newly_synchronized =syncOffset(channel, seq, old_stamp))) { - return false; + if (state_ == ts_wait_for_sync) { + if (!syncOffset(channel, camera_seq, camera_stamp)) { + return; } } - // Synchronized state - we also hit this if we synced successfully during the same function call - if (state_ == ts_synced) { - // Search correct sequence number and adjust - auto it = sequence_time_map_[channel].find(seq + trigger_sequence_offset_); + // Ignore frame, we are not synced yet by now + if (state_ != ts_synced) { + ROS_WARN_STREAM(log_prefix_ << " Trigger not synced yet - ignoring frame."); + return; + } - // if we haven't found the sequence - if (it == sequence_time_map_[channel].end()) { - ROS_WARN_STREAM( - log_prefix_ << "Sequence not found " << seq + trigger_sequence_offset_ << " original seq# " << seq); + // add frame to cache - /* Not sure if needed / meaningful: - if (seq < trigger_sequence_offset_ && from_image_queue) { - ROS_WARN_STREAM("REMOVED CACHE"); - state_ = ts_not_initalized; // reset - return true; - }*/ + // get trigger sequence for this camera frame + const uint32_t trigger_seq = cameraToTriggerSeq(camera_seq); - return false; - } + // get or create cache item for this trigger_seq + cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; - *new_stamp = it->second; - *new_stamp = shiftTimestampToMidExposure(*new_stamp, exposure); - sequence_time_map_[channel].erase(it); + if(!cache_entry.camera_info) + { + // complete information if needed + cache_entry.camera_info = true; + cache_entry.seq_camera = camera_seq; + cache_entry.stamp_camera = camera_stamp; + cache_entry.frame = frame; + cache_entry.exposure = exposure; } - double delay = old_stamp.toSec() - new_stamp->toSec(); - - // if we made it until here, it's a successful match. - geometry_msgs::PointStamped msg; - msg.header.stamp = *new_stamp; - msg.point.x = delay; - msg.point.y = newly_synchronized ? 1.0 : 0.0; // Mark new synchronization by a 1 - delay_pub_.publish(msg); + releaseCacheEntries(channel); + } - ROS_INFO_STREAM(log_prefix_ << " Remapped sequence " << seq << " -> " << seq + trigger_sequence_offset_ << - " and time " << old_stamp.toSec() << " -> " << new_stamp->toSec() << " ~ " << delay); - return true; - } ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, - double exposure_us) const { + const double exposure_us) const { ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(9.25 * 1e-3); return new_stamp; @@ -279,7 +228,10 @@ class MavrosTrigger { //ignore messages until they wrap back - we should receive a 0 eventually. if (state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0) { - ROS_WARN("[Cam Imu Sync] Ignoring old messages"); + ROS_WARN("[Cam Imu Sync] Ignoring old messages, clearing queues"); + for (auto channel : channel_set_) { + cache_queue_[channel].clear(); + } mutex_.unlock(); return; } @@ -287,7 +239,7 @@ class MavrosTrigger { // insert stamps into sequence map for all channels // each channel can use each time stamp once. for (auto channel : channel_set_) { - sequence_time_map_[channel][cam_imu_stamp.frame_seq_id] = cam_imu_stamp.frame_stamp; + processTriggerMessage(channel, cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_stamp); } ROS_INFO( @@ -297,53 +249,104 @@ class MavrosTrigger { cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, cam_imu_stamp.frame_stamp.toSec()); + // release waiting cache items + for (auto channel : channel_set_) { + releaseCacheEntries(channel); + } + mutex_.unlock(); // unlock here to prevent cascading locks. for now.. - // release potentially cached frames - constexpr bool kFromImageQueue = true; + } - for (auto channel : channel_set_) { + inline uint32_t triggerToCameraSeq(const uint32_t trigger_seq){ + return trigger_seq - trigger_sequence_offset_; + } - if (!cache_queue_[channel].frame) { - continue; - } + inline uint32_t cameraToTriggerSeq(const uint32_t camera_seq){ + return camera_seq + trigger_sequence_offset_; + } - ros::Time new_stamp; - - // even if we do have to discard cached image due to a missing callback fct, - // we want them to pass through the lookup logic, in order to not miss any sequence offsets etc. - if (lookupSequenceStamp(channel, cache_queue_[channel].seq, - cache_queue_[channel].old_stamp, - cache_queue_[channel].exposure, - &new_stamp, kFromImageQueue)) { - if (callback_) { - ROS_DEBUG_STREAM(log_prefix_ << "Released image w seq " << cache_queue_[channel].seq); - - callback_(channel, new_stamp, cache_queue_[channel].frame); - } else { - ROS_WARN_STREAM_THROTTLE(10, log_prefix_ << " No callback set - discarding cached images."); - } - cache_queue_[channel].frame.reset(); - } + void processTriggerMessage(const t_channel_id& channel, const uint32_t trigger_seq, const ros::Time& trigger_time){ + + // get or create cache item for this trigger_seq + cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; + + if(!cache_entry.trigger_info){ + + // complete information if needed + cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; + cache_entry.trigger_info = true; + cache_entry.stamp_trigger = trigger_time; } } + void releaseCacheEntries(const t_channel_id& channel){ + bool entry_released = false; + + // release all entries that are complete. + do { + uint32_t seq_trigger = last_published_trigger_seq_[channel] + 1; + + // check if "next" trigger sequence exists and is ready to be released + auto it_next_sequence = cache_queue_[channel].find(seq_trigger); + + + // if item exists and has all necessary info, release it + entry_released = (it_next_sequence != cache_queue_[channel].end()) && + (it_next_sequence->second.camera_info && it_next_sequence->second.trigger_info); + + if(entry_released) { + auto entry = it_next_sequence->second; + + // adjust timestamp + ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_camera, + entry.exposure); + + // call callback to publish + callback_(channel, midpoint_exposure, entry.frame); + + // output delay message and log + double delay = midpoint_exposure.toSec() - entry.stamp_camera.toSec(); + geometry_msgs::PointStamped msg; + msg.header.stamp = midpoint_exposure; + msg.point.x = delay; + msg.point.y = entry.seq_camera; + msg.point.z = seq_trigger; + delay_pub_.publish(msg); + + ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); + + // cleanup + last_published_trigger_seq_[channel]++; + cache_queue_[channel].erase(it_next_sequence); + + } else { + ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size()); + } + + } while(entry_released); + + } + + + private: ros::NodeHandle nh_; - const std::set channel_set_; + const std::set channel_set_; int trigger_sequence_offset_ = 0; ros::Subscriber cam_imu_sub_; ros::Publisher delay_pub_; - std::map> sequence_time_map_; + //std::map> sequence_time_map_; + std::mutex mutex_; caching_callback callback_; trigger_state state_; - std::map logging_name_; - std::map cache_queue_; + std::map> cache_queue_; + std::map last_published_trigger_seq_; const std::string log_prefix_ = "[Mavros Triggering] "; }; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 7aef106454..fd97892d65 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1339,20 +1339,13 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, // allow clearing of IMU queue before we lookup the timestamp ros::spinOnce(); - if(!_trigger.lookupSequenceStamp(stream, cam_info.header.seq, t, exposure, &hw_synced_stamp)){ + auto frame = std::make_shared(); + frame->img = img; + frame->info = cam_info; - auto cache = std::make_shared(); - cache->img = img; - cache->info = cam_info; - - // cache frame and return if timestamp not available - _trigger.cacheFrame(stream, cam_info.header.seq, t, exposure, cache); - return; - }else{ - img->header.stamp = hw_synced_stamp; - cam_info.header.stamp =hw_synced_stamp; - } + _trigger.addCameraFrame(stream, cam_info.header.seq, t, frame, exposure); + return; } info_publisher.publish(cam_info); From 310b51b44c05d3d673e86ea051273024ad5cc9ae Mon Sep 17 00:00:00 2001 From: loon Date: Thu, 11 Oct 2018 13:42:52 +0200 Subject: [PATCH 11/41] Loon hacks --- realsense2_camera/include/mavros_timestamp.h | 4 ++-- realsense2_camera/src/base_realsense_node.cpp | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 702573aa82..7ab31374b7 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -72,7 +72,7 @@ class MavrosTrigger { state_ = ts_not_initalized; callback_ = callback; cam_imu_sub_ = - nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, + nh_.subscribe("/loon/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); @@ -94,7 +94,7 @@ class MavrosTrigger { trigger_sequence_offset_ = 0; - const std::string mavros_trigger_service = "/mavros/cmd/trigger_control"; + const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; if (ros::service::exists(mavros_trigger_service, false)) { mavros_msgs::CommandTriggerControl req; req.request.trigger_enable = true; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index fd97892d65..16c8a5768e 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -80,6 +80,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _encoding[ACCEL] = sensor_msgs::image_encodings::TYPE_8UC1; // ROS message type _unit_step_size[ACCEL] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size _stream_name[ACCEL] = "accel"; + + sleep(5); } void BaseRealSenseNode::publishTopics() From fb0a2499b787eeb3afd0a2fd9a31c69efb4b9d05 Mon Sep 17 00:00:00 2001 From: loon Date: Thu, 11 Oct 2018 13:48:04 +0200 Subject: [PATCH 12/41] Revert "Loon hacks" This reverts commit 310b51b44c05d3d673e86ea051273024ad5cc9ae. --- realsense2_camera/include/mavros_timestamp.h | 4 ++-- realsense2_camera/src/base_realsense_node.cpp | 2 -- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 7ab31374b7..702573aa82 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -72,7 +72,7 @@ class MavrosTrigger { state_ = ts_not_initalized; callback_ = callback; cam_imu_sub_ = - nh_.subscribe("/loon/mavros/cam_imu_sync/cam_imu_stamp", 100, + nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); @@ -94,7 +94,7 @@ class MavrosTrigger { trigger_sequence_offset_ = 0; - const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; + const std::string mavros_trigger_service = "/mavros/cmd/trigger_control"; if (ros::service::exists(mavros_trigger_service, false)) { mavros_msgs::CommandTriggerControl req; req.request.trigger_enable = true; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 16c8a5768e..fd97892d65 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -80,8 +80,6 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _encoding[ACCEL] = sensor_msgs::image_encodings::TYPE_8UC1; // ROS message type _unit_step_size[ACCEL] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size _stream_name[ACCEL] = "accel"; - - sleep(5); } void BaseRealSenseNode::publishTopics() From b9963d33c2b029007f535fe5e8a6034b1abd9b62 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Thu, 11 Oct 2018 14:03:50 +0200 Subject: [PATCH 13/41] More debug output. --- realsense2_camera/include/mavros_timestamp.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 702573aa82..612e462298 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -299,7 +299,7 @@ class MavrosTrigger { auto entry = it_next_sequence->second; // adjust timestamp - ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_camera, + ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_trigger, entry.exposure); // call callback to publish @@ -321,7 +321,7 @@ class MavrosTrigger { cache_queue_[channel].erase(it_next_sequence); } else { - ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size()); + ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() << ". "); } } while(entry_released); From 399542746c87dc8bc05959491953672960f7b4b6 Mon Sep 17 00:00:00 2001 From: Michael Pantic Date: Thu, 11 Oct 2018 14:14:48 +0200 Subject: [PATCH 14/41] bugfixes --- realsense2_camera/include/mavros_timestamp.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 612e462298..d17b445727 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -147,11 +147,7 @@ class MavrosTrigger { const double kMaxExpectedDelay = 17.0 * 1e-3; if (delay > kMaxExpectedDelay || delay < kMinExpectedDelay) { - ROS_ERROR( - "%s Delay out of bounds! Actual delay: %f s, min: " - "%f s max: %f s. Resetting triggering on next image.", - log_prefix_, - delay, kMinExpectedDelay, kMaxExpectedDelay); + ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay ); state_ = ts_not_initalized; return false; } @@ -172,6 +168,7 @@ class MavrosTrigger { if (!channelValid(channel)) { ROS_WARN_STREAM_ONCE(log_prefix_ << "Unsynchronized channe - ignoring frame."); + return; } if (state_ == ts_wait_for_sync) { From f95937acd7eedc72691857b3d3d635c56b0a5b04 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Tue, 16 Oct 2018 19:32:52 +0200 Subject: [PATCH 15/41] Launchfile parameters for realsense-rovio --- realsense2_camera/launch/includes/nodelet.launch.xml | 10 +++++++--- realsense2_camera/launch/rs_camera.launch | 7 +++++++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index f15ae533a9..4ca5874cbe 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -43,10 +43,14 @@ - - + + + + + + + - diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index fc77ae5a85..f05f8996b8 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -35,6 +35,9 @@ + + + @@ -44,6 +47,9 @@ + + + @@ -76,3 +82,4 @@ + From f46c5012663815d86e28aa9b197bf14bbc1d6331 Mon Sep 17 00:00:00 2001 From: loon Date: Wed, 17 Oct 2018 10:18:14 +0200 Subject: [PATCH 16/41] Wait for service on startup, use steadily increasing seq nr --- realsense2_camera/include/mavros_timestamp.h | 28 ++++++++++++++----- realsense2_camera/src/base_realsense_node.cpp | 8 ++++-- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index d17b445727..e34c5a78da 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -72,7 +72,7 @@ class MavrosTrigger { state_ = ts_not_initalized; callback_ = callback; cam_imu_sub_ = - nh_.subscribe("/mavros/cam_imu_sync/cam_imu_stamp", 100, + nh_.subscribe("/loon/mavros/cam_imu_sync/cam_imu_stamp", 100, &MavrosTrigger::camImuStampCallback, this); ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); @@ -94,7 +94,8 @@ class MavrosTrigger { trigger_sequence_offset_ = 0; - const std::string mavros_trigger_service = "/mavros/cmd/trigger_control"; + const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; + if (ros::service::exists(mavros_trigger_service, false)) { mavros_msgs::CommandTriggerControl req; req.request.trigger_enable = true; @@ -110,14 +111,27 @@ class MavrosTrigger { ROS_ERROR("Mavros service not available!"); state_ = ts_disabled; } + } + void waitMavros(){ + + const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; + + ros::service::waitForService(mavros_trigger_service); + } + bool channelValid(const t_channel_id &channel) const { return channel_set_.count(channel) == 1; } bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { + if(cache_queue_[channel].empty()){ + ROS_ERROR_STREAM(log_prefix_ << "Queue empty"); + return false; + } + // Get the first from the sequence time map. auto it = cache_queue_[channel].begin(); if(!it->second.trigger_info){ @@ -163,7 +177,8 @@ class MavrosTrigger { void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ - + + ros::spinOnce(); std::lock_guard lg(mutex_); if (!channelValid(channel)) { @@ -188,6 +203,8 @@ class MavrosTrigger { // get trigger sequence for this camera frame const uint32_t trigger_seq = cameraToTriggerSeq(camera_seq); + ROS_INFO_STREAM(log_prefix_ << " Received camera frame for seq " << trigger_seq); + // get or create cache item for this trigger_seq cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; @@ -209,7 +226,7 @@ class MavrosTrigger { ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, const double exposure_us) const { - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(9.25 * 1e-3); + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) - ros::Duration(3.75 * 1e-3); return new_stamp; } @@ -226,9 +243,6 @@ class MavrosTrigger { //ignore messages until they wrap back - we should receive a 0 eventually. if (state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0) { ROS_WARN("[Cam Imu Sync] Ignoring old messages, clearing queues"); - for (auto channel : channel_set_) { - cache_queue_[channel].clear(); - } mutex_.unlock(); return; } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index fd97892d65..a9cb6edbfa 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -85,6 +85,10 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, void BaseRealSenseNode::publishTopics() { getParameters(); + if(_force_mavros_triggering){ + _trigger.waitMavros(); + } + setupDevice(); setupPublishers(); setupStreams(); @@ -693,7 +697,7 @@ void BaseRealSenseNode::setupStreams() auto stream_type = frame.get_profile().stream_type(); auto stream_index = frame.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + ROS_DEBUG(" Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); stream_index_pair sip{stream_type,stream_index}; @@ -1343,7 +1347,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, frame->img = img; frame->info = cam_info; - _trigger.addCameraFrame(stream, cam_info.header.seq, t, frame, exposure); + _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); return; } From 9149c1f2e13712b0d1282efe99a1b46c2caca5ad Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 13:17:38 +0200 Subject: [PATCH 17/41] Added mechanism for keeping track of trigger sequence without having to publish images (e.g. no subscribers on image topic) --- realsense2_camera/include/mavros_timestamp.h | 15 ++- realsense2_camera/src/base_realsense_node.cpp | 112 ++++++++++-------- 2 files changed, 72 insertions(+), 55 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index e34c5a78da..9fbccd4c4c 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -175,6 +175,12 @@ class MavrosTrigger { return true; } + void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp){ + ROS_DEBUG_STREAM("addCameraSequence called"); + std::shared_ptr empty = {}; + addCameraFrame(channel, camera_seq, camera_stamp, empty, 0.0); + } + void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ @@ -313,9 +319,12 @@ class MavrosTrigger { ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_trigger, entry.exposure); - // call callback to publish - callback_(channel, midpoint_exposure, entry.frame); - + // publish if there is a cached frame + // - sometimes we add camera sequences without frames, e.g. if there are no subscribers + // - e.g. by calling addCameraSequence + if(entry.frame != nullptr) { + callback_(channel, midpoint_exposure, entry.frame); + } // output delay message and log double delay = midpoint_exposure.toSec() - entry.stamp_camera.toSec(); geometry_msgs::PointStamped msg; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9876d194e1..9937561dc0 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1468,62 +1468,70 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, ++(seq[stream]); auto& info_publisher = info_publishers.at(stream); auto& image_publisher = image_publishers.at(stream); - /* if(0 != info_publisher.getNumSubscribers() || - 0 != image_publisher.first.getNumSubscribers())*/ - { - auto width = 0; - auto height = 0; - auto bpp = 1; - if (f.is()) - { - auto image = f.as(); - width = image.get_width(); - height = image.get_height(); - bpp = image.get_bytes_per_pixel(); - } - - sensor_msgs::ImagePtr img; - - img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); - img->width = width; - img->height = height; - img->is_bigendian = false; - img->step = width * bpp; - img->header.frame_id = optical_frame_id.at(stream); - img->header.stamp = t; - img->header.seq = f.get_frame_number(); - - - auto& cam_info = camera_info.at(stream); - cam_info.header.stamp = img->header.stamp; - cam_info.header.seq = img->header.seq; - - // correct timestamp if needed - ros::Time hw_synced_stamp; - if(_force_mavros_triggering) { - - // todo(mpantic): investigate why exposure is not available in the beginning. - double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? - static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; + bool has_subscribers = info_publisher.getNumSubscribers() != 0 ||image_publisher.first.getNumSubscribers() != 0; + + if(has_subscribers) { + auto width = 0; + auto height = 0; + auto bpp = 1; + if (f.is()) + { + auto image = f.as(); + width = image.get_width(); + height = image.get_height(); + bpp = image.get_bytes_per_pixel(); + } + + sensor_msgs::ImagePtr img; + + img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); + img->width = width; + img->height = height; + img->is_bigendian = false; + img->step = width * bpp; + img->header.frame_id = optical_frame_id.at(stream); + img->header.stamp = t; + img->header.seq = f.get_frame_number(); + + auto& cam_info = camera_info.at(stream); + cam_info.header.stamp = img->header.stamp; + cam_info.header.seq = img->header.seq; + + // send frame off to trigger handler for caching and later release through callback + // if mavros triggering is activated + if(_force_mavros_triggering) { + + // if exposure is available, supply it to trigger handler + double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? + static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; + + // allow clearing of IMU queue before we lookup the timestamp + ros::spinOnce(); + + // create cache frame + auto frame = std::make_shared(); + frame->img = img; + frame->info = cam_info; + + _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); - // allow clearing of IMU queue before we lookup the timestamp - ros::spinOnce(); - - auto frame = std::make_shared(); - frame->img = img; - frame->info = cam_info; - - _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); - - return; - } + return; + } - info_publisher.publish(cam_info); + // if we're not using mavros trigger, directly publish + info_publisher.publish(cam_info); - image_publisher.first.publish(img); - image_publisher.second->update(); - ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); + image_publisher.first.publish(img); + image_publisher.second->update(); + ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); + } + else if(!has_subscribers && _force_mavros_triggering) + { + // if there are no subscribers, but mavros triggering is activated, + // we still need to signal frame sequences to the trigger handler + _trigger.addCameraSequence(stream, seq[stream], t); } + } bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile) From a705d1194d79d4f317b66bbea1558ec75446fb7b Mon Sep 17 00:00:00 2001 From: loon Date: Wed, 17 Oct 2018 16:07:31 +0200 Subject: [PATCH 18/41] Dynamic Reconfigure fixes with new version --- realsense2_camera/include/mavros_timestamp.h | 5 ++++- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- realsense2_camera/src/rs435_node.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 9fbccd4c4c..521d45accd 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -176,7 +176,10 @@ class MavrosTrigger { } void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp){ - ROS_DEBUG_STREAM("addCameraSequence called"); + if(channelValid(channel)){ + ROS_INFO_STREAM("addCameraSequence called"); + } + std::shared_ptr empty = {}; addCameraFrame(channel, camera_seq, camera_stamp, empty, 0.0); } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9937561dc0..0b79e9cfd4 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1559,7 +1559,7 @@ BaseD400Node::BaseD400Node(ros::NodeHandle& nodeHandle, void BaseD400Node::callback(base_d400_paramsConfig &config, uint32_t level) { - ROS_DEBUG_STREAM("D400 - Level: " << level); + ROS_INFO_STREAM("D400 - Level: " << level); if (set_default_dynamic_reconfig_values == level) { @@ -1683,4 +1683,4 @@ void BaseD400Node::registerDynamicReconfigCb() _server = std::make_shared>(); _f = boost::bind(&BaseD400Node::callback, this, _1, _2); _server->setCallback(_f); -} \ No newline at end of file +} diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp index ce6a7c5690..cd90211a44 100644 --- a/realsense2_camera/src/rs435_node.cpp +++ b/realsense2_camera/src/rs435_node.cpp @@ -98,7 +98,7 @@ void RS435Node::setParam(rs435_paramsConfig &config, rs435_param param) } break; case rs435_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); + ROS_INFO_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); break; default: @@ -109,13 +109,14 @@ void RS435Node::setParam(rs435_paramsConfig &config, rs435_param param) void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) { - ROS_DEBUG_STREAM("RS435Node - Level: " << level); + ROS_WARN_STREAM("RS435Node - Level: " << level); if (set_default_dynamic_reconfig_values == level) { for (int i = 1 ; i < rs435_param_count ; ++i) { - ROS_DEBUG_STREAM("rs435_param = " << i); + ROS_WARN_STREAM("rs435_param =" << i); + if(i==8) continue; setParam(config ,(rs435_param)i); } } @@ -123,4 +124,4 @@ void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) { setParam(config, (rs435_param)level); } -} \ No newline at end of file +} From 3d5b9b5d3be6501c53a380f53a6d35896c5a90e8 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 16:11:50 +0200 Subject: [PATCH 19/41] Dynamic reconfigure fix cleanup --- realsense2_camera/src/base_realsense_node.cpp | 2 +- realsense2_camera/src/rs435_node.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0b79e9cfd4..27776466e8 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1559,7 +1559,7 @@ BaseD400Node::BaseD400Node(ros::NodeHandle& nodeHandle, void BaseD400Node::callback(base_d400_paramsConfig &config, uint32_t level) { - ROS_INFO_STREAM("D400 - Level: " << level); + ROS_DEBUG_STREAM("D400 - Level: " << level); if (set_default_dynamic_reconfig_values == level) { diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp index cd90211a44..2661164bb0 100644 --- a/realsense2_camera/src/rs435_node.cpp +++ b/realsense2_camera/src/rs435_node.cpp @@ -109,14 +109,19 @@ void RS435Node::setParam(rs435_paramsConfig &config, rs435_param param) void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) { - ROS_WARN_STREAM("RS435Node - Level: " << level); + ROS_DEBUG_STREAM("RS435Node - Level: " << level); if (set_default_dynamic_reconfig_values == level) { for (int i = 1 ; i < rs435_param_count ; ++i) { - ROS_WARN_STREAM("rs435_param =" << i); - if(i==8) continue; + + if((base_depth_param) i == base_sensors_enabled) { + // HACK/FIX: This param causes a crash if it is set during init. + // As it only enables / disables the sensors and we always want it enabled, it is ignored + // during init. + continue; + } setParam(config ,(rs435_param)i); } } From 209762028b0b306a0069245aa0878926749d2f12 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 16:24:51 +0200 Subject: [PATCH 20/41] Slight reformating to be more consistent w.r.t. intel code --- realsense2_camera/include/mavros_timestamp.h | 3 +- realsense2_camera/src/base_realsense_node.cpp | 158 +++++++++--------- realsense2_camera/src/rs435_node.cpp | 4 +- 3 files changed, 82 insertions(+), 83 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 521d45accd..595870f186 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -115,10 +115,11 @@ class MavrosTrigger { } void waitMavros(){ - + ROS_INFO_STREAM("Waiting for mavros service...."); const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; ros::service::waitForService(mavros_trigger_service); + ROS_INFO_STREAM("... mavros service ready."); } bool channelValid(const t_channel_id &channel) const { diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 27776466e8..e7734405a7 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -99,10 +99,12 @@ void BaseRealSenseNode::toggleSensors(bool enabled) void BaseRealSenseNode::publishTopics() { getParameters(); - if(_force_mavros_triggering){ - _trigger.waitMavros(); + + if(_force_mavros_triggering) + { + // wait until trigger service becomes available + _trigger.waitMavros(); } - setupDevice(); setupPublishers(); setupStreams(); @@ -149,23 +151,26 @@ void BaseRealSenseNode::getParameters() else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = 1; } else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = 2; } else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = -1; } - else { + else + { _inter_cam_sync_mode = -1; ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); } _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); - if(_force_mavros_triggering && _inter_cam_sync_mode != 2){ + if(_force_mavros_triggering && _inter_cam_sync_mode != 2) + { ROS_WARN_STREAM("Force mavros triggering enabled but device not set to slave triggering mode!"); } // set up mavros trigger if enabled - if(_force_mavros_triggering){ + if(_force_mavros_triggering) + { // create callback for cached images std::function&)> f1 = [this](const stream_index_pair& channel, + const std::shared_ptr&)> f_cb = [this](const stream_index_pair& channel, const ros::Time& new_stamp, const std::shared_ptr& cal){ // fix stamps @@ -180,8 +185,7 @@ void BaseRealSenseNode::getParameters() image_publisher.first.publish(cal->img); image_publisher.second->update(); }; - _trigger.setup(f1); - //ros::Duration(1.0).sleep(); + _trigger.setup(f_cb); } @@ -699,17 +703,13 @@ void BaseRealSenseNode::setupStreams() auto frame_callback = [this](rs2::frame frame) { - //todo(mpantic): find it where this code belongs ideally (during or before first callback?) if(_force_mavros_triggering) { ros::spinOnce(); _trigger.start(); - ros::spinOnce(); + ros::spinOnce(); // allow imu queues to be processed } - ros::spinOnce(); - - try{ - + try{ // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, // and the incremental timestamp from the camera. // In sync mode the timestamp is based on ROS time @@ -796,17 +796,17 @@ void BaseRealSenseNode::setupStreams() stream_index_pair sip{stream_type,stream_index}; frames_to_publish.insert(std::pair(sip, f)); } -// for (auto it = frames_to_publish.begin(); it != frames_to_publish.end(); ++it) -// { -// auto f = it->second; -// auto stream_type = f.get_profile().stream_type(); -// auto stream_index = f.get_profile().stream_index(); -// auto stream_format = f.get_profile().format(); -// auto stream_unique_id = f.get_profile().unique_id(); -// -// ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", -// rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); -// } + // for (auto it = frames_to_publish.begin(); it != frames_to_publish.end(); ++it) + // { + // auto f = it->second; + // auto stream_type = f.get_profile().stream_type(); + // auto stream_index = f.get_profile().stream_index(); + // auto stream_format = f.get_profile().format(); + // auto stream_unique_id = f.get_profile().unique_id(); + // + // ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + // rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + // } for (auto it = frames_to_publish.begin(); it != frames_to_publish.end(); ++it) { @@ -1470,66 +1470,64 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, auto& image_publisher = image_publishers.at(stream); bool has_subscribers = info_publisher.getNumSubscribers() != 0 ||image_publisher.first.getNumSubscribers() != 0; - if(has_subscribers) { - auto width = 0; - auto height = 0; - auto bpp = 1; - if (f.is()) - { - auto image = f.as(); - width = image.get_width(); - height = image.get_height(); - bpp = image.get_bytes_per_pixel(); - } - - sensor_msgs::ImagePtr img; - - img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); - img->width = width; - img->height = height; - img->is_bigendian = false; - img->step = width * bpp; - img->header.frame_id = optical_frame_id.at(stream); - img->header.stamp = t; - img->header.seq = f.get_frame_number(); - - auto& cam_info = camera_info.at(stream); - cam_info.header.stamp = img->header.stamp; - cam_info.header.seq = img->header.seq; - - // send frame off to trigger handler for caching and later release through callback - // if mavros triggering is activated - if(_force_mavros_triggering) { - - // if exposure is available, supply it to trigger handler - double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? - static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; - - // allow clearing of IMU queue before we lookup the timestamp - ros::spinOnce(); - - // create cache frame - auto frame = std::make_shared(); - frame->img = img; - frame->info = cam_info; - - _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); + if(has_subscribers) + { + auto width = 0; + auto height = 0; + auto bpp = 1; + if (f.is()) + { + auto image = f.as(); + width = image.get_width(); + height = image.get_height(); + bpp = image.get_bytes_per_pixel(); + } - return; - } + sensor_msgs::ImagePtr img; + img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); + img->width = width; + img->height = height; + img->is_bigendian = false; + img->step = width * bpp; + img->header.frame_id = optical_frame_id.at(stream); + img->header.stamp = t; + img->header.seq = f.get_frame_number(); + + auto& cam_info = camera_info.at(stream); + cam_info.header.stamp = img->header.stamp; + cam_info.header.seq = img->header.seq; + + // send frame off to trigger handler for caching and later release through callback + // if mavros triggering is activated + if(_force_mavros_triggering) + { + // if exposure is available, supply it to trigger handler + double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? + static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : 0.0; + + // allow clearing of IMU queue before we lookup the timestamp + ros::spinOnce(); + + // create cache frame and hand it over to trigger + auto frame = std::make_shared(); + frame->img = img; + frame->info = cam_info; + _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); + return; + } - // if we're not using mavros trigger, directly publish - info_publisher.publish(cam_info); + // if we're not using mavros trigger, directly publish + info_publisher.publish(cam_info); - image_publisher.first.publish(img); - image_publisher.second->update(); - ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); + image_publisher.first.publish(img); + image_publisher.second->update(); + ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); } else if(!has_subscribers && _force_mavros_triggering) { - // if there are no subscribers, but mavros triggering is activated, - // we still need to signal frame sequences to the trigger handler - _trigger.addCameraSequence(stream, seq[stream], t); + // if there are no subscribers, but mavros triggering is activated, + // we still need to signal frame sequences to the trigger handler + _trigger.addCameraSequence(stream, seq[stream], t); } } diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp index 2661164bb0..85c3840b86 100644 --- a/realsense2_camera/src/rs435_node.cpp +++ b/realsense2_camera/src/rs435_node.cpp @@ -115,8 +115,8 @@ void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) { for (int i = 1 ; i < rs435_param_count ; ++i) { - - if((base_depth_param) i == base_sensors_enabled) { + if((base_depth_param) i == base_sensors_enabled) + { // HACK/FIX: This param causes a crash if it is set during init. // As it only enables / disables the sensors and we always want it enabled, it is ignored // during init. From 0f65656f68725a004312d8dec981340f80f3b2cb Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 16:45:47 +0200 Subject: [PATCH 21/41] Cleanup of mavros_timestamp --- realsense2_camera/include/mavros_timestamp.h | 227 ++++++++++--------- 1 file changed, 122 insertions(+), 105 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 595870f186..9c24a50fab 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -1,6 +1,31 @@ -// -// Created by mpantic on 19.09.18. -// +/* + Copyright (c) 2018, Michael Pantic, ASL, ETH Zurich, Switzerland + Helen Oleynikova, ASL, ETH Zurich, Switzerland + Zac Taylor, ASL, ETH Zurich, Switzerland + + You can contact the author at + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ETHZ-ASL nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ #ifndef REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H #define REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H @@ -15,15 +40,17 @@ #include #include -// Note on multi threading: +// Note on multi threading / locking: // To avoid any confusion and non-defined state, this class locks a mutex for every function call // that is not const. This is due to the fact that many callbacks can happen simultaneously, especially // on callback based drivers such as the realsense. If not locked properly, this can lead to weird states. +// Note that all private functions assume that they are called in a locked context (no cascading locks). namespace mavros_trigger { + enum trigger_state { - ts_synced = 1, + ts_synced = 1, // Sequence offset synced between cam/trigger ts_not_initalized, ts_wait_for_sync, // MAVROS reset sent, but no image/timestamps correlated yet. ts_disabled @@ -32,6 +59,10 @@ enum trigger_state { template class MavrosTrigger { + const std::string kDelayTopic = "delay"; + const std::string kCamImuSyncTopic = "mavros/cam_imu_sync/cam_imu_stamp"; + const std::string kTriggerService = "mavros/cmd/trigger_control"; + // callback definition for processing cached frames (with type t_cache). typedef boost::function frame; - double exposure; + double exposure; // [us] } cache_queue_type; public: - MavrosTrigger(const std::set &channel_set) : channel_set_(channel_set), state_(ts_not_initalized) { @@ -68,11 +96,11 @@ class MavrosTrigger { std::lock_guard lg(mutex_); trigger_sequence_offset_ = 0; - delay_pub_ = nh_.advertise("delay", 100); + delay_pub_ = nh_.advertise(kDelayTopic, 100); state_ = ts_not_initalized; callback_ = callback; cam_imu_sub_ = - nh_.subscribe("/loon/mavros/cam_imu_sync/cam_imu_stamp", 100, + nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); @@ -82,7 +110,7 @@ class MavrosTrigger { std::lock_guard lg(mutex_); if (state_ != ts_not_initalized) { - //already started, ignore + //already started, nothing to do return; } @@ -94,17 +122,15 @@ class MavrosTrigger { trigger_sequence_offset_ = 0; - const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; - - if (ros::service::exists(mavros_trigger_service, false)) { + if (ros::service::exists(kTriggerService, false)) { mavros_msgs::CommandTriggerControl req; req.request.trigger_enable = true; // This is NOT integration time, this is actually the sequence reset. req.request.integration_time = 1.0; - ros::service::call(mavros_trigger_service, req); + ros::service::call(kTriggerService, req); - ROS_INFO("Called mavros trigger service! Success? %d Result? %d", + ROS_DEBUG("Called mavros trigger service! Success? %d Result? %d", req.response.success, req.response.result); state_ = ts_wait_for_sync; } else { @@ -116,66 +142,14 @@ class MavrosTrigger { void waitMavros(){ ROS_INFO_STREAM("Waiting for mavros service...."); - const std::string mavros_trigger_service = "/loon/mavros/cmd/trigger_control"; - - ros::service::waitForService(mavros_trigger_service); + ros::service::waitForService(kTriggerService); ROS_INFO_STREAM("... mavros service ready."); } - - bool channelValid(const t_channel_id &channel) const { - return channel_set_.count(channel) == 1; - } - - - bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { - if(cache_queue_[channel].empty()){ - ROS_ERROR_STREAM(log_prefix_ << "Queue empty"); - return false; - } - - // Get the first from the sequence time map. - auto it = cache_queue_[channel].begin(); - if(!it->second.trigger_info){ - ROS_ERROR_STREAM(log_prefix_ << "First queue entry without trigger info - should never happen"); - return false; - } - - // key is sequence of trigger - uint32_t seq_trigger = it->first; - - // Get offset between first frame sequence and mavros - trigger_sequence_offset_ = - static_cast(seq_trigger) - static_cast(seq_camera); - - double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); - - - ROS_INFO( - "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " - "correction: %f seconds.", - log_prefix_, channel, - trigger_sequence_offset_, it->first, seq_camera, - delay); - - // Check for divergence (too large delay) - const double kMinExpectedDelay = -17.0 * 1e-3; - const double kMaxExpectedDelay = 17.0 * 1e-3; - - if (delay > kMaxExpectedDelay || delay < kMinExpectedDelay) { - ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay ); - state_ = ts_not_initalized; - return false; - } - - state_ = ts_synced; - - for (auto ch_id : channel_set_) { - last_published_trigger_seq_[ch_id] = seq_trigger - 1; - } - - return true; - } + /* + * Adds a timestamp and sequence number of the camera without caching/publishing the image + * - Keeps sync alive if images are not processed (e.g. because not needed) + */ void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp){ if(channelValid(channel)){ ROS_INFO_STREAM("addCameraSequence called"); @@ -185,9 +159,13 @@ class MavrosTrigger { addCameraFrame(channel, camera_seq, camera_stamp, empty, 0.0); } + /* + * Adds a timestamp, sequence number and image frame to be published by the callback once it's timing is corrected. + * - Caches t_cache frame pointer until corresponding trigger time message from mavros is received + */ void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, - const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ - + const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ + ros::spinOnce(); std::lock_guard lg(mutex_); @@ -231,29 +209,21 @@ class MavrosTrigger { releaseCacheEntries(channel); } - - - ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, - const double exposure_us) const { - - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) - ros::Duration(3.75 * 1e-3); - return new_stamp; - } - + /* + * Processes IMU-Stamp from MAVROS + */ void camImuStampCallback(const mavros_msgs::CamIMUStamp &cam_imu_stamp) { - mutex_.lock(); // we don't use a lock guard here because we want to release before function ends. + std::lock_guard lg(mutex_); if (state_ == ts_not_initalized) { // Ignore stuff from before we *officially* start the triggering. // The triggering is techncially always running but... - mutex_.unlock(); return; } //ignore messages until they wrap back - we should receive a 0 eventually. if (state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0) { ROS_WARN("[Cam Imu Sync] Ignoring old messages, clearing queues"); - mutex_.unlock(); return; } @@ -263,7 +233,7 @@ class MavrosTrigger { processTriggerMessage(channel, cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_stamp); } - ROS_INFO( + ROS_DEBUG( "[Cam Imu Sync] Received a new stamp for sequence number: %ld (%ld) with " "stamp: %f", cam_imu_stamp.frame_seq_id, @@ -274,13 +244,68 @@ class MavrosTrigger { for (auto channel : channel_set_) { releaseCacheEntries(channel); } + } - mutex_.unlock(); // unlock here to prevent cascading locks. for now.. + private: + bool channelValid(const t_channel_id &channel) const { + return channel_set_.count(channel) == 1; } - inline uint32_t triggerToCameraSeq(const uint32_t trigger_seq){ - return trigger_seq - trigger_sequence_offset_; + /* + * Tries to determine offset between camera and trigger/imu and returns true if succeeded + */ + bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { + if(cache_queue_[channel].empty()){ + ROS_WARN_STREAM_ONCE(log_prefix_ << " syncOffset called with empty queue"); + return false; + } + + // Get the first from the sequence time map. + auto it = cache_queue_[channel].begin(); + if(!it->second.trigger_info){ + ROS_ERROR_STREAM(log_prefix_ << "First queue entry without trigger info - should never happen"); + return false; + } + + // key is sequence of trigger + uint32_t seq_trigger = it->first; + + // Get offset between first frame sequence and mavros + trigger_sequence_offset_ = + static_cast(seq_trigger) - static_cast(seq_camera); + + double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); + + // Check for divergence (too large delay) + const double kMinExpectedDelay = -17.0 * 1e-3; + const double kMaxExpectedDelay = 17.0 * 1e-3; + + if (delay > kMaxExpectedDelay || delay < kMinExpectedDelay) { + ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay ); + state_ = ts_not_initalized; + return false; + } + + state_ = ts_synced; + + for (auto ch_id : channel_set_) { + last_published_trigger_seq_[ch_id] = seq_trigger - 1; + } + + ROS_INFO( + "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " + "correction: %f seconds.", + log_prefix_, channel, + trigger_sequence_offset_, it->first, seq_camera, + delay); + return true; + } + + ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, + const double exposure_us) const { + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) - ros::Duration(3.75 * 1e-3); + return new_stamp; } inline uint32_t cameraToTriggerSeq(const uint32_t camera_seq){ @@ -288,12 +313,10 @@ class MavrosTrigger { } void processTriggerMessage(const t_channel_id& channel, const uint32_t trigger_seq, const ros::Time& trigger_time){ - // get or create cache item for this trigger_seq cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; if(!cache_entry.trigger_info){ - // complete information if needed cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; cache_entry.trigger_info = true; @@ -311,7 +334,6 @@ class MavrosTrigger { // check if "next" trigger sequence exists and is ready to be released auto it_next_sequence = cache_queue_[channel].find(seq_trigger); - // if item exists and has all necessary info, release it entry_released = (it_next_sequence != cache_queue_[channel].end()) && (it_next_sequence->second.camera_info && it_next_sequence->second.trigger_info); @@ -338,23 +360,20 @@ class MavrosTrigger { msg.point.z = seq_trigger; delay_pub_.publish(msg); - ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); + ROS_DEBUG_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); // cleanup last_published_trigger_seq_[channel]++; cache_queue_[channel].erase(it_next_sequence); } else { - ROS_INFO_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() << ". "); + ROS_DEBUG_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() << ". "); } } while(entry_released); } - - - - private: + ros::NodeHandle nh_; const std::set channel_set_; @@ -362,10 +381,8 @@ class MavrosTrigger { ros::Subscriber cam_imu_sub_; ros::Publisher delay_pub_; - //std::map> sequence_time_map_; std::mutex mutex_; - caching_callback callback_; trigger_state state_; From ada78e226d0b847891c4450dca5e4228d63ce849 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 16:55:56 +0200 Subject: [PATCH 22/41] Further cleanup of mavros_timestamp, added framerate param --- realsense2_camera/include/mavros_timestamp.h | 26 +++-- realsense2_camera/src/base_realsense_node.cpp | 106 +++++++++--------- 2 files changed, 69 insertions(+), 63 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 9c24a50fab..327c6aa929 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -92,18 +92,19 @@ class MavrosTrigger { } } - void setup(const caching_callback &callback) { + void setup(const caching_callback &callback, double framerate) { std::lock_guard lg(mutex_); trigger_sequence_offset_ = 0; delay_pub_ = nh_.advertise(kDelayTopic, 100); state_ = ts_not_initalized; + framerate_ = framerate; // approximate framerate to do outlier checking callback_ = callback; cam_imu_sub_ = nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); - ROS_DEBUG_STREAM(log_prefix_ << " Callback set and subscribed to cam_imu_stamp"); + ROS_DEBUG_STREAM(log_prefix_ << " Initialized with callback and framerate "<< framerate << " hz and subscribed to cam_imu_sub"); } void start() { @@ -278,11 +279,10 @@ class MavrosTrigger { double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); // Check for divergence (too large delay) - const double kMinExpectedDelay = -17.0 * 1e-3; - const double kMaxExpectedDelay = 17.0 * 1e-3; + double delay_bounds = 1.0/framerate_; - if (delay > kMaxExpectedDelay || delay < kMinExpectedDelay) { - ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay ); + if (std::abs(delay) > delay_bounds) { + ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay << ", bounds are +/-" << delay_bounds << " s"); state_ = ts_not_initalized; return false; } @@ -324,6 +324,12 @@ class MavrosTrigger { } } + /* + * Checks all cache entries in cache_queue_ and publishes those that + * - have both camera info and trigger info filled out + * - have sequence_id = last_published_trigger_seq_ + 1 ( so we only publish in order) + * Runs as long as messages are available that satisfy this. + */ void releaseCacheEntries(const t_channel_id& channel){ bool entry_released = false; @@ -345,8 +351,8 @@ class MavrosTrigger { ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_trigger, entry.exposure); - // publish if there is a cached frame - // - sometimes we add camera sequences without frames, e.g. if there are no subscribers + // callback to publish if there is a cached frame + // - sometimes we add camera sequences without cached frames, e.g. if there are no subscribers // - e.g. by calling addCameraSequence if(entry.frame != nullptr) { callback_(channel, midpoint_exposure, entry.frame); @@ -373,7 +379,7 @@ class MavrosTrigger { } while(entry_released); } - + ros::NodeHandle nh_; const std::set channel_set_; @@ -388,8 +394,10 @@ class MavrosTrigger { std::map> cache_queue_; std::map last_published_trigger_seq_; + double framerate_; // [hz] const std::string log_prefix_ = "[Mavros Triggering] "; + }; } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index e7734405a7..983eeec6ed 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -135,60 +135,6 @@ void BaseRealSenseNode::getParameters() { ROS_INFO("getParameters..."); - std::string inter_cam_sync_mode_param; - _pnh.param("inter_cam_sync_mode", inter_cam_sync_mode_param, INTER_CAM_SYNC_MODE); - std::transform(inter_cam_sync_mode_param.begin(), inter_cam_sync_mode_param.end(), - inter_cam_sync_mode_param.begin(), ::tolower); - - // note: added a "none" mode, as not all sensor types/firmware versions allow setting of the sync mode. - // Use "none" if nothing is specified or an error occurs. - // Default (mode = 0) here refers to the default sync mode as per Intel whitepaper, - // which corresponds to master mode but no trigger output on Pin 5. - // Master (mode = 1) activates trigger signal output on Pin 5. - // Slave (mode = 2) causes the realsense to listen to a trigger signal on pin 5. - - if(inter_cam_sync_mode_param == "default"){ _inter_cam_sync_mode = 0; } - else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = 1; } - else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = 2; } - else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = -1; } - else - { - _inter_cam_sync_mode = -1; - ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); - } - - _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); - if(_force_mavros_triggering && _inter_cam_sync_mode != 2) - { - ROS_WARN_STREAM("Force mavros triggering enabled but device not set to slave triggering mode!"); - } - - // set up mavros trigger if enabled - if(_force_mavros_triggering) - { - - // create callback for cached images - std::function&)> f_cb = [this](const stream_index_pair& channel, - const ros::Time& new_stamp, - const std::shared_ptr& cal){ - // fix stamps - cal->img->header.stamp = new_stamp; - cal->info.header.stamp = new_stamp; - - //publish - auto& info_publisher = this->_info_publisher.at(channel); - auto& image_publisher = this->_image_publishers.at(channel); - info_publisher.publish(cal->info); - - image_publisher.first.publish(cal->img); - image_publisher.second->update(); - }; - _trigger.setup(f_cb); - } - - _pnh.param("align_depth", _align_depth, ALIGN_DEPTH); _pnh.param("enable_pointcloud", _pointcloud, POINTCLOUD); std::string pc_texture_stream(""); @@ -262,6 +208,58 @@ void BaseRealSenseNode::getParameters() _pnh.param("aligned_depth_to_infra1_frame_id", _depth_aligned_frame_id[INFRA1], DEFAULT_ALIGNED_DEPTH_TO_INFRA1_FRAME_ID); _pnh.param("aligned_depth_to_infra2_frame_id", _depth_aligned_frame_id[INFRA2], DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID); _pnh.param("aligned_depth_to_fisheye_frame_id", _depth_aligned_frame_id[FISHEYE], DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID); + + std::string inter_cam_sync_mode_param; + _pnh.param("inter_cam_sync_mode", inter_cam_sync_mode_param, INTER_CAM_SYNC_MODE); + std::transform(inter_cam_sync_mode_param.begin(), inter_cam_sync_mode_param.end(), + inter_cam_sync_mode_param.begin(), ::tolower); + + // note: added a "none" mode, as not all sensor types/firmware versions allow setting of the sync mode. + // Use "none" if nothing is specified or an error occurs. + // Default (mode = 0) here refers to the default sync mode as per Intel whitepaper, + // which corresponds to master mode but no trigger output on Pin 5. + // Master (mode = 1) activates trigger signal output on Pin 5. + // Slave (mode = 2) causes the realsense to listen to a trigger signal on pin 5. + if(inter_cam_sync_mode_param == "default"){ _inter_cam_sync_mode = 0; } + else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = 1; } + else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = 2; } + else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = -1; } + else + { + _inter_cam_sync_mode = -1; + ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); + } + + _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); + if(_force_mavros_triggering && _inter_cam_sync_mode != 2) + { + ROS_WARN_STREAM("Force mavros triggering enabled but device not set to slave triggering mode!"); + } + + // set up mavros trigger if enabled + if(_force_mavros_triggering) + { + + // create callback for cached images + std::function&)> f_cb = [this](const stream_index_pair& channel, + const ros::Time& new_stamp, + const std::shared_ptr& cal){ + // fix stamps + cal->img->header.stamp = new_stamp; + cal->info.header.stamp = new_stamp; + + //publish + auto& info_publisher = this->_info_publisher.at(channel); + auto& image_publisher = this->_image_publishers.at(channel); + info_publisher.publish(cal->info); + + image_publisher.first.publish(cal->img); + image_publisher.second->update(); + }; + _trigger.setup(f_cb, static_cast(_fps[INFRA1])); + } } void BaseRealSenseNode::setupDevice() From 5fcc8234e56f60c650de40533c8babd470df76c0 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 17:02:25 +0200 Subject: [PATCH 23/41] Added timout in service waiter --- realsense2_camera/include/mavros_timestamp.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 327c6aa929..2e620609c3 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -45,8 +45,6 @@ // that is not const. This is due to the fact that many callbacks can happen simultaneously, especially // on callback based drivers such as the realsense. If not locked properly, this can lead to weird states. // Note that all private functions assume that they are called in a locked context (no cascading locks). - - namespace mavros_trigger { enum trigger_state { @@ -81,7 +79,6 @@ class MavrosTrigger { } cache_queue_type; - public: MavrosTrigger(const std::set &channel_set) : channel_set_(channel_set), @@ -104,7 +101,7 @@ class MavrosTrigger { nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); - ROS_DEBUG_STREAM(log_prefix_ << " Initialized with callback and framerate "<< framerate << " hz and subscribed to cam_imu_sub"); + ROS_DEBUG_STREAM(log_prefix_ << " Initialized with callback and framerate " << framerate << " hz and subscribed to cam_imu_sub"); } void start() { @@ -143,8 +140,13 @@ class MavrosTrigger { void waitMavros(){ ROS_INFO_STREAM("Waiting for mavros service...."); - ros::service::waitForService(kTriggerService); - ROS_INFO_STREAM("... mavros service ready."); + if(ros::service::waitForService(kTriggerService, ros::Duration(10))){ + ROS_INFO_STREAM("... mavros service ready."); + } else { + ROS_ERROR_STREAM("... mavros service wait timeout."); + // service disable is handled in start() function + } + } /* From f023429d350cc5478c7df4f1c9a15e160869b404 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Wed, 17 Oct 2018 17:02:53 +0200 Subject: [PATCH 24/41] Changes topic names to relative topics --- realsense2_camera/include/mavros_timestamp.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 2e620609c3..291810e396 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -58,8 +58,8 @@ template class MavrosTrigger { const std::string kDelayTopic = "delay"; - const std::string kCamImuSyncTopic = "mavros/cam_imu_sync/cam_imu_stamp"; - const std::string kTriggerService = "mavros/cmd/trigger_control"; + const std::string kCamImuSyncTopic = "cam_imu_stamp"; + const std::string kTriggerService = "trigger_control"; // callback definition for processing cached frames (with type t_cache). typedef boost::function Date: Wed, 17 Oct 2018 17:14:11 +0200 Subject: [PATCH 25/41] Added remap for topic, changed some INFO outputs to DEBUG --- realsense2_camera/include/mavros_timestamp.h | 6 +----- realsense2_camera/launch/includes/nodelet.launch.xml | 6 ++++++ realsense2_camera/launch/rs_camera.launch | 5 ++++- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 291810e396..a3a21dd227 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -154,10 +154,6 @@ class MavrosTrigger { * - Keeps sync alive if images are not processed (e.g. because not needed) */ void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp){ - if(channelValid(channel)){ - ROS_INFO_STREAM("addCameraSequence called"); - } - std::shared_ptr empty = {}; addCameraFrame(channel, camera_seq, camera_stamp, empty, 0.0); } @@ -194,7 +190,7 @@ class MavrosTrigger { // get trigger sequence for this camera frame const uint32_t trigger_seq = cameraToTriggerSeq(camera_seq); - ROS_INFO_STREAM(log_prefix_ << " Received camera frame for seq " << trigger_seq); + ROS_DEBUG_STREAM(log_prefix_ << " Received camera frame for seq " << trigger_seq); // get or create cache item for this trigger_seq cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 5bda2f6a29..ac27cb4425 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -51,6 +51,8 @@ + + @@ -75,9 +77,13 @@ + + + + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index d6eda470fc..174e58083d 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -42,7 +42,8 @@ - + + @@ -59,6 +60,8 @@ + + From ec6255997bfef1e3c3b268f249ef4f5e90bcf45d Mon Sep 17 00:00:00 2001 From: loon Date: Fri, 19 Oct 2018 17:17:57 +0200 Subject: [PATCH 26/41] Multiple synchronized channels and bugfixes for it --- realsense2_camera/include/mavros_timestamp.h | 11 ++++++++++- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index a3a21dd227..b3786f9124 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -288,7 +288,7 @@ class MavrosTrigger { state_ = ts_synced; for (auto ch_id : channel_set_) { - last_published_trigger_seq_[ch_id] = seq_trigger - 1; + last_published_trigger_seq_[ch_id] = seq_trigger; } ROS_INFO( @@ -376,6 +376,15 @@ class MavrosTrigger { } while(entry_released); + // cleanup old entries + for(auto it = cache_queue_[channel].cbegin(); it != cache_queue_[channel].cend();/*no inc++*/){ + if(it->first < last_published_trigger_seq_[channel]){ + cache_queue_[channel].erase(it++); + ROS_WARN_STREAM(log_prefix_ << "Removing old entries from cache_queue_ - shouldn't happen after startup."); + } else { + break; + } + } } ros::NodeHandle nh_; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 983eeec6ed..74944d2aac 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -21,7 +21,7 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _serial_no(serial_no), _base_frame_id(""), _intialize_time_base(false), _namespace(getNamespaceStr()), - _trigger(std::set({INFRA1})) + _trigger(std::set({INFRA1, DEPTH})) { // Types for depth stream _is_frame_arrived[DEPTH] = false; @@ -1457,7 +1457,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, const std::map& encoding, bool copy_data_from_frame) { - ROS_DEBUG("publishFrame(...)"); + ROS_INFO_STREAM("Publish Frame " << rs2_stream_to_string(f.get_profile().stream_type()) << " w t="<first < last_published_trigger_seq_[channel]){ + // remove all entries that cannot be published anymore cache_queue_[channel].erase(it++); ROS_WARN_STREAM(log_prefix_ << "Removing old entries from cache_queue_ - shouldn't happen after startup."); } else { break; } } + + // check cache size and issue warning if its large (larger than 1 second worth of messages) + // currently there is no bound on cache size in order to not loose anything, maybe change in the future + if(cache_queue_[channel].size() > static_cast(framerate_)){ + ROS_WARN_STREAM(log_prefix_ << "Cache queue too large (" << cache_queue_[channel].size() << " entries)"); + } } ros::NodeHandle nh_; From ab19f41ad7fc4b44a20df701593ac5f37f461cb3 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Fri, 19 Oct 2018 18:01:35 +0200 Subject: [PATCH 28/41] Gitignore stuff --- realsense2_camera/.gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/.gitignore b/realsense2_camera/.gitignore index 01e00f3af8..12c876404e 100644 --- a/realsense2_camera/.gitignore +++ b/realsense2_camera/.gitignore @@ -1 +1,2 @@ CMakeLists.txt.user +cmake-build-debug/* From 7fe3760195a3aab1ecf4f633a33626afa7fc6ae1 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Fri, 19 Oct 2018 18:01:45 +0200 Subject: [PATCH 29/41] Warnings, wrong casts etc --- realsense2_camera/include/mavros_timestamp.h | 57 +++++++------------ realsense2_camera/src/base_realsense_node.cpp | 4 +- 2 files changed, 24 insertions(+), 37 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index b0f0c4ce33..afba77a8c6 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -60,6 +60,7 @@ class MavrosTrigger { const std::string kDelayTopic = "delay"; const std::string kCamImuSyncTopic = "cam_imu_stamp"; const std::string kTriggerService = "trigger_control"; + const std::string kLogPrefix = "[Mavros Triggering] "; // callback definition for processing cached frames (with type t_cache). typedef boost::function &channel_set) : channel_set_(channel_set), state_(ts_not_initalized) { - ROS_DEBUG_STREAM(log_prefix_ << " Initialized with " << channel_set_.size() << " channels."); + ROS_DEBUG_STREAM(kLogPrefix << " Initialized with " << channel_set_.size() << " channels."); for (t_channel_id id : channel_set_) { cache_queue_[id].clear(); } @@ -101,7 +101,7 @@ class MavrosTrigger { nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); - ROS_DEBUG_STREAM(log_prefix_ << " Initialized with callback and framerate " << framerate << " hz and subscribed to cam_imu_sub"); + ROS_DEBUG_STREAM(kLogPrefix << " Initialized with callback and framerate " << framerate << " hz and subscribed to cam_imu_sub"); } void start() { @@ -112,7 +112,7 @@ class MavrosTrigger { return; } - ROS_INFO_STREAM(log_prefix_ << " Started triggering."); + ROS_INFO_STREAM(kLogPrefix << " Started triggering."); for (t_channel_id id : channel_set_) { cache_queue_[id].clear(); @@ -163,12 +163,11 @@ class MavrosTrigger { */ void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ - ros::spinOnce(); std::lock_guard lg(mutex_); if (!channelValid(channel)) { - ROS_WARN_STREAM_ONCE(log_prefix_ << "Unsynchronized channe - ignoring frame."); + ROS_WARN_STREAM_ONCE(kLogPrefix << "Unsynchronized channe - ignoring frame."); return; } @@ -180,16 +179,14 @@ class MavrosTrigger { // Ignore frame, we are not synced yet by now if (state_ != ts_synced) { - ROS_WARN_STREAM(log_prefix_ << " Trigger not synced yet - ignoring frame."); + ROS_WARN_STREAM(kLogPrefix << " Trigger not synced yet - ignoring frame."); return; } // add frame to cache - // get trigger sequence for this camera frame const uint32_t trigger_seq = cameraToTriggerSeq(camera_seq); - - ROS_DEBUG_STREAM(log_prefix_ << " Received camera frame for seq " << trigger_seq); + ROS_DEBUG_STREAM(kLogPrefix << " Received camera frame for seq " << trigger_seq); // get or create cache item for this trigger_seq cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; @@ -232,7 +229,7 @@ class MavrosTrigger { } ROS_DEBUG( - "[Cam Imu Sync] Received a new stamp for sequence number: %ld (%ld) with " + "[Cam Imu Sync] Received a new stamp for sequence number: %i (%i) with " "stamp: %f", cam_imu_stamp.frame_seq_id, cam_imu_stamp.frame_seq_id - trigger_sequence_offset_, @@ -245,7 +242,6 @@ class MavrosTrigger { } private: - bool channelValid(const t_channel_id &channel) const { return channel_set_.count(channel) == 1; } @@ -255,14 +251,14 @@ class MavrosTrigger { */ bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { if(cache_queue_[channel].empty()){ - ROS_WARN_STREAM_ONCE(log_prefix_ << " syncOffset called with empty queue"); + ROS_WARN_STREAM_ONCE(kLogPrefix << " syncOffset called with empty queue"); return false; } // Get the first from the sequence time map. auto it = cache_queue_[channel].begin(); if(!it->second.trigger_info){ - ROS_ERROR_STREAM(log_prefix_ << "First queue entry without trigger info - should never happen"); + ROS_ERROR_STREAM(kLogPrefix << "First queue entry without trigger info - should never happen"); return false; } @@ -272,28 +268,27 @@ class MavrosTrigger { // Get offset between first frame sequence and mavros trigger_sequence_offset_ = static_cast(seq_trigger) - static_cast(seq_camera); - double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); // Check for divergence (too large delay) double delay_bounds = 1.0/framerate_; if (std::abs(delay) > delay_bounds) { - ROS_ERROR_STREAM(log_prefix_ << "Delay out of bounds: " << delay << ", bounds are +/-" << delay_bounds << " s"); + ROS_ERROR_STREAM(kLogPrefix << "Delay out of bounds: " << delay << ", bounds are +/-" << delay_bounds << " s"); state_ = ts_not_initalized; return false; } + // if we survived until here, node is synchronized state_ = ts_synced; - for (auto ch_id : channel_set_) { last_published_trigger_seq_[ch_id] = seq_trigger; } ROS_INFO( - "%s New header offset determined by channel %i: %d, from %d to %d, timestamp " + "%s New header offset determined by channel %ld: %d, from %d to %d, timestamp " "correction: %f seconds.", - log_prefix_, channel, + kLogPrefix.c_str(), distance(channel_set_.begin(),channel_set_.find(channel)), trigger_sequence_offset_, it->first, seq_camera, delay); return true; @@ -363,24 +358,24 @@ class MavrosTrigger { msg.point.z = seq_trigger; delay_pub_.publish(msg); - ROS_DEBUG_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); + ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); // cleanup last_published_trigger_seq_[channel]++; cache_queue_[channel].erase(it_next_sequence); } else { - ROS_DEBUG_STREAM(log_prefix_ << "|cache|= " << cache_queue_[channel].size() << ". "); + ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() << ". "); } } while(entry_released); // cleanup old entries - warning - special structure of for-loop needed to traverse while removing. for(auto it = cache_queue_[channel].cbegin(); it != cache_queue_[channel].cend();/*no inc++*/){ - if(it->first < last_published_trigger_seq_[channel]){ + if(static_cast(it->first) < last_published_trigger_seq_[channel]){ // remove all entries that cannot be published anymore cache_queue_[channel].erase(it++); - ROS_WARN_STREAM(log_prefix_ << "Removing old entries from cache_queue_ - shouldn't happen after startup."); + ROS_WARN_STREAM(kLogPrefix << "Removing old entries from cache_queue_ - shouldn't happen after startup."); } else { break; } @@ -388,31 +383,23 @@ class MavrosTrigger { // check cache size and issue warning if its large (larger than 1 second worth of messages) // currently there is no bound on cache size in order to not loose anything, maybe change in the future - if(cache_queue_[channel].size() > static_cast(framerate_)){ - ROS_WARN_STREAM(log_prefix_ << "Cache queue too large (" << cache_queue_[channel].size() << " entries)"); + if(cache_queue_[channel].size() > static_cast(framerate_)){ + ROS_WARN_STREAM(kLogPrefix << "Cache queue too large (" << cache_queue_[channel].size() << " entries)"); } } ros::NodeHandle nh_; - - const std::set channel_set_; - int trigger_sequence_offset_ = 0; - ros::Subscriber cam_imu_sub_; ros::Publisher delay_pub_; + const std::set channel_set_; + int trigger_sequence_offset_ = 0; std::mutex mutex_; caching_callback callback_; trigger_state state_; - std::map> cache_queue_; std::map last_published_trigger_seq_; double framerate_; // [hz] - - const std::string log_prefix_ = "[Mavros Triggering] "; - }; - } - #endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 74944d2aac..dbf6b88950 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -20,8 +20,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _pnh(privateNodeHandle), _json_file_path(""), _serial_no(serial_no), _base_frame_id(""), _intialize_time_base(false), - _namespace(getNamespaceStr()), - _trigger(std::set({INFRA1, DEPTH})) + _trigger(std::set({INFRA1, DEPTH})), + _namespace(getNamespaceStr()) { // Types for depth stream _is_frame_arrived[DEPTH] = false; From f6574de54f6297889d08471a8b20a913e01009ee Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Fri, 19 Oct 2018 18:09:21 +0200 Subject: [PATCH 30/41] Set frame-based logs to debug --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index dbf6b88950..7fdebd22fc 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1457,7 +1457,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, const std::map& encoding, bool copy_data_from_frame) { - ROS_INFO_STREAM("Publish Frame " << rs2_stream_to_string(f.get_profile().stream_type()) << " w t="< - - + + diff --git a/realsense2_camera/launch/rs_mavros.launch b/realsense2_camera/launch/rs_mavros.launch new file mode 100644 index 0000000000..9806d7f869 --- /dev/null +++ b/realsense2_camera/launch/rs_mavros.launch @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 7fdebd22fc..9f9b4c11a9 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1497,7 +1497,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, // send frame off to trigger handler for caching and later release through callback // if mavros triggering is activated - if(_force_mavros_triggering) + if(_force_mavros_triggering && _trigger.supportsChannel(stream)) { // if exposure is available, supply it to trigger handler double exposure = f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? @@ -1521,7 +1521,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, image_publisher.second->update(); ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); } - else if(!has_subscribers && _force_mavros_triggering) + else if(!has_subscribers && _force_mavros_triggering && _trigger.supportsChannel(stream)) { // if there are no subscribers, but mavros triggering is activated, // we still need to signal frame sequences to the trigger handler From 79e3c5d8a2583e566d75ee8194d7976523226ebb Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Mon, 22 Oct 2018 15:54:14 +0200 Subject: [PATCH 32/41] Changes for higher resolution + trigger time approximation for non synced frames --- realsense2_camera/include/mavros_timestamp.h | 23 +++++++++++++++---- realsense2_camera/src/base_realsense_node.cpp | 8 +++++++ 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 70108ca91a..1e3f33649c 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -62,6 +62,7 @@ class MavrosTrigger { const std::string kTriggerService = "trigger_control"; const std::string kLogPrefix = "[Mavros Triggering] "; + // callback definition for processing cached frames (with type t_cache). typedef boost::function &channel_set) : + allow_shifted_trigger_seq_(true), channel_set_(channel_set), state_(ts_not_initalized) { ROS_DEBUG_STREAM(kLogPrefix << " Initialized with " << channel_set_.size() << " channels."); @@ -247,6 +249,10 @@ class MavrosTrigger { // internal channelValid in future - therefore seperate interface. } + void approximateTriggerTime(const ros::Time& camera_time, ros::Time* trigger_time){ + *trigger_time = camera_time + ros::Duration(last_delay_); + } + private: bool channelValid(const t_channel_id &channel) const { return channel_set_.count(channel) == 1; @@ -274,12 +280,18 @@ class MavrosTrigger { // Get offset between first frame sequence and mavros trigger_sequence_offset_ = static_cast(seq_trigger) - static_cast(seq_camera); - double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); + double delay = stamp_camera.toSec()- 1.0/29.94f - it->second.stamp_trigger.toSec(); // Check for divergence (too large delay) double delay_bounds = 1.0/framerate_; - if (std::abs(delay) > delay_bounds*1.5f) { + if(std::abs(delay) > delay_bounds && + std::abs(delay-1.0/29.94) <= delay_bounds + && allow_shifted_trigger_seq_) + { + ROS_INFO_STREAM(kLogPrefix << "Performing init with shifted trigger_sequence_offset!"); + trigger_sequence_offset_+=1; + } else if (std::abs(delay) > delay_bounds) { ROS_ERROR_STREAM(kLogPrefix << "Delay out of bounds: " << delay << ", bounds are +/-" << delay_bounds << " s"); state_ = ts_not_initalized; return false; @@ -356,10 +368,11 @@ class MavrosTrigger { callback_(channel, midpoint_exposure, entry.frame); } // output delay message and log - double delay = midpoint_exposure.toSec() - entry.stamp_camera.toSec(); + last_delay_ = midpoint_exposure.toSec() - entry.stamp_camera.toSec(); + geometry_msgs::PointStamped msg; msg.header.stamp = midpoint_exposure; - msg.point.x = delay; + msg.point.x = last_delay_; msg.point.y = entry.seq_camera; msg.point.z = seq_trigger; delay_pub_.publish(msg); @@ -398,6 +411,7 @@ class MavrosTrigger { ros::Subscriber cam_imu_sub_; ros::Publisher delay_pub_; + const bool allow_shifted_trigger_seq_; const std::set channel_set_; int trigger_sequence_offset_ = 0; std::mutex mutex_; @@ -406,6 +420,7 @@ class MavrosTrigger { std::map> cache_queue_; std::map last_published_trigger_seq_; double framerate_; // [hz] + double last_delay_; // [s] }; } #endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9f9b4c11a9..de41165b72 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1513,6 +1513,14 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, _trigger.addCameraFrame(stream, seq[stream], t, frame, exposure); return; } + else if(_force_mavros_triggering){ + /* approximate trigger time */ + ros::Time trigger_time; + _trigger.approximateTriggerTime(cam_info.header.stamp, &trigger_time); + + cam_info.header.stamp = trigger_time; + frame->info.header.stamp = trigger_time; + } // if we're not using mavros trigger, directly publish info_publisher.publish(cam_info); From 46f9249225390477bf9097553826e788eb5bef24 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Mon, 22 Oct 2018 15:57:40 +0200 Subject: [PATCH 33/41] build fixes --- realsense2_camera/include/mavros_timestamp.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 1e3f33649c..7e2b1e84c7 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -377,7 +377,7 @@ class MavrosTrigger { msg.point.z = seq_trigger; delay_pub_.publish(msg); - ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << delay); + ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << last_delay_); // cleanup last_published_trigger_seq_[channel]++; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index de41165b72..c333c5d0c1 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1519,7 +1519,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, _trigger.approximateTriggerTime(cam_info.header.stamp, &trigger_time); cam_info.header.stamp = trigger_time; - frame->info.header.stamp = trigger_time; + img->header.stamp = trigger_time; } // if we're not using mavros trigger, directly publish From 83d27d9c2851ba61e9103d9e748d23ac96bf1f10 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Thu, 25 Oct 2018 11:14:51 +0200 Subject: [PATCH 34/41] Fixes --- realsense2_camera/include/mavros_timestamp.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 7e2b1e84c7..0dd60d178a 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -81,8 +81,8 @@ class MavrosTrigger { } cache_queue_type; public: - MavrosTrigger(const std::set &channel_set) : - allow_shifted_trigger_seq_(true), + MavrosTrigger(const std::set &channel_set, bool allow_shifted_trigger_seq = true) : + allow_shifted_trigger_seq_(allow_shifted_trigger_seq), channel_set_(channel_set), state_(ts_not_initalized) { ROS_DEBUG_STREAM(kLogPrefix << " Initialized with " << channel_set_.size() << " channels."); @@ -280,13 +280,13 @@ class MavrosTrigger { // Get offset between first frame sequence and mavros trigger_sequence_offset_ = static_cast(seq_trigger) - static_cast(seq_camera); - double delay = stamp_camera.toSec()- 1.0/29.94f - it->second.stamp_trigger.toSec(); + double delay = stamp_camera.toSec()- it->second.stamp_trigger.toSec(); // Check for divergence (too large delay) double delay_bounds = 1.0/framerate_; if(std::abs(delay) > delay_bounds && - std::abs(delay-1.0/29.94) <= delay_bounds + std::abs(delay - delay_bounds) <= delay_bounds && allow_shifted_trigger_seq_) { ROS_INFO_STREAM(kLogPrefix << "Performing init with shifted trigger_sequence_offset!"); From ff0f09bd9204bcfa6f36d2fe598ce840a3baebb7 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Thu, 25 Oct 2018 13:29:40 +0200 Subject: [PATCH 35/41] Static timeshift correction --- realsense2_camera/include/mavros_timestamp.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index 0dd60d178a..e27c782e06 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -314,7 +314,7 @@ class MavrosTrigger { ros::Time shiftTimestampToMidExposure(const ros::Time &stamp, const double exposure_us) const { - ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) - ros::Duration(3.75 * 1e-3); + ros::Time new_stamp = stamp + ros::Duration(exposure_us * 1e-6 / 2.0) + ros::Duration(static_time_shift_); return new_stamp; } @@ -421,6 +421,7 @@ class MavrosTrigger { std::map last_published_trigger_seq_; double framerate_; // [hz] double last_delay_; // [s] + double static_time_shift_ = 10.7*1e-3; //[s] }; } #endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H From 4ef0d136a2cad0fe17925dc8ee405ca1508f74f6 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Thu, 25 Oct 2018 18:05:48 +0200 Subject: [PATCH 36/41] Added timeshift as parameter (resolution etc dependent) --- realsense2_camera/include/mavros_timestamp.h | 15 ++++++++++----- .../launch/includes/nodelet.launch.xml | 2 ++ realsense2_camera/launch/rs_camera.launch | 2 ++ realsense2_camera/src/base_realsense_node.cpp | 9 ++++++++- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index e27c782e06..c8e247cdad 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -91,7 +91,7 @@ class MavrosTrigger { } } - void setup(const caching_callback &callback, double framerate) { + void setup(const caching_callback &callback, const double framerate, const double static_time_shift = 0.0) { std::lock_guard lg(mutex_); trigger_sequence_offset_ = 0; @@ -99,11 +99,16 @@ class MavrosTrigger { state_ = ts_not_initalized; framerate_ = framerate; // approximate framerate to do outlier checking callback_ = callback; + static_time_shift_ = static_time_shift; // constant shift for bus transfers, sync-signal weirdness etc. + // Values for realsense infra channel: + // +10*1e-3 for 640x480, -6*1e-3 for 1280x720 + cam_imu_sub_ = nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); - ROS_DEBUG_STREAM(kLogPrefix << " Initialized with callback and framerate " << framerate << " hz and subscribed to cam_imu_sub"); + ROS_DEBUG_STREAM(kLogPrefix << " Initialized with framerate " << framerate << + " hz, static time offset "<< static_time_shift_<< " and subscribed to cam_imu_sub"); } void start() { @@ -419,9 +424,9 @@ class MavrosTrigger { trigger_state state_; std::map> cache_queue_; std::map last_published_trigger_seq_; - double framerate_; // [hz] - double last_delay_; // [s] - double static_time_shift_ = 10.7*1e-3; //[s] + double framerate_ = 30.0; // [hz] + double last_delay_ = 0.0; // [s] + double static_time_shift_ = 0.0; //[s] }; } #endif //REALSENSE2_CAMERA_MAVROS_TIMESTAMP_H diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 1be0e2746b..129e7055ba 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -50,6 +50,7 @@ + @@ -95,6 +96,7 @@ + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 174e58083d..13d3eaab49 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -41,6 +41,7 @@ + @@ -59,6 +60,7 @@ + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index c333c5d0c1..9c310dbfb5 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -211,6 +211,8 @@ void BaseRealSenseNode::getParameters() std::string inter_cam_sync_mode_param; _pnh.param("inter_cam_sync_mode", inter_cam_sync_mode_param, INTER_CAM_SYNC_MODE); + + std::transform(inter_cam_sync_mode_param.begin(), inter_cam_sync_mode_param.end(), inter_cam_sync_mode_param.begin(), ::tolower); @@ -230,6 +232,7 @@ void BaseRealSenseNode::getParameters() ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); } + _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); if(_force_mavros_triggering && _inter_cam_sync_mode != 2) { @@ -258,7 +261,11 @@ void BaseRealSenseNode::getParameters() image_publisher.first.publish(cal->img); image_publisher.second->update(); }; - _trigger.setup(f_cb, static_cast(_fps[INFRA1])); + + double mavros_static_time_shift; + _pnh.param("mavros_static_time_shift", mavros_static_time_shift, 0.0); + + _trigger.setup(f_cb, static_cast(_fps[INFRA1]), mavros_static_time_shift); } } From b97e3c778f4d65c3f97ac4d9ef1a4082ad48ba56 Mon Sep 17 00:00:00 2001 From: loon Date: Thu, 25 Oct 2018 18:26:54 +0200 Subject: [PATCH 37/41] increased logging --- realsense2_camera/include/mavros_timestamp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index c8e247cdad..b46a909686 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -107,7 +107,7 @@ class MavrosTrigger { nh_.subscribe(kCamImuSyncTopic, 100, &MavrosTrigger::camImuStampCallback, this); - ROS_DEBUG_STREAM(kLogPrefix << " Initialized with framerate " << framerate << + ROS_INFO_STREAM(kLogPrefix << " Initialized with framerate " << framerate << " hz, static time offset "<< static_time_shift_<< " and subscribed to cam_imu_sub"); } From 43142d3f7f1852f64318b6ca6b824d840f41f1ae Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Tue, 30 Oct 2018 14:40:51 +0100 Subject: [PATCH 38/41] Changed enum to enum class, addressed comments, autoformater --- realsense2_camera/include/mavros_timestamp.h | 106 ++++++++++--------- 1 file changed, 54 insertions(+), 52 deletions(-) diff --git a/realsense2_camera/include/mavros_timestamp.h b/realsense2_camera/include/mavros_timestamp.h index c8e247cdad..39f68b26ac 100644 --- a/realsense2_camera/include/mavros_timestamp.h +++ b/realsense2_camera/include/mavros_timestamp.h @@ -47,11 +47,11 @@ // Note that all private functions assume that they are called in a locked context (no cascading locks). namespace mavros_trigger { -enum trigger_state { - ts_synced = 1, // Sequence offset synced between cam/trigger - ts_not_initalized, - ts_wait_for_sync, // MAVROS reset sent, but no image/timestamps correlated yet. - ts_disabled +enum class TriggerState { + kSynced = 0, // Sequence offset synced between cam/trigger + kNotInitialized, + kWaitForSync, // MAVROS reset sent, but no image/timestamps correlated yet. + kDisabled }; template @@ -62,7 +62,6 @@ class MavrosTrigger { const std::string kTriggerService = "trigger_control"; const std::string kLogPrefix = "[Mavros Triggering] "; - // callback definition for processing cached frames (with type t_cache). typedef boost::function &channel_set, bool allow_shifted_trigger_seq = true) : allow_shifted_trigger_seq_(allow_shifted_trigger_seq), channel_set_(channel_set), - state_(ts_not_initalized) { + state_(TriggerState::kNotInitialized) { ROS_DEBUG_STREAM(kLogPrefix << " Initialized with " << channel_set_.size() << " channels."); for (t_channel_id id : channel_set_) { cache_queue_[id].clear(); @@ -96,7 +95,7 @@ class MavrosTrigger { trigger_sequence_offset_ = 0; delay_pub_ = nh_.advertise(kDelayTopic, 100); - state_ = ts_not_initalized; + state_ = TriggerState::kNotInitialized; framerate_ = framerate; // approximate framerate to do outlier checking callback_ = callback; static_time_shift_ = static_time_shift; // constant shift for bus transfers, sync-signal weirdness etc. @@ -108,13 +107,13 @@ class MavrosTrigger { &MavrosTrigger::camImuStampCallback, this); ROS_DEBUG_STREAM(kLogPrefix << " Initialized with framerate " << framerate << - " hz, static time offset "<< static_time_shift_<< " and subscribed to cam_imu_sub"); + " hz, static time offset " << static_time_shift_ << " and subscribed to cam_imu_sub"); } void start() { std::lock_guard lg(mutex_); - if (state_ != ts_not_initalized) { + if (state_ != TriggerState::kNotInitialized) { //already started, nothing to do return; } @@ -136,18 +135,18 @@ class MavrosTrigger { ros::service::call(kTriggerService, req); ROS_DEBUG("Called mavros trigger service! Success? %d Result? %d", - req.response.success, req.response.result); - state_ = ts_wait_for_sync; + req.response.success, req.response.result); + state_ = TriggerState::kWaitForSync; } else { ROS_ERROR("Mavros service not available!"); - state_ = ts_disabled; + state_ = TriggerState::kDisabled; } } - void waitMavros(){ + void waitMavros() { ROS_INFO_STREAM("Waiting for mavros service...."); - if(ros::service::waitForService(kTriggerService, ros::Duration(10))){ + if (ros::service::waitForService(kTriggerService, ros::Duration(10))) { ROS_INFO_STREAM("... mavros service ready."); } else { ROS_ERROR_STREAM("... mavros service wait timeout."); @@ -159,7 +158,7 @@ class MavrosTrigger { * Adds a timestamp and sequence number of the camera without caching/publishing the image * - Keeps sync alive if images are not processed (e.g. because not needed) */ - void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp){ + void addCameraSequence(const t_channel_id &channel, const uint32_t camera_seq, const ros::Time &camera_stamp) { std::shared_ptr empty = {}; addCameraFrame(channel, camera_seq, camera_stamp, empty, 0.0); } @@ -169,8 +168,11 @@ class MavrosTrigger { * - Caches t_cache frame pointer until corresponding trigger time message from mavros is received */ void addCameraFrame(const t_channel_id &channel, const uint32_t camera_seq, - const ros::Time &camera_stamp, const std::shared_ptr& frame, const double exposure){ + const ros::Time &camera_stamp, const std::shared_ptr &frame, const double exposure) { + + // allow threading subsystem to check for new IMU messages right before we lock. ros::spinOnce(); + std::lock_guard lg(mutex_); if (!channelValid(channel)) { @@ -178,14 +180,14 @@ class MavrosTrigger { return; } - if (state_ == ts_wait_for_sync) { + if (state_ == TriggerState::kWaitForSync) { if (!syncOffset(channel, camera_seq, camera_stamp)) { return; } } // Ignore frame, we are not synced yet by now - if (state_ != ts_synced) { + if (state_ != TriggerState::kSynced) { ROS_WARN_STREAM(kLogPrefix << " Trigger not synced yet - ignoring frame."); return; } @@ -196,10 +198,9 @@ class MavrosTrigger { ROS_DEBUG_STREAM(kLogPrefix << " Received camera frame for seq " << trigger_seq); // get or create cache item for this trigger_seq - cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; + cache_queue_type &cache_entry = cache_queue_[channel][trigger_seq]; - if(!cache_entry.camera_info) - { + if (!cache_entry.camera_info) { // complete information if needed cache_entry.camera_info = true; cache_entry.seq_camera = camera_seq; @@ -217,14 +218,14 @@ class MavrosTrigger { void camImuStampCallback(const mavros_msgs::CamIMUStamp &cam_imu_stamp) { std::lock_guard lg(mutex_); - if (state_ == ts_not_initalized) { + if (state_ == TriggerState::kNotInitialized) { // Ignore stuff from before we *officially* start the triggering. // The triggering is techncially always running but... return; } //ignore messages until they wrap back - we should receive a 0 eventually. - if (state_ == ts_wait_for_sync && cam_imu_stamp.frame_seq_id != 0) { + if (state_ == TriggerState::kWaitForSync && cam_imu_stamp.frame_seq_id != 0) { ROS_WARN("[Cam Imu Sync] Ignoring old messages, clearing queues"); return; } @@ -254,7 +255,7 @@ class MavrosTrigger { // internal channelValid in future - therefore seperate interface. } - void approximateTriggerTime(const ros::Time& camera_time, ros::Time* trigger_time){ + void approximateTriggerTime(const ros::Time &camera_time, ros::Time *trigger_time) { *trigger_time = camera_time + ros::Duration(last_delay_); } @@ -267,14 +268,14 @@ class MavrosTrigger { * Tries to determine offset between camera and trigger/imu and returns true if succeeded */ bool syncOffset(const t_channel_id &channel, const uint32_t seq_camera, const ros::Time &stamp_camera) { - if(cache_queue_[channel].empty()){ + if (cache_queue_[channel].empty()) { ROS_WARN_STREAM_ONCE(kLogPrefix << " syncOffset called with empty queue"); return false; } // Get the first from the sequence time map. auto it = cache_queue_[channel].begin(); - if(!it->second.trigger_info){ + if (!it->second.trigger_info) { ROS_ERROR_STREAM(kLogPrefix << "First queue entry without trigger info - should never happen"); return false; } @@ -285,25 +286,24 @@ class MavrosTrigger { // Get offset between first frame sequence and mavros trigger_sequence_offset_ = static_cast(seq_trigger) - static_cast(seq_camera); - double delay = stamp_camera.toSec()- it->second.stamp_trigger.toSec(); + double delay = stamp_camera.toSec() - it->second.stamp_trigger.toSec(); // Check for divergence (too large delay) - double delay_bounds = 1.0/framerate_; + double delay_bounds = 1.0 / framerate_; - if(std::abs(delay) > delay_bounds && - std::abs(delay - delay_bounds) <= delay_bounds - && allow_shifted_trigger_seq_) - { + if (std::abs(delay) > delay_bounds && + std::abs(delay - delay_bounds) <= delay_bounds + && allow_shifted_trigger_seq_) { ROS_INFO_STREAM(kLogPrefix << "Performing init with shifted trigger_sequence_offset!"); - trigger_sequence_offset_+=1; + trigger_sequence_offset_ += 1; } else if (std::abs(delay) > delay_bounds) { ROS_ERROR_STREAM(kLogPrefix << "Delay out of bounds: " << delay << ", bounds are +/-" << delay_bounds << " s"); - state_ = ts_not_initalized; + state_ = TriggerState::kNotInitialized; return false; } // if we survived until here, node is synchronized - state_ = ts_synced; + state_ = TriggerState::kSynced; for (auto ch_id : channel_set_) { last_published_trigger_seq_[ch_id] = seq_trigger; } @@ -311,7 +311,7 @@ class MavrosTrigger { ROS_INFO( "%s New header offset determined by channel %ld: %d, from %d to %d, timestamp " "correction: %f seconds.", - kLogPrefix.c_str(), distance(channel_set_.begin(),channel_set_.find(channel)), + kLogPrefix.c_str(), distance(channel_set_.begin(), channel_set_.find(channel)), trigger_sequence_offset_, it->first, seq_camera, delay); return true; @@ -323,17 +323,17 @@ class MavrosTrigger { return new_stamp; } - inline uint32_t cameraToTriggerSeq(const uint32_t camera_seq){ + inline uint32_t cameraToTriggerSeq(const uint32_t camera_seq) { return camera_seq + trigger_sequence_offset_; } - void processTriggerMessage(const t_channel_id& channel, const uint32_t trigger_seq, const ros::Time& trigger_time){ + void processTriggerMessage(const t_channel_id &channel, const uint32_t trigger_seq, const ros::Time &trigger_time) { // get or create cache item for this trigger_seq - cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; + cache_queue_type &cache_entry = cache_queue_[channel][trigger_seq]; - if(!cache_entry.trigger_info){ + if (!cache_entry.trigger_info) { // complete information if needed - cache_queue_type& cache_entry = cache_queue_[channel][trigger_seq]; + cache_queue_type &cache_entry = cache_queue_[channel][trigger_seq]; cache_entry.trigger_info = true; cache_entry.stamp_trigger = trigger_time; } @@ -345,7 +345,7 @@ class MavrosTrigger { * - have sequence_id = last_published_trigger_seq_ + 1 ( so we only publish in order) * Runs as long as messages are available that satisfy this. */ - void releaseCacheEntries(const t_channel_id& channel){ + void releaseCacheEntries(const t_channel_id &channel) { bool entry_released = false; // release all entries that are complete. @@ -359,17 +359,17 @@ class MavrosTrigger { entry_released = (it_next_sequence != cache_queue_[channel].end()) && (it_next_sequence->second.camera_info && it_next_sequence->second.trigger_info); - if(entry_released) { + if (entry_released) { auto entry = it_next_sequence->second; // adjust timestamp ros::Time midpoint_exposure = shiftTimestampToMidExposure(entry.stamp_trigger, - entry.exposure); + entry.exposure); // callback to publish if there is a cached frame // - sometimes we add camera sequences without cached frames, e.g. if there are no subscribers // - e.g. by calling addCameraSequence - if(entry.frame != nullptr) { + if (entry.frame != nullptr) { callback_(channel, midpoint_exposure, entry.frame); } // output delay message and log @@ -382,7 +382,9 @@ class MavrosTrigger { msg.point.z = seq_trigger; delay_pub_.publish(msg); - ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera << ", seq_t=" << seq_trigger << " released w t_diff=" << last_delay_); + ROS_DEBUG_STREAM( + kLogPrefix << "|cache|= " << cache_queue_[channel].size() - 1 << " Frame w seq_c=" << entry.seq_camera + << ", seq_t=" << seq_trigger << " released w t_diff=" << last_delay_); // cleanup last_published_trigger_seq_[channel]++; @@ -392,11 +394,11 @@ class MavrosTrigger { ROS_DEBUG_STREAM(kLogPrefix << "|cache|= " << cache_queue_[channel].size() << ". "); } - } while(entry_released); + } while (entry_released); // cleanup old entries - warning - special structure of for-loop needed to traverse while removing. - for(auto it = cache_queue_[channel].cbegin(); it != cache_queue_[channel].cend();/*no inc++*/){ - if(static_cast(it->first) < last_published_trigger_seq_[channel]){ + for (auto it = cache_queue_[channel].cbegin(); it != cache_queue_[channel].cend();/*no inc++*/) { + if (static_cast(it->first) < last_published_trigger_seq_[channel]) { // remove all entries that cannot be published anymore cache_queue_[channel].erase(it++); ROS_WARN_STREAM(kLogPrefix << "Removing old entries from cache_queue_ - shouldn't happen after startup."); @@ -407,7 +409,7 @@ class MavrosTrigger { // check cache size and issue warning if its large (larger than 1 second worth of messages) // currently there is no bound on cache size in order to not loose anything, maybe change in the future - if(cache_queue_[channel].size() > static_cast(framerate_)){ + if (cache_queue_[channel].size() > static_cast(framerate_)) { ROS_WARN_STREAM(kLogPrefix << "Cache queue too large (" << cache_queue_[channel].size() << " entries)"); } } @@ -421,7 +423,7 @@ class MavrosTrigger { int trigger_sequence_offset_ = 0; std::mutex mutex_; caching_callback callback_; - trigger_state state_; + TriggerState state_; std::map> cache_queue_; std::map last_published_trigger_seq_; double framerate_ = 30.0; // [hz] From 5fa134b9dafbdfc8b913e318350b0a8091937f7c Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Tue, 30 Oct 2018 14:53:55 +0100 Subject: [PATCH 39/41] Added enum for inter_cam_sync_mode --- realsense2_camera/include/base_realsense_node.h | 10 +++++++++- realsense2_camera/src/base_realsense_node.cpp | 14 +++++++------- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index f09c9da1d0..d73b6af844 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -29,6 +29,14 @@ namespace realsense2_camera base_depth_count }; + // not using enum class here for consistence with realsense code. + enum inter_cam_sync_mode{ + inter_cam_sync_default = 0, + inter_cam_sync_master = 1, + inter_cam_sync_slave = 2, + inter_cam_sync_none = -1 + }; + struct FrequencyDiagnostics { FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) : @@ -179,7 +187,7 @@ namespace realsense2_camera std::map _camera_info; bool _intialize_time_base; double _camera_time_base; - int _inter_cam_sync_mode; + inter_cam_sync_mode _inter_cam_sync_mode; bool _force_mavros_triggering; typedef struct{ sensor_msgs::ImagePtr img; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9c310dbfb5..6d385d5ab0 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -222,19 +222,19 @@ void BaseRealSenseNode::getParameters() // which corresponds to master mode but no trigger output on Pin 5. // Master (mode = 1) activates trigger signal output on Pin 5. // Slave (mode = 2) causes the realsense to listen to a trigger signal on pin 5. - if(inter_cam_sync_mode_param == "default"){ _inter_cam_sync_mode = 0; } - else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = 1; } - else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = 2; } - else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = -1; } + if(inter_cam_sync_mode_param == "default"){ _inter_cam_sync_mode = inter_cam_sync_default; } + else if(inter_cam_sync_mode_param == "master") { _inter_cam_sync_mode = inter_cam_sync_master; } + else if(inter_cam_sync_mode_param == "slave"){ _inter_cam_sync_mode = inter_cam_sync_slave; } + else if(inter_cam_sync_mode_param == "none") { _inter_cam_sync_mode = inter_cam_sync_none; } else { - _inter_cam_sync_mode = -1; + _inter_cam_sync_mode = inter_cam_sync_none; ROS_WARN_STREAM("Invalid inter cam sync mode (" << inter_cam_sync_mode_param << ")! Not using inter cam sync mode."); } _pnh.param("force_mavros_triggering", _force_mavros_triggering, FORCE_MAVROS_TRIGGERING); - if(_force_mavros_triggering && _inter_cam_sync_mode != 2) + if(_force_mavros_triggering && _inter_cam_sync_mode != inter_cam_sync_slave) { ROS_WARN_STREAM("Force mavros triggering enabled but device not set to slave triggering mode!"); } @@ -369,7 +369,7 @@ void BaseRealSenseNode::setupDevice() } // set cam sync mode - if(_inter_cam_sync_mode != -1) + if(_inter_cam_sync_mode != inter_cam_sync_none) { _sensors[DEPTH].set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, _inter_cam_sync_mode); ROS_INFO_STREAM("Inter cam sync mode set to " << _inter_cam_sync_mode); From 5312121987943cb1ffeda920357139ef96a0ab7f Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Tue, 30 Oct 2018 15:12:48 +0100 Subject: [PATCH 40/41] Added dummy parameter because redirect can't be empty --- realsense2_camera/launch/rs_camera.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 13d3eaab49..3d7aa5eaf1 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -43,8 +43,8 @@ - - + + From d697a61316fa0a2b044020f2683cf5086bfb6f62 Mon Sep 17 00:00:00 2001 From: michaelpantic Date: Tue, 30 Oct 2018 15:13:13 +0100 Subject: [PATCH 41/41] Added dummy parameter because redirect can't be empty --- realsense2_camera/launch/rs_camera.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 3d7aa5eaf1..6cfb0dd903 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -43,8 +43,8 @@ - - + +