From d5710fe2919925a8a12475dc7c369a96207c83f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 21 Feb 2017 09:44:19 +0100 Subject: [PATCH] Offline node improvements (#266) * Unify signatures of ReadTransformsFromBag and ReadStaticTransformsFromUrdf. Both functions append data into the buffer, so they can be combined. * Modifies offline_node and assets_writer to optionally use both the .urdf file and bag transform data, if available (use case: fixed joint descriptions of the robot are in the .urdf, while the odometry-base link transforms are read from the bag); the static transforms are added to the buffer after it has been filled with transforms from the bag (if any). * offline_node optionally publishes all transforms from the bag (checking if any are in conflict with Cartographer is still TODO; however, this should be the responsibility of the bag provider, just like when using the online node, so it should be OK to leave this unimplemented) and all static transforms from the URDF. --- .../cartographer_ros/assets_writer_main.cc | 15 ++++++-- .../cartographer_ros/bag_reader.cc | 7 +--- .../cartographer_ros/bag_reader.h | 4 +- .../cartographer_ros/offline_node_main.cc | 38 ++++++++++++++++--- .../cartographer_ros/urdf_reader.cc | 9 +++-- .../cartographer_ros/urdf_reader.h | 5 ++- 6 files changed, 56 insertions(+), 22 deletions(-) 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