diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index fb0385f..317a1b8 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -29,7 +29,6 @@ #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" -#include "cartographer_ros/bag_reader.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" #include "cartographer_ros/urdf_reader.h" @@ -64,8 +63,41 @@ DEFINE_bool(use_bag_transforms, true, namespace cartographer_ros { 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 void HandleMessage( const T& message, const string& tracking_frame, @@ -128,16 +160,15 @@ void Run(const string& trajectory_filename, const string& bag_filename, builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); - auto tf_buffer = - ::cartographer::common::make_unique(::ros::DURATION_MAX); + 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.get()); + ReadTransformsFromBag(bag_filename, &tf_buffer); } if (!urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get()); + ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); } const string tracking_frame = @@ -156,17 +187,17 @@ void Run(const string& trajectory_filename, const string& bag_filename, for (const rosbag::MessageInstance& message : view) { if (message.isType()) { HandleMessage(*message.instantiate(), - tracking_frame, *tf_buffer, + tracking_frame, tf_buffer, *transform_interpolation_buffer, pipeline); } if (message.isType()) { HandleMessage(*message.instantiate(), - tracking_frame, *tf_buffer, + tracking_frame, tf_buffer, *transform_interpolation_buffer, pipeline); } if (message.isType()) { HandleMessage(*message.instantiate(), - tracking_frame, *tf_buffer, + tracking_frame, tf_buffer, *transform_interpolation_buffer, pipeline); } LOG_EVERY_N(INFO, 100000) diff --git a/cartographer_ros/cartographer_ros/bag_reader.cc b/cartographer_ros/cartographer_ros/bag_reader.cc deleted file mode 100644 index bc515e1..0000000 --- a/cartographer_ros/cartographer_ros/bag_reader.cc +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer_ros/bag_reader.h" - -#include -#include - -#include "cartographer/common/make_unique.h" -#include "glog/logging.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" -#include "tf2_msgs/TFMessage.h" - -namespace cartographer_ros { - -constexpr char kTfStaticTopic[] = "/tf_static"; - -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(); -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/bag_reader.h b/cartographer_ros/cartographer_ros/bag_reader.h deleted file mode 100644 index d1ce751..0000000 --- a/cartographer_ros/cartographer_ros/bag_reader.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_ROS_BAG_READER_H_ -#define CARTOGRAPHER_ROS_BAG_READER_H_ - -#include "cartographer/common/port.h" -#include "tf2_ros/buffer.h" - -namespace cartographer_ros { - -void ReadTransformsFromBag(const string& bag_filename, - tf2_ros::Buffer* const tf_buffer); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_BAG_READER_H_ diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 121f427..22be323 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -23,7 +23,6 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" -#include "cartographer_ros/bag_reader.h" #include "cartographer_ros/node.h" #include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/urdf_reader.h" @@ -53,9 +52,10 @@ DEFINE_bool(use_bag_transforms, true, namespace cartographer_ros { namespace { -constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; +constexpr char kTfStaticTopic[] = "/tf_static"; constexpr char kTfTopic[] = "tf"; +constexpr int kLatestOnlyPublisherQueueSize = 1; volatile std::sig_atomic_t sigint_triggered = 0; @@ -86,29 +86,21 @@ NodeOptions LoadOptions() { void Run(const std::vector& bag_filenames) { auto options = LoadOptions(); - auto tf_buffer = - ::cartographer::common::make_unique(::ros::DURATION_MAX); - - if (FLAGS_use_bag_transforms) { - LOG(INFO) << "Pre-loading transforms from bag..."; - // TODO(damonkohler): Support multi-trajectory. - CHECK_EQ(bag_filenames.size(), 1); - ReadTransformsFromBag(bag_filenames.back(), tf_buffer.get()); - } + tf2_ros::Buffer tf_buffer; std::vector urdf_transforms; if (!FLAGS_urdf_filename.empty()) { urdf_transforms = - ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get()); + ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer); } - tf_buffer->setUsingDedicatedThread(true); + tf_buffer.setUsingDedicatedThread(true); // Since we preload the transform buffer, we should never have to wait for a // transform. When we finish processing the bag, we will simply drop any // remaining sensor data that cannot be transformed due to missing transforms. options.lookup_transform_timeout_sec = 0.; - Node node(options, tf_buffer.get()); + Node node(options, &tf_buffer); node.Initialize(); std::unordered_set expected_sensor_ids; @@ -178,6 +170,10 @@ 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. + std::deque delayed_messages; for (const rosbag::MessageInstance& msg : view) { if (sigint_triggered) { break; @@ -186,6 +182,57 @@ void Run(const std::vector& bag_filenames) { if (FLAGS_use_bag_transforms && msg.isType()) { auto tf_message = msg.instantiate(); tf_publisher.publish(tf_message); + + if (FLAGS_use_bag_transforms) { + for (const auto& transform : tf_message->transforms) { + try { + tf_buffer.setTransform(transform, "unused_authority", + msg.getTopic() == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + } + } + } + + while (!delayed_messages.empty() && + delayed_messages.front().getTime() < + msg.getTime() + ::ros::Duration(1.)) { + const rosbag::MessageInstance& delayed_msg = delayed_messages.front(); + const string topic = node.node_handle()->resolveName( + delayed_msg.getTopic(), false /* resolve */); + if (delayed_msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleLaserScanMessage( + topic, delayed_msg.instantiate()); + } + if (delayed_msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage( + topic, + delayed_msg.instantiate()); + } + if (delayed_msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandlePointCloud2Message( + topic, delayed_msg.instantiate()); + } + if (delayed_msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleImuMessage(topic, + delayed_msg.instantiate()); + } + if (delayed_msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleOdometryMessage( + topic, delayed_msg.instantiate()); + } + delayed_messages.pop_front(); } const string topic = @@ -193,35 +240,7 @@ void Run(const std::vector& bag_filenames) { if (expected_sensor_ids.count(topic) == 0) { continue; } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleLaserScanMessage(topic, - msg.instantiate()); - } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage( - topic, msg.instantiate()); - } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandlePointCloud2Message( - topic, msg.instantiate()); - } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleImuMessage(topic, msg.instantiate()); - } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleOdometryMessage(topic, - msg.instantiate()); - } + delayed_messages.push_back(msg); rosgraph_msgs::Clock clock; clock.clock = msg.getTime();