cartographer_ros: pbstream_trajectories_to_rosbag

Go to the documentation of this file.
00001 /*
00002  * Copyright 2019 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include <iostream>
00018 #include <string>
00019 #include <vector>
00020 
00021 #include "absl/strings/str_cat.h"
00022 #include "cartographer/transform/transform.h"
00023 #include "cartographer_ros/msg_conversion.h"
00024 #include "cartographer_ros/time_conversion.h"
00025 #include "geometry_msgs/TransformStamped.h"
00026 #include "gflags/gflags.h"
00027 #include "glog/logging.h"
00028 #include "rosbag/bag.h"
00029 #include "tf2_msgs/TFMessage.h"
00030 
00031 DEFINE_string(input, "", "pbstream file to process");
00032 DEFINE_string(output, "", "Bag file to write to.");
00033 DEFINE_string(parent_frame, "map", "Frame id to use as parent frame.");
00034 
00035 namespace cartographer_ros {
00036 namespace {
00037 
00038 geometry_msgs::TransformStamped ToTransformStamped(
00039     int64_t timestamp_uts, const std::string& parent_frame_id,
00040     const std::string& child_frame_id,
00041     const cartographer::transform::proto::Rigid3d& parent_T_child) {
00042   static int64_t seq = 0;
00043   geometry_msgs::TransformStamped transform_stamped;
00044   transform_stamped.header.seq = ++seq;
00045   transform_stamped.header.frame_id = parent_frame_id;
00046   transform_stamped.header.stamp = cartographer_ros::ToRos(
00047       ::cartographer::common::FromUniversal(timestamp_uts));
00048   transform_stamped.child_frame_id = child_frame_id;
00049   transform_stamped.transform = cartographer_ros::ToGeometryMsgTransform(
00050       ::cartographer::transform::ToRigid3(parent_T_child));
00051   return transform_stamped;
00052 }
00053 
00054 void pbstream_trajectories_to_bag(const std::string& pbstream_filename,
00055                                   const std::string& output_bag_filename,
00056                                   const std::string& parent_frame_id) {
00057   const auto pose_graph =
00058       cartographer::io::DeserializePoseGraphFromFile(FLAGS_input);
00059 
00060   rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write);
00061   for (const auto trajectory : pose_graph.trajectory()) {
00062     const auto child_frame_id =
00063         absl::StrCat("trajectory_", trajectory.trajectory_id());
00064     LOG(INFO)
00065         << "Writing tf and geometry_msgs/TransformStamped for trajectory id "
00066         << trajectory.trajectory_id() << " with " << trajectory.node_size()
00067         << " nodes.";
00068     for (const auto& node : trajectory.node()) {
00069       tf2_msgs::TFMessage tf_msg;
00070       geometry_msgs::TransformStamped transform_stamped = ToTransformStamped(
00071           node.timestamp(), parent_frame_id, child_frame_id, node.pose());
00072       tf_msg.transforms.push_back(transform_stamped);
00073       bag.write(child_frame_id, transform_stamped.header.stamp,
00074                 transform_stamped);
00075       bag.write("/tf", transform_stamped.header.stamp, tf_msg);
00076     }
00077   }
00078 }
00079 
00080 }  // namespace
00081 }  // namespace cartographer_ros
00082 
00083 int main(int argc, char* argv[]) {
00084   FLAGS_alsologtostderr = true;
00085   google::InitGoogleLogging(argv[0]);
00086   google::SetUsageMessage(
00087       "\n\n"
00088       "Extracts all trajectories from the pbstream and creates a bag file with "
00089       "the trajectory poses stored in /tf.\nAdditionally, each trajectory is "
00090       "also written separately to a geometry_msgs/TransformStamped topic named "
00091       "after the TF child_frame_id of the trajectory.\n For each trajectory, "
00092       "the tool will write transforms with the tf parent_frame_id set "
00093       "according to the `parent_frame` commandline flag and child_frame_id to "
00094       "`trajectory_i`, with `i` corresponding to the `trajectory_id`.");
00095   google::ParseCommandLineFlags(&argc, &argv, true);
00096   CHECK(!FLAGS_input.empty()) << "-input pbstream is missing.";
00097   CHECK(!FLAGS_output.empty()) << "-output is missing.";
00098 
00099   cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output,
00100                                                  FLAGS_parent_frame);
00101   return 0;
00102 }