diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index c1f38ba..20c9ecb 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -10,6 +10,15 @@ google_library(assets_writer occupancy_grid ) +google_library(bag_reader + USES_CARTOGRAPHER + USES_GLOG + SRCS + bag_reader.cc + HDRS + bag_reader.h +) + google_library(map_builder_bridge USES_CARTOGRAPHER SRCS @@ -158,6 +167,7 @@ google_binary(cartographer_assets_writer SRCS assets_writer_main.cc DEPENDS + bag_reader msg_conversion time_conversion urdf_reader @@ -188,6 +198,7 @@ google_binary(cartographer_offline_node SRCS offline_node_main.cc DEPENDS + bag_reader node ros_log_sink urdf_reader diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index e31cd2e..7da4cda 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -24,6 +24,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor_pipeline_builder.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" @@ -34,6 +35,7 @@ #include "rosbag/bag.h" #include "rosbag/view.h" #include "tf2_eigen/tf2_eigen.h" +#include "tf2_msgs/TFMessage.h" #include "tf2_ros/buffer.h" #include "urdf/model.h" @@ -57,20 +59,43 @@ namespace { namespace carto = ::cartographer; +void HandlePointCloud2Message( + const sensor_msgs::PointCloud2::ConstPtr& msg, + const string& tracking_frame, + const tf2_ros::Buffer& tf_buffer, + const carto::transform::TransformInterpolationBuffer& + transform_interpolation_buffer, + const std::vector>& pipeline) { + const carto::common::Time time = FromRos(msg->header.stamp); + if (!transform_interpolation_buffer.Has(time)) { + return; + } + + const carto::transform::Rigid3d tracking_to_map = + transform_interpolation_buffer.Lookup(time); + const carto::transform::Rigid3d sensor_to_tracking = + ToRigid3d(tf_buffer.lookupTransform(tracking_frame, msg->header.frame_id, + msg->header.stamp)); + const carto::transform::Rigid3f sensor_to_map = + (tracking_to_map * sensor_to_tracking).cast(); + + auto batch = carto::common::make_unique(); + batch->time = time; + batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); + batch->frame_id = msg->header.frame_id; + + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(*msg, pcl_point_cloud); + for (const auto& point : pcl_point_cloud) { + batch->points.push_back(sensor_to_map * + Eigen::Vector3f(point.x, point.y, point.z)); + } + pipeline.back()->Process(std::move(batch)); +} + void Run(const string& trajectory_filename, const string& bag_filename, const string& configuration_directory, const string& configuration_basename, const string& urdf_filename) { - std::ifstream stream(trajectory_filename.c_str()); - carto::mapping::proto::Trajectory trajectory_proto; - CHECK(trajectory_proto.ParseFromIstream(&stream)); - - auto transform_interpolation_buffer = - carto::transform::TransformInterpolationBuffer::FromTrajectory( - trajectory_proto); - - carto::io::PointsProcessorPipelineBuilder builder; - carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder); - auto file_resolver = carto::common::make_unique( std::vector{configuration_directory}); @@ -78,50 +103,31 @@ void Run(const string& trajectory_filename, const string& bag_filename, file_resolver->GetFileContentOrDie(configuration_basename); carto::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); - const string tracking_frame = - lua_parameter_dictionary.GetString("tracking_frame"); + std::ifstream stream(trajectory_filename.c_str()); + carto::mapping::proto::Trajectory trajectory_proto; + CHECK(trajectory_proto.ParseFromIstream(&stream)); + + carto::io::PointsProcessorPipelineBuilder builder; + carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder); std::vector> pipeline = builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); - tf2_ros::Buffer buffer; - // TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to - // support non-rigid sensor configurations. + auto tf_buffer = ::cartographer::common::make_unique(); if (!urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(urdf_filename, &buffer); + ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get()); + } else { + LOG(INFO) << "Pre-loading transforms from bag..."; + tf_buffer = ReadTransformsFromBag(bag_filename); } - auto handle_point_cloud_2_message = [&]( - const sensor_msgs::PointCloud2::ConstPtr& msg) { - const carto::common::Time time = FromRos(msg->header.stamp); - if (!transform_interpolation_buffer->Has(time)) { - return; - } - carto::transform::Rigid3d tracking_to_map = - transform_interpolation_buffer->Lookup(time); - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*msg, pcl_point_cloud); - - const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d( - buffer.lookupTransform(tracking_frame, msg->header.frame_id, - msg->header.stamp)); - - const carto::transform::Rigid3f sensor_to_map = - (tracking_to_map * sensor_to_tracking).cast(); - - auto batch = carto::common::make_unique(); - batch->time = time; - batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); - batch->frame_id = msg->header.frame_id; - - for (const auto& point : pcl_point_cloud) { - batch->points.push_back(sensor_to_map * - Eigen::Vector3f(point.x, point.y, point.z)); - } - pipeline.back()->Process(std::move(batch)); - }; - + const string tracking_frame = + lua_parameter_dictionary.GetString("tracking_frame"); + const auto transform_interpolation_buffer = + carto::transform::TransformInterpolationBuffer::FromTrajectory( + trajectory_proto); + LOG(INFO) << "Processing pipeline..."; do { rosbag::Bag bag; bag.open(bag_filename, rosbag::bagmode::Read); @@ -131,7 +137,9 @@ void Run(const string& trajectory_filename, const string& bag_filename, for (const rosbag::MessageInstance& msg : view) { if (msg.isType()) { - handle_point_cloud_2_message(msg.instantiate()); + HandlePointCloud2Message(msg.instantiate(), + tracking_frame, *tf_buffer, + *transform_interpolation_buffer, pipeline); } LOG_EVERY_N(INFO, 100000) << "Processed " << (msg.getTime() - begin_time).toSec() << " of " @@ -146,6 +154,7 @@ void Run(const string& trajectory_filename, const string& bag_filename, } // namespace cartographer_ros int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); google::ParseCommandLineFlags(&argc, &argv, true); @@ -156,7 +165,6 @@ int main(int argc, char** argv) { CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; CHECK(!FLAGS_trajectory_filename.empty()) << "-trajectory_filename is missing."; - CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing."; ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename, FLAGS_configuration_directory, diff --git a/cartographer_ros/cartographer_ros/bag_reader.cc b/cartographer_ros/cartographer_ros/bag_reader.cc new file mode 100644 index 0000000..6bc26af --- /dev/null +++ b/cartographer_ros/cartographer_ros/bag_reader.cc @@ -0,0 +1,64 @@ +/* + * 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"; + +std::unique_ptr ReadTransformsFromBag( + const string& bag_filename) { + 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) { + 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(); + return tf_buffer; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/bag_reader.h b/cartographer_ros/cartographer_ros/bag_reader.h new file mode 100644 index 0000000..7cf9854 --- /dev/null +++ b/cartographer_ros/cartographer_ros/bag_reader.h @@ -0,0 +1,30 @@ +/* + * 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 { + +std::unique_ptr ReadTransformsFromBag( + const string& bag_filename); + +} // 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 2e5d34b..000e928 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -22,6 +22,7 @@ #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" @@ -29,6 +30,7 @@ #include "rosbag/bag.h" #include "rosbag/view.h" #include "rosgraph_msgs/Clock.h" +#include "tf2_msgs/TFMessage.h" #include "urdf/model.h" DEFINE_string(configuration_directory, "", @@ -48,6 +50,7 @@ namespace { constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; +constexpr char kTfStaticTopic[] = "/tf_static"; std::vector SplitString(const string& input, const char delimiter) { std::stringstream stream(input); @@ -68,11 +71,23 @@ void Run(std::vector bag_filenames) { cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); - tf2_ros::Buffer tf_buffer; - tf_buffer.setUsingDedicatedThread(true); - ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer); - const auto options = CreateNodeOptions(&lua_parameter_dictionary); - Node node(options, &tf_buffer); + auto tf_buffer = ::cartographer::common::make_unique(); + if (!FLAGS_urdf_filename.empty()) { + ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get()); + } else { + LOG(INFO) << "Pre-loading transforms from bag..."; + // TODO(damonkohler): Support multi-trajectory. + CHECK_EQ(bag_filenames.size(), 1); + tf_buffer = ReadTransformsFromBag(bag_filenames.back()); + } + tf_buffer->setUsingDedicatedThread(true); + + auto options = CreateNodeOptions(&lua_parameter_dictionary); + // 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.Initialize(); std::unordered_set expected_sensor_ids; @@ -138,6 +153,7 @@ void Run(std::vector bag_filenames) { return; } + // TODO(damonkohler): Republish non-conflicting tf messages. const string topic = node.node_handle()->resolveName(msg.getTopic()); if (expected_sensor_ids.count(topic) == 0) { continue; @@ -147,21 +163,25 @@ void Run(std::vector bag_filenames) { ->sensor_bridge(trajectory_id) ->HandleLaserScanMessage(topic, msg.instantiate()); - } else if (msg.isType()) { + } + if (msg.isType()) { node.map_builder_bridge() ->sensor_bridge(trajectory_id) ->HandleMultiEchoLaserScanMessage( topic, msg.instantiate()); - } else if (msg.isType()) { + } + if (msg.isType()) { node.map_builder_bridge() ->sensor_bridge(trajectory_id) ->HandlePointCloud2Message( topic, msg.instantiate()); - } else if (msg.isType()) { + } + if (msg.isType()) { node.map_builder_bridge() ->sensor_bridge(trajectory_id) ->HandleImuMessage(topic, msg.instantiate()); - } else if (msg.isType()) { + } + if (msg.isType()) { node.map_builder_bridge() ->sensor_bridge(trajectory_id) ->HandleOdometryMessage(topic, @@ -190,6 +210,7 @@ void Run(std::vector bag_filenames) { } // namespace cartographer_ros int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); google::ParseCommandLineFlags(&argc, &argv, true); @@ -198,7 +219,6 @@ int main(int argc, char** argv) { CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing."; - CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing."; ::ros::init(argc, argv, "cartographer_offline_node"); ::ros::start();