12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Simultaneous offline multi trajectories (#636) 

RFC=[0009](https://github.com/googlecartographer/rfcs/pull/4)
master
Juraj Oršulić 2018-01-24 11:19:43 +01:00 committed by Wally B. Feed
parent d96aa2105b
commit d65f3c0f47
7 changed files with 425 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -257,8 +257,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
visualization_msgs::Marker residual_inter_same_trajectory_marker =
constraint_intra_marker;
residual_inter_same_trajectory_marker.id = marker_id++;
residual_inter_same_trajectory_marker.ns =
"Inter residuals, same trajectory";
residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory";
residual_inter_same_trajectory_marker.pose.position.z = 0.1;
visualization_msgs::Marker constraint_inter_diff_trajectory_marker =
@ -309,7 +308,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
// Bright orange
color_constraint.a = 1.0;
color_constraint.r = 1.0;
color_constraint.g = 165./255.;
color_constraint.g = 165. / 255.;
}
// Bright cyan
color_residual.a = 1.0;

2
cartographer_ros/cartographer_ros/node.cc
View File

@ -122,7 +122,7 @@ Node::Node(
&Node::PublishConstraintList, this));
}
Node::~Node() {}
Node::~Node() { FinishAllTrajectories(); }
::ros::NodeHandle* Node::node_handle() { return &node_handle_; }

305
cartographer_ros/cartographer_ros/offline_node.cc
View File

