From f818bc79d9aeeab0716e468818b31e59757e822c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Fri, 6 Apr 2018 16:48:21 +0200 Subject: [PATCH] moved run method of assets writer main to separate files (#807) Moved the run method of the assets_writer_main to the separate assets_writer files. Will extract asset_writer class in the future to keep the main file small and allow re-usability and more flexibility of the asset_writer. --- .../cartographer_ros/assets_writer.cc | 244 ++++++++++++++++++ .../cartographer_ros/assets_writer.h | 37 +++ .../cartographer_ros/assets_writer_main.cc | 228 +--------------- cartographer_ros/cartographer_ros/ros_map.h | 2 +- 4 files changed, 285 insertions(+), 226 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/assets_writer.cc create mode 100644 cartographer_ros/cartographer_ros/assets_writer.h diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc new file mode 100644 index 0000000..594d717 --- /dev/null +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -0,0 +1,244 @@ +/* + * 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/assets_writer.h" + +#include +#include +#include + +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/ros_map_writing_points_processor.h" +#include "cartographer_ros/split_string.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros/urdf_reader.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/time.h" +#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" + +namespace cartographer_ros { +namespace { + +constexpr char kTfStaticTopic[] = "/tf_static"; +namespace carto = ::cartographer; + +template +std::unique_ptr HandleMessage( + const T& message, const std::string& tracking_frame, + const tf2_ros::Buffer& tf_buffer, + const carto::transform::TransformInterpolationBuffer& + transform_interpolation_buffer) { + const carto::common::Time start_time = FromRos(message.header.stamp); + + auto points_batch = carto::common::make_unique(); + points_batch->start_time = start_time; + points_batch->frame_id = message.header.frame_id; + + ::cartographer::sensor::PointCloudWithIntensities point_cloud; + ::cartographer::common::Time point_cloud_time; + std::tie(point_cloud, point_cloud_time) = + ToPointCloudWithIntensities(message); + CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); + + for (size_t i = 0; i < point_cloud.points.size(); ++i) { + const carto::common::Time time = + point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); + if (!transform_interpolation_buffer.Has(time)) { + continue; + } + 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, message.header.frame_id, ToRos(time))); + const carto::transform::Rigid3f sensor_to_map = + (tracking_to_map * sensor_to_tracking).cast(); + points_batch->points.push_back(sensor_to_map * + point_cloud.points[i].head<3>()); + points_batch->intensities.push_back(point_cloud.intensities[i]); + // We use the last transform for the origin, which is approximately correct. + points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); + } + if (points_batch->points.empty()) { + return nullptr; + } + return points_batch; +} + +} // namespace + +void RunAssetsWriterPipeline(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, + const std::string& output_file_prefix, + const bool use_bag_transforms) { + auto file_resolver = + carto::common::make_unique( + std::vector{configuration_directory}); + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + carto::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver)); + + carto::io::ProtoStreamReader reader(pose_graph_filename); + carto::mapping::proto::PoseGraph pose_graph_proto; + CHECK(reader.ReadProto(&pose_graph_proto)); + ::cartographer::mapping::proto::AllTrajectoryBuilderOptions + all_trajectory_builder_options; + CHECK(reader.ReadProto(&all_trajectory_builder_options)); + CHECK_EQ(pose_graph_proto.trajectory_size(), bag_filenames.size()) + << "Pose graphs contains " << pose_graph_proto.trajectory_size() + << " trajectories while " << bag_filenames.size() + << " bags were provided. This tool requires one bag for each " + "trajectory in the same order as the correponding trajectories in the " + "pose graph proto."; + + const std::string file_prefix = !output_file_prefix.empty() + ? output_file_prefix + : bag_filenames.front() + "_"; + const auto file_writer_factory = [file_prefix](const std::string& filename) { + return carto::common::make_unique(file_prefix + + filename); + }; + + // This vector must outlive the pipeline. + std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories( + pose_graph_proto.trajectory().begin(), + pose_graph_proto.trajectory().end()); + + carto::io::PointsProcessorPipelineBuilder builder; + carto::io::RegisterBuiltInPointsProcessors(all_trajectories, + file_writer_factory, &builder); + builder.Register( + RosMapWritingPointsProcessor::kConfigurationFileActionName, + [file_writer_factory]( + ::cartographer::common::LuaParameterDictionary* const dictionary, + ::cartographer::io::PointsProcessor* const next) + -> std::unique_ptr<::cartographer::io::PointsProcessor> { + return RosMapWritingPointsProcessor::FromDictionary(file_writer_factory, + dictionary, next); + }); + + std::vector> pipeline = + builder.CreatePipeline( + lua_parameter_dictionary.GetDictionary("pipeline").get()); + + const std::string tracking_frame = + lua_parameter_dictionary.GetString("tracking_frame"); + do { + for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size(); + ++trajectory_id) { + const carto::mapping::proto::Trajectory& trajectory_proto = + pose_graph_proto.trajectory(trajectory_id); + const std::string& bag_filename = bag_filenames[trajectory_id]; + LOG(INFO) << "Processing " << bag_filename << "..."; + if (trajectory_proto.node_size() == 0) { + continue; + } + tf2_ros::Buffer tf_buffer; + if (!urdf_filename.empty()) { + ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); + } + + const carto::transform::TransformInterpolationBuffer + transform_interpolation_buffer(trajectory_proto); + 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(); + + // 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) { + if (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(); + } + } + } + + 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..."; + } + bag.close(); + } + } while (pipeline.back()->Flush() == + carto::io::PointsProcessor::FlushResult::kRestartStream); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h new file mode 100644 index 0000000..8dbeed6 --- /dev/null +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -0,0 +1,37 @@ +/* + * 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 +#include + +#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_ +#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_ + +namespace cartographer_ros { + +// Configures a point processing pipeline and pushes the points from the bag +// through it. +void RunAssetsWriterPipeline(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, + const std::string& output_file_prefix, + bool use_bag_transforms); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_ASSETS_WRITER_H_ diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 4cc09f7..e17fa6f 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -14,39 +14,10 @@ * limitations under the License. */ -#include -#include -#include -#include -#include - -#include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/make_unique.h" -#include "cartographer/common/math.h" -#include "cartographer/io/file_writer.h" -#include "cartographer/io/points_processor.h" -#include "cartographer/io/points_processor_pipeline_builder.h" -#include "cartographer/io/proto_stream.h" -#include "cartographer/mapping/proto/pose_graph.pb.h" -#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include "cartographer/sensor/point_cloud.h" -#include "cartographer/sensor/range_data.h" -#include "cartographer/transform/transform_interpolation_buffer.h" -#include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/ros_map_writing_points_processor.h" +#include "cartographer_ros/assets_writer.h" #include "cartographer_ros/split_string.h" -#include "cartographer_ros/time_conversion.h" -#include "cartographer_ros/urdf_reader.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "ros/ros.h" -#include "ros/time.h" -#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" DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " @@ -70,199 +41,6 @@ DEFINE_string(output_file_prefix, "", "define the output directory. If empty, the first bag filename " "will be used."); -namespace cartographer_ros { -namespace { - -constexpr char kTfStaticTopic[] = "/tf_static"; -namespace carto = ::cartographer; - -template -std::unique_ptr HandleMessage( - const T& message, const std::string& tracking_frame, - const tf2_ros::Buffer& tf_buffer, - const carto::transform::TransformInterpolationBuffer& - transform_interpolation_buffer) { - const carto::common::Time start_time = FromRos(message.header.stamp); - - auto points_batch = carto::common::make_unique(); - points_batch->start_time = start_time; - points_batch->frame_id = message.header.frame_id; - - ::cartographer::sensor::PointCloudWithIntensities point_cloud; - ::cartographer::common::Time point_cloud_time; - std::tie(point_cloud, point_cloud_time) = - ToPointCloudWithIntensities(message); - CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); - - for (size_t i = 0; i < point_cloud.points.size(); ++i) { - const carto::common::Time time = - point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); - if (!transform_interpolation_buffer.Has(time)) { - continue; - } - 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, message.header.frame_id, ToRos(time))); - const carto::transform::Rigid3f sensor_to_map = - (tracking_to_map * sensor_to_tracking).cast(); - points_batch->points.push_back(sensor_to_map * - point_cloud.points[i].head<3>()); - points_batch->intensities.push_back(point_cloud.intensities[i]); - // We use the last transform for the origin, which is approximately correct. - points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); - } - if (points_batch->points.empty()) { - return nullptr; - } - return points_batch; -} - -void Run(const std::string& pose_graph_filename, - const std::vector& bag_filenames, - const std::string& configuration_directory, - const std::string& configuration_basename, - const std::string& urdf_filename, - const std::string& output_file_prefix) { - auto file_resolver = - carto::common::make_unique( - std::vector{configuration_directory}); - const std::string code = - file_resolver->GetFileContentOrDie(configuration_basename); - carto::common::LuaParameterDictionary lua_parameter_dictionary( - code, std::move(file_resolver)); - - carto::io::ProtoStreamReader reader(pose_graph_filename); - carto::mapping::proto::PoseGraph pose_graph_proto; - CHECK(reader.ReadProto(&pose_graph_proto)); - ::cartographer::mapping::proto::AllTrajectoryBuilderOptions - all_trajectory_builder_options; - CHECK(reader.ReadProto(&all_trajectory_builder_options)); - CHECK_EQ(pose_graph_proto.trajectory_size(), bag_filenames.size()) - << "Pose graphs contains " << pose_graph_proto.trajectory_size() - << " trajectories while " << bag_filenames.size() - << " bags were provided. This tool requires one bag for each " - "trajectory in the same order as the correponding trajectories in the " - "pose graph proto."; - - const std::string file_prefix = !output_file_prefix.empty() - ? output_file_prefix - : bag_filenames.front() + "_"; - const auto file_writer_factory = [file_prefix](const std::string& filename) { - return carto::common::make_unique(file_prefix + - filename); - }; - - // This vector must outlive the pipeline. - std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories( - pose_graph_proto.trajectory().begin(), - pose_graph_proto.trajectory().end()); - - carto::io::PointsProcessorPipelineBuilder builder; - carto::io::RegisterBuiltInPointsProcessors(all_trajectories, - file_writer_factory, &builder); - builder.Register( - RosMapWritingPointsProcessor::kConfigurationFileActionName, - [file_writer_factory]( - ::cartographer::common::LuaParameterDictionary* const dictionary, - ::cartographer::io::PointsProcessor* const next) - -> std::unique_ptr<::cartographer::io::PointsProcessor> { - return RosMapWritingPointsProcessor::FromDictionary(file_writer_factory, - dictionary, next); - }); - - std::vector> pipeline = - builder.CreatePipeline( - lua_parameter_dictionary.GetDictionary("pipeline").get()); - - const std::string tracking_frame = - lua_parameter_dictionary.GetString("tracking_frame"); - do { - for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size(); - ++trajectory_id) { - const carto::mapping::proto::Trajectory& trajectory_proto = - pose_graph_proto.trajectory(trajectory_id); - const std::string& bag_filename = bag_filenames[trajectory_id]; - LOG(INFO) << "Processing " << bag_filename << "..."; - if (trajectory_proto.node_size() == 0) { - continue; - } - tf2_ros::Buffer tf_buffer; - if (!urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); - } - - const carto::transform::TransformInterpolationBuffer - transform_interpolation_buffer(trajectory_proto); - 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(); - - // 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) { - 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(); - } - } - } - - 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..."; - } - bag.close(); - } - } while (pipeline.back()->Flush() == - carto::io::PointsProcessor::FlushResult::kRestartStream); -} - -} // namespace -} // namespace cartographer_ros - int main(int argc, char** argv) { FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); @@ -276,9 +54,9 @@ int main(int argc, char** argv) { CHECK(!FLAGS_pose_graph_filename.empty()) << "-pose_graph_filename is missing."; - ::cartographer_ros::Run( + ::cartographer_ros::RunAssetsWriterPipeline( FLAGS_pose_graph_filename, cartographer_ros::SplitString(FLAGS_bag_filenames, ','), FLAGS_configuration_directory, FLAGS_configuration_basename, - FLAGS_urdf_filename, FLAGS_output_file_prefix); + FLAGS_urdf_filename, FLAGS_output_file_prefix, FLAGS_use_bag_transforms); } diff --git a/cartographer_ros/cartographer_ros/ros_map.h b/cartographer_ros/cartographer_ros/ros_map.h index cdeab4a..6cfde6e 100644 --- a/cartographer_ros/cartographer_ros/ros_map.h +++ b/cartographer_ros/cartographer_ros/ros_map.h @@ -38,4 +38,4 @@ void WriteYaml(const double resolution, const Eigen::Vector2d& origin, } // namespace cartographer_ros -#endif /* end of include guard: CARTOGRAPHER_ROS_ROS_MAP_H_ */ +#endif // CARTOGRAPHER_ROS_ROS_MAP_H_