Refactor offline_node_main.cc to prepare for offline bridge. (#643)
[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)master
Christoph Schütte
Wally B. Feed
parent
d43c07d940
commit
d32c8b9d91
|
|
@ -0,0 +1,236 @@
|
|||
/*
|
||||
* Copyright 2018 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/offline_node.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <sys/resource.h>
|
||||
#include <time.h>
|
||||
#include <chrono>
|
||||
|
||||
#include "cartographer_ros/node.h"
|
||||
#include "cartographer_ros/urdf_reader.h"
|
||||
#include "ros/callback_queue.h"
|
||||
#include "rosbag/bag.h"
|
||||
#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(
|
||||
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.");
|
||||
DEFINE_string(pbstream_filename, "",
|
||||
"If non-empty, filename of a pbstream to load.");
|
||||
DEFINE_bool(keep_running, false,
|
||||
"Keep running the offline node after all messages from the bag "
|
||||
"have been processed.");
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
constexpr char kClockTopic[] = "clock";
|
||||
constexpr char kTfStaticTopic[] = "/tf_static";
|
||||
constexpr char kTfTopic[] = "tf";
|
||||
constexpr double kClockPublishFrequencySec = 1. / 30.;
|
||||
constexpr int kSingleThreaded = 1;
|
||||
|
||||
void RunOfflineNode(
|
||||
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
|
||||
const cartographer_ros::NodeOptions& node_options,
|
||||
const cartographer_ros::TrajectoryOptions& trajectory_options,
|
||||
const std::vector<std::string>& bag_filenames) {
|
||||
const std::chrono::time_point<std::chrono::steady_clock> start_time =
|
||||
std::chrono::steady_clock::now();
|
||||
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
|
||||
std::vector<geometry_msgs::TransformStamped> urdf_transforms;
|
||||
if (!FLAGS_urdf_filename.empty()) {
|
||||
urdf_transforms =
|
||||
ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
|
||||
}
|
||||
|
||||
tf_buffer.setUsingDedicatedThread(true);
|
||||
|
||||
Node node(node_options, std::move(map_builder), &tf_buffer);
|
||||
if (!FLAGS_pbstream_filename.empty()) {
|
||||
// TODO(jihoonl): LoadMap should be replaced by some better deserialization
|
||||
// of full SLAM state as non-frozen trajectories once possible
|
||||
node.LoadMap(FLAGS_pbstream_filename);
|
||||
}
|
||||
|
||||
std::unordered_set<std::string> expected_sensor_ids;
|
||||
for (const std::string& topic :
|
||||
node.ComputeDefaultTopics(trajectory_options)) {
|
||||
CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
|
||||
.second);
|
||||
}
|
||||
|
||||
::ros::Publisher tf_publisher =
|
||||
node.node_handle()->advertise<tf2_msgs::TFMessage>(
|
||||
kTfTopic, kLatestOnlyPublisherQueueSize);
|
||||
|
||||
::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
|
||||
|
||||
::ros::Publisher clock_publisher =
|
||||
node.node_handle()->advertise<rosgraph_msgs::Clock>(
|
||||
kClockTopic, kLatestOnlyPublisherQueueSize);
|
||||
|
||||
if (urdf_transforms.size() > 0) {
|
||||
static_tf_broadcaster.sendTransform(urdf_transforms);
|
||||
}
|
||||
|
||||
ros::AsyncSpinner async_spinner(kSingleThreaded);
|
||||
async_spinner.start();
|
||||
rosgraph_msgs::Clock clock;
|
||||
auto clock_republish_timer = node.node_handle()->createWallTimer(
|
||||
::ros::WallDuration(kClockPublishFrequencySec),
|
||||
[&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
|
||||
clock_publisher.publish(clock);
|
||||
},
|
||||
false /* oneshot */, false /* autostart */);
|
||||
|
||||
for (const std::string& bag_filename : bag_filenames) {
|
||||
if (!::ros::ok()) {
|
||||
break;
|
||||
}
|
||||
|
||||
const int trajectory_id =
|
||||
node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options);
|
||||
|
||||
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& msg : view) {
|
||||
if (!::ros::ok()) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
|
||||
auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
|
||||
tf_publisher.publish(tf_message);
|
||||
|
||||
for (const auto& transform : tf_message->transforms) {
|
||||
try {
|
||||
tf_buffer.setTransform(transform, "unused_authority",
|
||||
msg.getTopic() == kTfStaticTopic);
|
||||
} catch (const tf2::TransformException& ex) {
|
||||
LOG(WARNING) << ex.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (!delayed_messages.empty() &&
|
||||
delayed_messages.front().getTime() < msg.getTime() - kDelay) {
|
||||
const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
|
||||
const std::string topic = node.node_handle()->resolveName(
|
||||
delayed_msg.getTopic(), false /* resolve */);
|
||||
if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
|
||||
node.HandleLaserScanMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::LaserScan>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
node.HandleMultiEchoLaserScanMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
|
||||
node.HandlePointCloud2Message(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::PointCloud2>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::Imu>()) {
|
||||
node.HandleImuMessage(trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::Imu>());
|
||||
}
|
||||
if (delayed_msg.isType<nav_msgs::Odometry>()) {
|
||||
node.HandleOdometryMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<nav_msgs::Odometry>());
|
||||
}
|
||||
clock.clock = delayed_msg.getTime();
|
||||
clock_publisher.publish(clock);
|
||||
|
||||
LOG_EVERY_N(INFO, 100000)
|
||||
<< "Processed " << (delayed_msg.getTime() - begin_time).toSec()
|
||||
<< " of " << duration_in_seconds << " bag time seconds...";
|
||||
|
||||
delayed_messages.pop_front();
|
||||
}
|
||||
|
||||
const std::string topic =
|
||||
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
|
||||
if (expected_sensor_ids.count(topic) == 0) {
|
||||
continue;
|
||||
}
|
||||
delayed_messages.push_back(msg);
|
||||
}
|
||||
|
||||
bag.close();
|
||||
node.FinishTrajectory(trajectory_id);
|
||||
}
|
||||
|
||||
// Ensure the clock is republished after the bag has been finished, during the
|
||||
// final optimization, serialization, and optional indefinite spinning at the
|
||||
// end.
|
||||
clock_republish_timer.start();
|
||||
node.RunFinalOptimization();
|
||||
|
||||
const std::chrono::time_point<std::chrono::steady_clock> end_time =
|
||||
std::chrono::steady_clock::now();
|
||||
const double wall_clock_seconds =
|
||||
std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
|
||||
start_time)
|
||||
.count();
|
||||
|
||||
LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
|
||||
#ifdef __linux__
|
||||
timespec cpu_timespec = {};
|
||||
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
|
||||
LOG(INFO) << "Elapsed CPU time: "
|
||||
<< (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
|
||||
rusage usage;
|
||||
CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
|
||||
LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
|
||||
#endif
|
||||
|
||||
if (::ros::ok()) {
|
||||
const std::string output_filename = bag_filenames.front() + ".pbstream";
|
||||
LOG(INFO) << "Writing state to '" << output_filename << "'...";
|
||||
node.SerializeState(output_filename);
|
||||
}
|
||||
if (FLAGS_keep_running) {
|
||||
::ros::waitForShutdown();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* Copyright 2018 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_OFFLINE_NODE_H_
|
||||
#define CARTOGRAPHER_ROS_OFFLINE_NODE_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/mapping/map_builder_interface.h"
|
||||
#include "cartographer_ros/node_options.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
void RunOfflineNode(
|
||||
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
|
||||
const cartographer_ros::NodeOptions& node_options,
|
||||
const cartographer_ros::TrajectoryOptions& trajectory_options,
|
||||
const std::vector<std::string>& bag_filenames);
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_OFFLINE_NODE_H_
|
||||
|
|
@ -14,32 +14,11 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <sys/resource.h>
|
||||
#include <sys/time.h>
|
||||
#include <time.h>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/map_builder.h"
|
||||
#include "cartographer_ros/node.h"
|
||||
#include "cartographer_ros/node_options.h"
|
||||
#include "cartographer_ros/offline_node.h"
|
||||
#include "cartographer_ros/ros_log_sink.h"
|
||||
#include "cartographer_ros/split_string.h"
|
||||
#include "cartographer_ros/urdf_reader.h"
|
||||
#include "gflags/gflags.h"
|
||||
#include "ros/callback_queue.h"
|
||||
#include "rosbag/bag.h"
|
||||
#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, "",
|
||||
"First directory in which configuration files are searched, "
|
||||
|
|
@ -49,217 +28,6 @@ DEFINE_string(configuration_basename, "",
|
|||
"Basename, i.e. not containing any directory prefix, of the "
|
||||
"configuration file.");
|
||||
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.");
|
||||
DEFINE_string(pbstream_filename, "",
|
||||
"If non-empty, filename of a pbstream to load.");
|
||||
DEFINE_bool(keep_running, false,
|
||||
"Keep running the offline node after all messages from the bag "
|
||||
"have been processed.");
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace {
|
||||
|
||||
constexpr char kClockTopic[] = "clock";
|
||||
constexpr char kTfStaticTopic[] = "/tf_static";
|
||||
constexpr char kTfTopic[] = "tf";
|
||||
constexpr double kClockPublishFrequencySec = 1. / 30.;
|
||||
constexpr int kSingleThreaded = 1;
|
||||
|
||||
void Run(const std::vector<std::string>& bag_filenames) {
|
||||
const std::chrono::time_point<std::chrono::steady_clock> start_time =
|
||||
std::chrono::steady_clock::now();
|
||||
NodeOptions node_options;
|
||||
TrajectoryOptions trajectory_options;
|
||||
std::tie(node_options, trajectory_options) =
|
||||
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
|
||||
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
|
||||
std::vector<geometry_msgs::TransformStamped> urdf_transforms;
|
||||
if (!FLAGS_urdf_filename.empty()) {
|
||||
urdf_transforms =
|
||||
ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
|
||||
}
|
||||
|
||||
tf_buffer.setUsingDedicatedThread(true);
|
||||
|
||||
// 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.
|
||||
node_options.lookup_transform_timeout_sec = 0.;
|
||||
auto map_builder =
|
||||
cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
|
||||
node_options.map_builder_options);
|
||||
Node node(node_options, std::move(map_builder), &tf_buffer);
|
||||
if (!FLAGS_pbstream_filename.empty()) {
|
||||
// TODO(jihoonl): LoadMap should be replaced by some better deserialization
|
||||
// of full SLAM state as non-frozen trajectories once possible
|
||||
node.LoadMap(FLAGS_pbstream_filename);
|
||||
}
|
||||
|
||||
std::unordered_set<std::string> expected_sensor_ids;
|
||||
for (const std::string& topic :
|
||||
node.ComputeDefaultTopics(trajectory_options)) {
|
||||
CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
|
||||
.second);
|
||||
}
|
||||
|
||||
::ros::Publisher tf_publisher =
|
||||
node.node_handle()->advertise<tf2_msgs::TFMessage>(
|
||||
kTfTopic, kLatestOnlyPublisherQueueSize);
|
||||
|
||||
::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
|
||||
|
||||
::ros::Publisher clock_publisher =
|
||||
node.node_handle()->advertise<rosgraph_msgs::Clock>(
|
||||
kClockTopic, kLatestOnlyPublisherQueueSize);
|
||||
|
||||
if (urdf_transforms.size() > 0) {
|
||||
static_tf_broadcaster.sendTransform(urdf_transforms);
|
||||
}
|
||||
|
||||
ros::AsyncSpinner async_spinner(kSingleThreaded);
|
||||
async_spinner.start();
|
||||
rosgraph_msgs::Clock clock;
|
||||
auto clock_republish_timer = node.node_handle()->createWallTimer(
|
||||
::ros::WallDuration(kClockPublishFrequencySec),
|
||||
[&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
|
||||
clock_publisher.publish(clock);
|
||||
},
|
||||
false /* oneshot */, false /* autostart */);
|
||||
|
||||
for (const std::string& bag_filename : bag_filenames) {
|
||||
if (!::ros::ok()) {
|
||||
break;
|
||||
}
|
||||
|
||||
const int trajectory_id =
|
||||
node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options);
|
||||
|
||||
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& msg : view) {
|
||||
if (!::ros::ok()) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
|
||||
auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
|
||||
tf_publisher.publish(tf_message);
|
||||
|
||||
for (const auto& transform : tf_message->transforms) {
|
||||
try {
|
||||
tf_buffer.setTransform(transform, "unused_authority",
|
||||
msg.getTopic() == kTfStaticTopic);
|
||||
} catch (const tf2::TransformException& ex) {
|
||||
LOG(WARNING) << ex.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (!delayed_messages.empty() &&
|
||||
delayed_messages.front().getTime() < msg.getTime() - kDelay) {
|
||||
const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
|
||||
const std::string topic = node.node_handle()->resolveName(
|
||||
delayed_msg.getTopic(), false /* resolve */);
|
||||
if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
|
||||
node.HandleLaserScanMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::LaserScan>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
node.HandleMultiEchoLaserScanMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
|
||||
node.HandlePointCloud2Message(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::PointCloud2>());
|
||||
}
|
||||
if (delayed_msg.isType<sensor_msgs::Imu>()) {
|
||||
node.HandleImuMessage(trajectory_id, topic,
|
||||
delayed_msg.instantiate<sensor_msgs::Imu>());
|
||||
}
|
||||
if (delayed_msg.isType<nav_msgs::Odometry>()) {
|
||||
node.HandleOdometryMessage(
|
||||
trajectory_id, topic,
|
||||
delayed_msg.instantiate<nav_msgs::Odometry>());
|
||||
}
|
||||
clock.clock = delayed_msg.getTime();
|
||||
clock_publisher.publish(clock);
|
||||
|
||||
LOG_EVERY_N(INFO, 100000)
|
||||
<< "Processed " << (delayed_msg.getTime() - begin_time).toSec()
|
||||
<< " of " << duration_in_seconds << " bag time seconds...";
|
||||
|
||||
delayed_messages.pop_front();
|
||||
}
|
||||
|
||||
const std::string topic =
|
||||
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
|
||||
if (expected_sensor_ids.count(topic) == 0) {
|
||||
continue;
|
||||
}
|
||||
delayed_messages.push_back(msg);
|
||||
}
|
||||
|
||||
bag.close();
|
||||
node.FinishTrajectory(trajectory_id);
|
||||
}
|
||||
|
||||
// Ensure the clock is republished after the bag has been finished, during the
|
||||
// final optimization, serialization, and optional indefinite spinning at the
|
||||
// end.
|
||||
clock_republish_timer.start();
|
||||
node.RunFinalOptimization();
|
||||
|
||||
const std::chrono::time_point<std::chrono::steady_clock> end_time =
|
||||
std::chrono::steady_clock::now();
|
||||
const double wall_clock_seconds =
|
||||
std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
|
||||
start_time)
|
||||
.count();
|
||||
|
||||
LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
|
||||
#ifdef __linux__
|
||||
timespec cpu_timespec = {};
|
||||
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
|
||||
LOG(INFO) << "Elapsed CPU time: "
|
||||
<< (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
|
||||
rusage usage;
|
||||
CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
|
||||
LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
|
||||
#endif
|
||||
|
||||
if (::ros::ok()) {
|
||||
const std::string output_filename = bag_filenames.front() + ".pbstream";
|
||||
LOG(INFO) << "Writing state to '" << output_filename << "'...";
|
||||
node.SerializeState(output_filename);
|
||||
}
|
||||
if (FLAGS_keep_running) {
|
||||
::ros::waitForShutdown();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cartographer_ros
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
|
|
@ -275,7 +43,23 @@ int main(int argc, char** argv) {
|
|||
::ros::start();
|
||||
|
||||
cartographer_ros::ScopedRosLogSink ros_log_sink;
|
||||
cartographer_ros::Run(
|
||||
|
||||
cartographer_ros::NodeOptions node_options;
|
||||
cartographer_ros::TrajectoryOptions trajectory_options;
|
||||
std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
|
||||
FLAGS_configuration_directory, FLAGS_configuration_basename);
|
||||
|
||||
// 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.
|
||||
node_options.lookup_transform_timeout_sec = 0.;
|
||||
|
||||
auto map_builder =
|
||||
cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
|
||||
node_options.map_builder_options);
|
||||
|
||||
cartographer_ros::RunOfflineNode(
|
||||
std::move(map_builder), node_options, trajectory_options,
|
||||
cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
|
||||
|
||||
::ros::shutdown();
|
||||
|
|
|
|||
Loading…
Reference in New Issue