@ -21,17 +21,14 @@
#include <sys/resource.h>
#include <time.h>
#include <chrono>
#include <queue>
#include "cartographer_ros/node.h"
#include "cartographer_ros/playable_bag.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"
@ -39,20 +36,35 @@ DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
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.");
configuration_basenames, "",
"Comma-separated list of basenames, i.e. not containing any "
"directory prefix, of the configuration files for each trajectory. "
"The first configuration file will be used for node options."
"If less configuration files are specified than trajectories, the "
"first file will be used the remaining trajectories.");
DEFINE_string(
bag_filenames, "",
"Comma-separated list of bags to process. One bag per trajectory.");
DEFINE_string(urdf_filenames, "",
"Comma-separated list of one or more URDF files that contain "
"static links for the sensor configuration(s).");
DEFINE_bool(use_bag_transforms, true,
"Whether to read, use and republish the transforms from the bag.");
"Whether to read, use and republish transforms from bags.");
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.");
DEFINE_string(sensor_topics, "",
"Optional comma-separated list of colon-separated lists of "
"sensor topics for each trajectory. A single trajectory's topics "
"are delimited with colons, while different trajectories are "
"delimited with commas. If a blank list is given for a "
"trajectory, the default topics are used. If a single "
"colon-separated list is given, it will be used for all "
"trajectories. If omitted, default topics will be used "
"for all trajectories.");
namespace cartographer_ros {
@ -61,19 +73,37 @@ constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// 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 = ::ros::Duration(1.0);
void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
CHECK(!FLAGS_configuration_basenames.empty())
<< "-configuration_basenames is missing.";
CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
const auto bag_filenames =
cartographer_ros::SplitString(FLAGS_bag_filenames, ',');
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);
const auto configuration_basenames =
cartographer_ros::SplitString(FLAGS_configuration_basenames, ',');
std::vector<TrajectoryOptions> bag_trajectory_options(1);
std::tie(node_options, bag_trajectory_options.at(0)) =
LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));
for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
TrajectoryOptions current_trajectory_options;
if (bag_index < configuration_basenames.size()) {
std::tie(std::ignore, current_trajectory_options) = LoadOptions(
FLAGS_configuration_directory, configuration_basenames.at(bag_index));
} else {
current_trajectory_options = bag_trajectory_options.at(0);
}
bag_trajectory_options.push_back(current_trajectory_options);
}
CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
// 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
@ -88,9 +118,13 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
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);
for (const std::string& urdf_filename :
cartographer_ros::SplitString(FLAGS_urdf_filenames, ',')) {
const auto current_urdf_transforms =
ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
urdf_transforms.insert(urdf_transforms.end(),
current_urdf_transforms.begin(),
current_urdf_transforms.end());
}
tf_buffer.setUsingDedicatedThread(true);
@ -102,13 +136,6 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
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);
@ -133,101 +160,155 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
},
false /* oneshot */, false /* autostart */);
for (const std::string& bag_filename : bag_filenames) {
// Colon-delimited string lists of sensor topics for each bag.
std::vector<std::string> bag_sensor_topics_strings;
if (FLAGS_sensor_topics.empty()) {
// Use default topic names for all bags, denoted by an empty sensor
// topic list string for each bag.
bag_sensor_topics_strings =
std::vector<std::string>(bag_filenames.size(), std::string());
} else {
bag_sensor_topics_strings =
cartographer_ros::SplitString(FLAGS_sensor_topics, ',');
if (bag_sensor_topics_strings.size() == 1) {
// Use the single specified topic list string for all bags.
bag_sensor_topics_strings.insert(bag_sensor_topics_strings.end(),
bag_filenames.size() - 1,
bag_sensor_topics_strings.at(0));
}
}
CHECK_EQ(bag_sensor_topics_strings.size(), bag_filenames.size());
std::vector<std::unordered_set<std::string>> bag_sensor_topics;
std::unordered_map<int, int> bag_index_to_trajectory_id;
PlayableBagMultiplexer playable_bag_multiplexer;
for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
++current_bag_index) {
const std::string& bag_filename = bag_filenames.at(current_bag_index);
if (!::ros::ok()) {
break;
return;
}
std::vector<std::string> unresolved_current_bag_sensor_topics;
if (bag_sensor_topics_strings.at(current_bag_index).empty()) {
// Empty topic list string provided for this trajectory,
// use default topics.
const auto default_topics = node.ComputeDefaultTopics(
bag_trajectory_options.at(current_bag_index));
unresolved_current_bag_sensor_topics = std::vector<std::string>(
default_topics.begin(), default_topics.end());
} else {
unresolved_current_bag_sensor_topics = cartographer_ros::SplitString(
bag_sensor_topics_strings.at(current_bag_index), ':');
}
std::unordered_set<std::string> current_bag_sensor_topics;
for (const auto& topic : unresolved_current_bag_sensor_topics) {
CHECK(current_bag_sensor_topics
.insert(node.node_handle()->resolveName(topic))
.second);
}
bag_sensor_topics.push_back(current_bag_sensor_topics);
const int trajectory_id =
node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options);
playable_bag_multiplexer.AddPlayableBag(PlayableBag(
bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
// PlayableBag::FilteringEarlyMessageHandler is used to get an early
// peek at the tf messages in the bag and insert them into 'tf_buffer'.
// When a message is retrieved by GetNextMessage() further below,
// we will have already inserted further 'kDelay' seconds worth of
// transforms into 'tf_buffer' via this lambda.
[&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
if (msg.isType<tf2_msgs::TFMessage>()) {
if (FLAGS_use_bag_transforms) {
const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
tf_publisher.publish(tf_message);
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();
for (const auto& transform : tf_message->transforms) {
try {
// 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.
tf_buffer.setTransform(transform, "unused_authority",
msg.getTopic() == kTfStaticTopic);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
}
// Tell 'PlayableBag' to filter the tf message since there is no
// further use for it.
return false;
} else {
return true;
}
}
}
}));
}
CHECK_EQ(bag_sensor_topics.size(), bag_filenames.size());
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>());
}
if (delayed_msg.isType<sensor_msgs::NavSatFix>()) {
node.HandleNavSatFixMessage(
trajectory_id, topic,
delayed_msg.instantiate<sensor_msgs::NavSatFix>());
}
clock.clock = delayed_msg.getTime();
clock_publisher.publish(clock);
while (playable_bag_multiplexer.IsMessageAvailable()) {
const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
const int bag_index = std::get<1>(next_msg_tuple);
const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);
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);
int trajectory_id;
// Lazily add trajectories only when the first message arrives in order
// to avoid blocking the sensor queue.
if (bag_index_to_trajectory_id.count(bag_index) == 0) {
trajectory_id =
node.AddOfflineTrajectory(bag_sensor_topics.at(bag_index),
bag_trajectory_options.at(bag_index));
CHECK(bag_index_to_trajectory_id
.emplace(std::piecewise_construct,
std::forward_as_tuple(bag_index),
std::forward_as_tuple(trajectory_id))
.second);
LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
<< bag_filenames.at(bag_index);
} else {
trajectory_id = bag_index_to_trajectory_id.at(bag_index);
}
bag.close();
node.FinishTrajectory(trajectory_id);
if (!::ros::ok()) {
return;
}
const std::string topic =
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
if (bag_sensor_topics.at(bag_index).count(topic) == 0) {
continue;
}
if (msg.isType<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::LaserScan>());
}
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.HandleMultiEchoLaserScanMessage(
trajectory_id, topic,
msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (msg.isType<sensor_msgs::PointCloud2>()) {
node.HandlePointCloud2Message(
trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
}
if (msg.isType<sensor_msgs::Imu>()) {
node.HandleImuMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::Imu>());
}
if (msg.isType<nav_msgs::Odometry>()) {
node.HandleOdometryMessage(trajectory_id, topic,
msg.instantiate<nav_msgs::Odometry>());
}
if (msg.isType<sensor_msgs::NavSatFix>()) {
node.HandleNavSatFixMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::NavSatFix>());
}
clock.clock = msg.getTime();
clock_publisher.publish(clock);
if (is_last_message_in_bag) {
node.FinishTrajectory(trajectory_id);
}
}
// Ensure the clock is republished after the bag has been finished, during the

