diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index dc9589f..28798dc 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -70,38 +70,6 @@ namespace { constexpr char kTfStaticTopic[] = "/tf_static"; namespace carto = ::cartographer; -// TODO(hrapp): We discovered that using tf_buffer with a large CACHE -// is very inefficient. Switch asset writer to use our own -// TransformInterpolationBuffer. -void ReadTransformsFromBag(const string& bag_filename, - tf2_ros::Buffer* const tf_buffer) { - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); - rosbag::View view(bag); - - const ::ros::Time begin_time = view.getBeginTime(); - const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); - for (const rosbag::MessageInstance& msg : view) { - if (msg.isType()) { - const auto tf_msg = msg.instantiate(); - for (const auto& transform : tf_msg->transforms) { - try { - // TODO(damonkohler): Handle topic remapping. - tf_buffer->setTransform(transform, "unused_authority", - msg.getTopic() == kTfStaticTopic); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } - } - } - LOG_EVERY_N(INFO, 100000) - << "Processed " << (msg.getTime() - begin_time).toSec() << " of " - << duration_in_seconds << " bag time seconds..."; - } - - bag.close(); -} - template std::unique_ptr HandleMessage( const T& message, const string& tracking_frame, @@ -186,12 +154,7 @@ void Run(const string& pose_graph_filename, if (trajectory_proto.node_size() == 0) { continue; } - tf2_ros::Buffer tf_buffer(::ros::DURATION_MAX); - if (FLAGS_use_bag_transforms) { - LOG(INFO) << "Pre-loading transforms from bag..."; - ReadTransformsFromBag(bag_filename, &tf_buffer); - } - + tf2_ros::Buffer tf_buffer; if (!urdf_filename.empty()) { ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); } @@ -205,25 +168,55 @@ void Run(const string& pose_graph_filename, const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); + // We need to keep 'tf_buffer' small because it becomes very inefficient + // otherwise. We make sure that tf_messages are published before any data + // messages, so that tf lookups always work. + std::deque delayed_messages; + // We publish tf messages one second earlier than other messages. Under + // the assumption of higher frequency tf this should ensure that tf can + // always interpolate. + const ::ros::Duration kDelay(1.); for (const rosbag::MessageInstance& message : view) { - std::unique_ptr points_batch; - if (message.isType()) { - points_batch = HandleMessage( - *message.instantiate(), tracking_frame, - tf_buffer, transform_interpolation_buffer); - } else if (message.isType()) { - points_batch = HandleMessage( - *message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } else if (message.isType()) { - points_batch = HandleMessage( - *message.instantiate(), tracking_frame, - tf_buffer, transform_interpolation_buffer); + if (FLAGS_use_bag_transforms && message.isType()) { + auto tf_message = message.instantiate(); + for (const auto& transform : tf_message->transforms) { + try { + tf_buffer.setTransform(transform, "unused_authority", + message.getTopic() == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + } } - if (points_batch != nullptr) { - points_batch->trajectory_id = trajectory_id; - pipeline.back()->Process(std::move(points_batch)); + + while (!delayed_messages.empty() && + delayed_messages.front().getTime() < + message.getTime() - kDelay) { + const rosbag::MessageInstance& delayed_message = + delayed_messages.front(); + + std::unique_ptr points_batch; + if (delayed_message.isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (delayed_message + .isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (delayed_message.isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + if (points_batch != nullptr) { + points_batch->trajectory_id = trajectory_id; + pipeline.back()->Process(std::move(points_batch)); + } + delayed_messages.pop_front(); } + delayed_messages.push_back(message); LOG_EVERY_N(INFO, 100000) << "Processed " << (message.getTime() - begin_time).toSec() << " of " << duration_in_seconds << " bag time seconds..."; diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index c4b3fdd..3b6d63c 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -141,10 +141,14 @@ void Run(const std::vector& bag_filenames) { const ::ros::Time begin_time = view.getBeginTime(); const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); - // We make sure that tf_messages are published before any data messages, so - // that tf lookups always work and that tf_buffer has a small cache size - - // because it gets very inefficient with a large one. + // We need to keep 'tf_buffer' small because it becomes very inefficient + // otherwise. We make sure that tf_messages are published before any data + // messages, so that tf lookups always work. std::deque delayed_messages; + // We publish tf messages one second earlier than other messages. Under + // the assumption of higher frequency tf this should ensure that tf can + // always interpolate. + const ::ros::Duration kDelay(1.); for (const rosbag::MessageInstance& msg : view) { if (!::ros::ok()) { break; @@ -166,7 +170,7 @@ void Run(const std::vector& bag_filenames) { while (!delayed_messages.empty() && delayed_messages.front().getTime() < - msg.getTime() - ::ros::Duration(1.)) { + msg.getTime() - kDelay) { const rosbag::MessageInstance& delayed_msg = delayed_messages.front(); const string topic = node.node_handle()->resolveName( delayed_msg.getTopic(), false /* resolve */);