diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 77b3044..3611c8f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -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; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index babb311..e1e580d 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -122,7 +122,7 @@ Node::Node( &Node::PublishConstraintList, this)); } -Node::~Node() {} +Node::~Node() { FinishAllTrajectories(); } ::ros::NodeHandle* Node::node_handle() { return &node_handle_; } diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index da29368..0361faf 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -21,17 +21,14 @@ #include #include #include -#include #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 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 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 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( 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 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(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> bag_sensor_topics; + std::unordered_map 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 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( + 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 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()) { + if (FLAGS_use_bag_transforms) { + const auto tf_message = msg.instantiate(); + 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 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()) { - auto tf_message = msg.instantiate(); - 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()) { - node.HandleLaserScanMessage( - trajectory_id, topic, - delayed_msg.instantiate()); - } - if (delayed_msg.isType()) { - node.HandleMultiEchoLaserScanMessage( - trajectory_id, topic, - delayed_msg.instantiate()); - } - if (delayed_msg.isType()) { - node.HandlePointCloud2Message( - trajectory_id, topic, - delayed_msg.instantiate()); - } - if (delayed_msg.isType()) { - node.HandleImuMessage(trajectory_id, topic, - delayed_msg.instantiate()); - } - if (delayed_msg.isType()) { - node.HandleOdometryMessage( - trajectory_id, topic, - delayed_msg.instantiate()); - } - if (delayed_msg.isType()) { - node.HandleNavSatFixMessage( - trajectory_id, topic, - delayed_msg.instantiate()); - } - 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()) { + node.HandleLaserScanMessage(trajectory_id, topic, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleMultiEchoLaserScanMessage( + trajectory_id, topic, + msg.instantiate()); + } + if (msg.isType()) { + node.HandlePointCloud2Message( + trajectory_id, topic, msg.instantiate()); + } + if (msg.isType()) { + node.HandleImuMessage(trajectory_id, topic, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleOdometryMessage(trajectory_id, topic, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleNavSatFixMessage(trajectory_id, topic, + msg.instantiate()); + } + 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 diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc new file mode 100644 index 0000000..820706f --- /dev/null +++ b/cartographer_ros/cartographer_ros/playable_bag.cc @@ -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( + bag_filename, rosbag::bagmode::Read)), + view_(cartographer::common::make_unique(*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 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(playable_bags_.size() - 1)}); +} + +bool PlayableBagMultiplexer::IsMessageAvailable() { + return !next_message_queue_.empty(); +} + +std::tuple +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 diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h new file mode 100644 index 0000000..e2a3865 --- /dev/null +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -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 +#include + +#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; + + 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 GetBeginEndTime(); + + int bag_id(); + + private: + void AdvanceOneMessage(); + void AdvanceUntilMessageAvailable(); + + std::unique_ptr bag_; + std::unique_ptr 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 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 + 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 playable_bags_; + std::priority_queue, + BagMessageItem::TimestampIsGreater> + next_message_queue_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_PLAYABLE_BAG_H_ diff --git a/cartographer_ros/launch/offline_backpack_2d.launch b/cartographer_ros/launch/offline_backpack_2d.launch index 5c0cb3a..b6b93cb 100644 --- a/cartographer_ros/launch/offline_backpack_2d.launch +++ b/cartographer_ros/launch/offline_backpack_2d.launch @@ -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"> diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch index e9d7ba8..214b3c0 100644 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ b/cartographer_ros/launch/offline_backpack_3d.launch @@ -23,8 +23,8 @@