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.master
Martin Schwörer
Wally B. Feed
parent
d14d0e2b90
commit
f818bc79d9
|
|
@ -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 <algorithm>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
#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 <typename T>
|
||||
std::unique_ptr<carto::io::PointsBatch> 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<carto::io::PointsBatch>();
|
||||
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<float>();
|
||||
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<std::string>& 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<carto::common::ConfigurationFileResolver>(
|
||||
std::vector<std::string>{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<carto::io::StreamFileWriter>(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<std::unique_ptr<carto::io::PointsProcessor>> 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<rosbag::MessageInstance> 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<tf2_msgs::TFMessage>()) {
|
||||
auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
|
||||
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<carto::io::PointsBatch> points_batch;
|
||||
if (delayed_message.isType<sensor_msgs::PointCloud2>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::PointCloud2>(),
|
||||
tracking_frame, tf_buffer, transform_interpolation_buffer);
|
||||
} else if (delayed_message
|
||||
.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
|
||||
tracking_frame, tf_buffer, transform_interpolation_buffer);
|
||||
} else if (delayed_message.isType<sensor_msgs::LaserScan>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::LaserScan>(),
|
||||
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
|
||||
|
|
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
#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<std::string>& 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_
|
||||
|
|
@ -14,39 +14,10 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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 <typename T>
|
||||
std::unique_ptr<carto::io::PointsBatch> 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<carto::io::PointsBatch>();
|
||||
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<float>();
|
||||
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<std::string>& 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<carto::common::ConfigurationFileResolver>(
|
||||
std::vector<std::string>{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<carto::io::StreamFileWriter>(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<std::unique_ptr<carto::io::PointsProcessor>> 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<rosbag::MessageInstance> 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<tf2_msgs::TFMessage>()) {
|
||||
auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
|
||||
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<carto::io::PointsBatch> points_batch;
|
||||
if (delayed_message.isType<sensor_msgs::PointCloud2>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::PointCloud2>(),
|
||||
tracking_frame, tf_buffer, transform_interpolation_buffer);
|
||||
} else if (delayed_message
|
||||
.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
|
||||
tracking_frame, tf_buffer, transform_interpolation_buffer);
|
||||
} else if (delayed_message.isType<sensor_msgs::LaserScan>()) {
|
||||
points_batch = HandleMessage(
|
||||
*delayed_message.instantiate<sensor_msgs::LaserScan>(),
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_
|
||||
|
|
|
|||
Loading…
Reference in New Issue