129
cartographer_ros/cartographer_ros/playable_bag.cc Normal file
View File

@ -0,0 +1,129 @@
/*
* 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/playable_bag.h"
#include "cartographer/common/make_unique.h"
#include "cartographer_ros/node_constants.h"
#include "glog/logging.h"
#include "tf2_msgs/TFMessage.h"
namespace cartographer_ros {
PlayableBag::PlayableBag(
const std::string& bag_filename, const int bag_id,
const ros::Time start_time, const ros::Time end_time,
const ros::Duration buffer_delay,
FilteringEarlyMessageHandler filtering_early_message_handler)
: bag_(cartographer::common::make_unique<rosbag::Bag>(
bag_filename, rosbag::bagmode::Read)),
view_(cartographer::common::make_unique<rosbag::View>(*bag_, start_time,
end_time)),
view_iterator_(view_->begin()),
finished_(false),
bag_id_(bag_id),
bag_filename_(bag_filename),
duration_in_seconds_(
(view_->getEndTime() - view_->getBeginTime()).toSec()),
log_counter_(0),
buffer_delay_(buffer_delay),
filtering_early_message_handler_(
std::move(filtering_early_message_handler)) {
AdvanceUntilMessageAvailable();
}
ros::Time PlayableBag::PeekMessageTime() {
CHECK(IsMessageAvailable());
return buffered_messages_.front().getTime();
}
std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() {
return std::make_tuple(view_->getBeginTime(), view_->getEndTime());
}
rosbag::MessageInstance PlayableBag::GetNextMessage() {
CHECK(IsMessageAvailable());
const rosbag::MessageInstance msg = buffered_messages_.front();
buffered_messages_.pop_front();
AdvanceUntilMessageAvailable();
if ((log_counter_++ % 10000) == 0) {
LOG(INFO) << "Processed " << (msg.getTime() - view_->getBeginTime()).toSec()
<< " of " << duration_in_seconds_ << " seconds of bag "
<< bag_filename_;
}
return msg;
}
bool PlayableBag::IsMessageAvailable() {
return !buffered_messages_.empty() &&
(buffered_messages_.front().getTime() <
buffered_messages_.back().getTime() - buffer_delay_);
}
int PlayableBag::bag_id() { return bag_id_; }
void PlayableBag::AdvanceOneMessage() {
CHECK(!finished_);
if (view_iterator_ == view_->end()) {
finished_ = true;
return;
}
rosbag::MessageInstance& msg = *view_iterator_;
if (!filtering_early_message_handler_ ||
filtering_early_message_handler_(msg)) {
buffered_messages_.push_back(msg);
}
++view_iterator_;
}
void PlayableBag::AdvanceUntilMessageAvailable() {
if (finished_) {
return;
}
do {
AdvanceOneMessage();
} while (!finished_ && !IsMessageAvailable());
}
void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
playable_bags_.push_back(std::move(playable_bag));
CHECK(playable_bags_.back().IsMessageAvailable());
next_message_queue_.emplace(
BagMessageItem{playable_bags_.back().PeekMessageTime(),
static_cast<int>(playable_bags_.size() - 1)});
}
bool PlayableBagMultiplexer::IsMessageAvailable() {
return !next_message_queue_.empty();
}
std::tuple<rosbag::MessageInstance, int, bool>
PlayableBagMultiplexer::GetNextMessage() {
CHECK(IsMessageAvailable());
const int current_bag_index = next_message_queue_.top().bag_index;
PlayableBag& current_bag = playable_bags_.at(current_bag_index);
rosbag::MessageInstance msg = current_bag.GetNextMessage();
CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp);
next_message_queue_.pop();
if (current_bag.IsMessageAvailable()) {
next_message_queue_.emplace(
BagMessageItem{current_bag.PeekMessageTime(), current_bag_index});
}
return std::make_tuple(std::move(msg), current_bag.bag_id(),
!current_bag.IsMessageAvailable());
}
} // namespace cartographer_ros

96
cartographer_ros/cartographer_ros/playable_bag.h Normal file
View File

@ -0,0 +1,96 @@
/*
* 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_PLAYABLE_BAG_H_
#define CARTOGRAPHER_ROS_PLAYABLE_BAG_H_
#include <functional>
#include <queue>
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "tf2_ros/buffer.h"
namespace cartographer_ros {
class PlayableBag {
public:
// Handles messages early, i.e. when they are about to enter the buffer.
// Returns a boolean indicating whether the message should enter the buffer.
using FilteringEarlyMessageHandler =
std::function<bool /* forward_message_to_buffer */ (
const rosbag::MessageInstance&)>;
PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time,
ros::Time end_time, ros::Duration buffer_delay,
FilteringEarlyMessageHandler filtering_early_message_handler);
ros::Time PeekMessageTime();
rosbag::MessageInstance GetNextMessage();
bool IsMessageAvailable();
std::tuple<ros::Time, ros::Time> GetBeginEndTime();
int bag_id();
private:
void AdvanceOneMessage();
void AdvanceUntilMessageAvailable();
std::unique_ptr<rosbag::Bag> bag_;
std::unique_ptr<rosbag::View> view_;
rosbag::View::const_iterator view_iterator_;
bool finished_;
const int bag_id_;
const std::string bag_filename_;
const double duration_in_seconds_;
int log_counter_;
std::deque<rosbag::MessageInstance> buffered_messages_;
const ::ros::Duration buffer_delay_;
FilteringEarlyMessageHandler filtering_early_message_handler_;
};
class PlayableBagMultiplexer {
public:
void AddPlayableBag(PlayableBag playable_bag);
// Returns the next message from the multiplexed (merge-sorted) message
// stream, along with the bag id corresponding to the message, and whether
// this was the last message in that bag.
std::tuple<rosbag::MessageInstance, int /* bag_id */,
bool /* is_last_message_in_bag */>
GetNextMessage();
bool IsMessageAvailable();
private:
struct BagMessageItem {
ros::Time message_timestamp;
int bag_index;
struct TimestampIsGreater {
bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
return l.message_timestamp > r.message_timestamp;
}
};
};
std::vector<PlayableBag> playable_bags_;
std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
BagMessageItem::TimestampIsGreater>
next_message_queue_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_PLAYABLE_BAG_H_

4
cartographer_ros/launch/offline_backpack_2d.launch
View File

@ -30,8 +30,8 @@
required="$(arg no_rviz)"
type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua
-urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
-configuration_basenames backpack_2d.lua
-urdf_filenames $(find cartographer_ros)/urdf/backpack_2d.urdf
-bag_filenames $(arg bag_filenames)"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />

4
cartographer_ros/launch/offline_backpack_3d.launch
View File

@ -23,8 +23,8 @@
<node name="cartographer_offline_node" pkg="cartographer_ros"
type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_3d.lua
-urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
-configuration_basenames backpack_3d.lua
-urdf_filenames $(find cartographer_ros)/urdf/backpack_3d.urdf
-bag_filenames $(arg bag_filenames)"
output="screen">
<remap from="points2_1" to="horizontal_laser_3d" />