diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 0788dab..9c3c0d2 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -58,6 +58,9 @@ DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_string( trajectory_filename, "", "Proto containing the trajectory written by /finish_trajectory service."); +DEFINE_bool( + use_bag_transforms, true, + "Whether to read and use the transforms from the bag."); namespace cartographer_ros { namespace { @@ -150,12 +153,16 @@ 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(); + auto tf_buffer = + ::cartographer::common::make_unique(::ros::DURATION_MAX); + + if (FLAGS_use_bag_transforms) { + LOG(INFO) << "Pre-loading transforms from bag..."; + ReadTransformsFromBag(bag_filename, tf_buffer.get()); + } + if (!urdf_filename.empty()) { ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get()); - } else { - LOG(INFO) << "Pre-loading transforms from bag..."; - tf_buffer = ReadTransformsFromBag(bag_filename); } const string tracking_frame = diff --git a/cartographer_ros/cartographer_ros/bag_reader.cc b/cartographer_ros/cartographer_ros/bag_reader.cc index 9097c3a..e3c7b7e 100644 --- a/cartographer_ros/cartographer_ros/bag_reader.cc +++ b/cartographer_ros/cartographer_ros/bag_reader.cc @@ -29,14 +29,12 @@ namespace cartographer_ros { constexpr char kTfStaticTopic[] = "/tf_static"; -std::unique_ptr ReadTransformsFromBag( - const string& bag_filename) { +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); - auto tf_buffer = - ::cartographer::common::make_unique(::ros::DURATION_MAX); const ::ros::Time begin_time = view.getBeginTime(); const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); for (const rosbag::MessageInstance& msg : view) { @@ -58,7 +56,6 @@ std::unique_ptr ReadTransformsFromBag( } bag.close(); - return tf_buffer; } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/bag_reader.h b/cartographer_ros/cartographer_ros/bag_reader.h index 7cf9854..48964ee 100644 --- a/cartographer_ros/cartographer_ros/bag_reader.h +++ b/cartographer_ros/cartographer_ros/bag_reader.h @@ -22,8 +22,8 @@ namespace cartographer_ros { -std::unique_ptr ReadTransformsFromBag( - const string& bag_filename); +void ReadTransformsFromBag( + const string& bag_filename, tf2_ros::Buffer* const tf_buffer); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 349e73f..c30b136 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -33,6 +33,7 @@ #include "rosbag/view.h" #include "rosgraph_msgs/Clock.h" #include "tf2_msgs/TFMessage.h" +#include "tf2_ros/static_transform_broadcaster.h" #include "urdf/model.h" DEFINE_string(configuration_directory, "", @@ -46,12 +47,16 @@ DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process."); DEFINE_string( urdf_filename, "", "URDF file that contains static links for your sensor configuration."); +DEFINE_bool( + use_bag_transforms, true, + "Whether to read, use and republish the transforms from the bag."); namespace cartographer_ros { namespace { constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; +constexpr char kTfTopic [] = "tf"; volatile std::sig_atomic_t sigint_triggered = 0; @@ -82,15 +87,22 @@ NodeOptions LoadOptions() { void Run(const std::vector& bag_filenames) { auto options = LoadOptions(); - auto tf_buffer = ::cartographer::common::make_unique(); - if (!FLAGS_urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get()); - } else { + 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); - tf_buffer = ReadTransformsFromBag(bag_filenames.back()); + ReadTransformsFromBag(bag_filenames.back(), tf_buffer.get()); } + + std::vector urdf_transforms; + if (!FLAGS_urdf_filename.empty()) { + urdf_transforms = + ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get()); + } + tf_buffer->setUsingDedicatedThread(true); // Since we preload the transform buffer, we should never have to wait for a @@ -139,10 +151,20 @@ void Run(const std::vector& bag_filenames) { check_insert(kOdometryTopic); } + ::ros::Publisher tf_publisher = + node.node_handle()->advertise( + kTfTopic, kLatestOnlyPublisherQueueSize); + + ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; + ::ros::Publisher clock_publisher = node.node_handle()->advertise( kClockTopic, kLatestOnlyPublisherQueueSize); + if (urdf_transforms.size() > 0) { + static_tf_broadcaster.sendTransform(urdf_transforms); + } + for (const string& bag_filename : bag_filenames) { if (sigint_triggered) { break; @@ -162,7 +184,11 @@ void Run(const std::vector& bag_filenames) { break; } - // TODO(damonkohler): Republish non-conflicting tf messages. + if (FLAGS_use_bag_transforms && msg.isType()) { + auto tf_message = msg.instantiate(); + tf_publisher.publish(tf_message); + } + const string topic = node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); if (expected_sensor_ids.count(topic) == 0) { diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/cartographer_ros/urdf_reader.cc index 07ab385..7acf30d 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.cc +++ b/cartographer_ros/cartographer_ros/urdf_reader.cc @@ -24,8 +24,8 @@ namespace cartographer_ros { -void ReadStaticTransformsFromUrdf(const string& urdf_filename, - tf2_ros::Buffer* const buffer) { +std::vector ReadStaticTransformsFromUrdf( + const string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { urdf::Model model; CHECK(model.initFile(urdf_filename)); #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS @@ -34,6 +34,7 @@ void ReadStaticTransformsFromUrdf(const string& urdf_filename, std::vector > links; #endif model.getLinks(links); + std::vector transforms; for (const auto& link : links) { if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { continue; @@ -49,8 +50,10 @@ void ReadStaticTransformsFromUrdf(const string& urdf_filename, pose.rotation.y, pose.rotation.z))); transform.child_frame_id = link->name; transform.header.frame_id = link->getParent()->name; - buffer->setTransform(transform, "urdf", true /* is_static */); + tf_buffer->setTransform(transform, "urdf", true /* is_static */); + transforms.push_back(transform); } + return transforms; } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/cartographer_ros/urdf_reader.h index 0642870..7156137 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.h +++ b/cartographer_ros/cartographer_ros/urdf_reader.h @@ -19,11 +19,12 @@ #include "cartographer/common/port.h" #include "tf2_ros/buffer.h" +#include namespace cartographer_ros { -void ReadStaticTransformsFromUrdf(const string& urdf_filename, - tf2_ros::Buffer* buffer); +std::vector ReadStaticTransformsFromUrdf( + const string& urdf_filename, tf2_ros::Buffer* tf_buffer); } // namespace cartographer_ros