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

Do not use tf_buffer for keeping all transforms in memory. (#342) 

PAIR=wohe
master
Holger Rapp 2017-05-17 15:45:18 +02:00 committed by GitHub
parent 4cd812d806
commit 8b239ded86
4 changed files with 101 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -29,7 +29,6 @@
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "cartographer_ros/bag_reader.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/urdf_reader.h"
@ -64,8 +63,41 @@ DEFINE_bool(use_bag_transforms, true,
namespace cartographer_ros {
namespace {
constexpr char kTfStaticTopic[] = "/tf_static";
namespace carto = ::cartographer;
// TODO(hrapp): We discovered that using tf_buffer with a large CACHE
// is very inefficient. Switch asset writer to use our own
// TransformInterpolationBuffer.
void ReadTransformsFromBag(const string& bag_filename,
tf2_ros::Buffer* const tf_buffer) {
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();
for (const rosbag::MessageInstance& msg : view) {
if (msg.isType<tf2_msgs::TFMessage>()) {
const auto tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
for (const auto& transform : tf_msg->transforms) {
try {
// TODO(damonkohler): Handle topic remapping.
tf_buffer->setTransform(transform, "unused_authority",
msg.getTopic() == kTfStaticTopic);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
}
LOG_EVERY_N(INFO, 100000)
<< "Processed " << (msg.getTime() - begin_time).toSec() << " of "
<< duration_in_seconds << " bag time seconds...";
}
bag.close();
}
template <typename T>
void HandleMessage(
const T& message, const string& tracking_frame,
@ -128,16 +160,15 @@ void Run(const string& trajectory_filename, const string& bag_filename,
builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get());
auto tf_buffer =
::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
tf2_ros::Buffer tf_buffer(::ros::DURATION_MAX);
if (FLAGS_use_bag_transforms) {
LOG(INFO) << "Pre-loading transforms from bag...";
ReadTransformsFromBag(bag_filename, tf_buffer.get());
ReadTransformsFromBag(bag_filename, &tf_buffer);
}
if (!urdf_filename.empty()) {
ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get());
ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
}
const string tracking_frame =
@ -156,17 +187,17 @@ void Run(const string& trajectory_filename, const string& bag_filename,
for (const rosbag::MessageInstance& message : view) {
if (message.isType<sensor_msgs::PointCloud2>()) {
HandleMessage(*message.instantiate<sensor_msgs::PointCloud2>(),
tracking_frame, *tf_buffer,
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
}
if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
HandleMessage(*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
tracking_frame, *tf_buffer,
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
}
if (message.isType<sensor_msgs::LaserScan>()) {
HandleMessage(*message.instantiate<sensor_msgs::LaserScan>(),
tracking_frame, *tf_buffer,
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
}
LOG_EVERY_N(INFO, 100000)

61
cartographer_ros/cartographer_ros/bag_reader.cc
View File

@ -1,61 +0,0 @@
/*
* 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/bag_reader.h"
#include <string>
#include <vector>
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "tf2_msgs/TFMessage.h"
namespace cartographer_ros {
constexpr char kTfStaticTopic[] = "/tf_static";
void ReadTransformsFromBag(const string& bag_filename,
tf2_ros::Buffer* const tf_buffer) {
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();
for (const rosbag::MessageInstance& msg : view) {
if (msg.isType<tf2_msgs::TFMessage>()) {
const auto tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
for (const auto& transform : tf_msg->transforms) {
try {
// TODO(damonkohler): Handle topic remapping.
tf_buffer->setTransform(transform, "unused_authority",
msg.getTopic() == kTfStaticTopic);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
}
LOG_EVERY_N(INFO, 100000)
<< "Processed " << (msg.getTime() - begin_time).toSec() << " of "
<< duration_in_seconds << " bag time seconds...";
}
bag.close();
}
} // namespace cartographer_ros

30
cartographer_ros/cartographer_ros/bag_reader.h
View File

@ -1,30 +0,0 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_ROS_BAG_READER_H_
#define CARTOGRAPHER_ROS_BAG_READER_H_
#include "cartographer/common/port.h"
#include "tf2_ros/buffer.h"
namespace cartographer_ros {
void ReadTransformsFromBag(const string& bag_filename,
tf2_ros::Buffer* const tf_buffer);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_BAG_READER_H_

105
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -23,7 +23,6 @@
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/bag_reader.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/urdf_reader.h"
@ -53,9 +52,10 @@ DEFINE_bool(use_bag_transforms, true,
namespace cartographer_ros {
namespace {
constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr int kLatestOnlyPublisherQueueSize = 1;
volatile std::sig_atomic_t sigint_triggered = 0;
@ -86,29 +86,21 @@ NodeOptions LoadOptions() {
void Run(const std::vector<string>& bag_filenames) {
auto options = LoadOptions();
auto tf_buffer =
::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
if (FLAGS_use_bag_transforms) {
LOG(INFO) << "Pre-loading transforms from bag...";
// TODO(damonkohler): Support multi-trajectory.
CHECK_EQ(bag_filenames.size(), 1);
ReadTransformsFromBag(bag_filenames.back(), tf_buffer.get());
}
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.get());
ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
}
tf_buffer->setUsingDedicatedThread(true);
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.
options.lookup_transform_timeout_sec = 0.;
Node node(options, tf_buffer.get());
Node node(options, &tf_buffer);
node.Initialize();
std::unordered_set<string> expected_sensor_ids;
@ -178,6 +170,10 @@ void Run(const std::vector<string>& bag_filenames) {
const ::ros::Time begin_time = view.getBeginTime();
const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
// We make sure that tf_messages are published before any data messages, so
// that tf lookups always work and that tf_buffer has a small cache size -
// because it gets very inefficient with a large one.
std::deque<rosbag::MessageInstance> delayed_messages;
for (const rosbag::MessageInstance& msg : view) {
if (sigint_triggered) {
break;
@ -186,6 +182,57 @@ void Run(const std::vector<string>& bag_filenames) {
if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
tf_publisher.publish(tf_message);
if (FLAGS_use_bag_transforms) {
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() + ::ros::Duration(1.)) {
const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
const string topic = node.node_handle()->resolveName(
delayed_msg.getTopic(), false /* resolve */);
if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleLaserScanMessage(
topic, delayed_msg.instantiate<sensor_msgs::LaserScan>());
}
if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(
topic,
delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandlePointCloud2Message(
topic, delayed_msg.instantiate<sensor_msgs::PointCloud2>());
}
if (delayed_msg.isType<sensor_msgs::Imu>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleImuMessage(topic,
delayed_msg.instantiate<sensor_msgs::Imu>());
}
if (delayed_msg.isType<nav_msgs::Odometry>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleOdometryMessage(
topic, delayed_msg.instantiate<nav_msgs::Odometry>());
}
delayed_messages.pop_front();
}
const string topic =
@ -193,35 +240,7 @@ void Run(const std::vector<string>& bag_filenames) {
if (expected_sensor_ids.count(topic) == 0) {
continue;
}
if (msg.isType<sensor_msgs::LaserScan>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleLaserScanMessage(topic,
msg.instantiate<sensor_msgs::LaserScan>());
}
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(
topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (msg.isType<sensor_msgs::PointCloud2>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandlePointCloud2Message(
topic, msg.instantiate<sensor_msgs::PointCloud2>());
}
if (msg.isType<sensor_msgs::Imu>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleImuMessage(topic, msg.instantiate<sensor_msgs::Imu>());
}
if (msg.isType<nav_msgs::Odometry>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleOdometryMessage(topic,
msg.instantiate<nav_msgs::Odometry>());
}
delayed_messages.push_back(msg);
rosgraph_msgs::Clock clock;
clock.clock = msg.getTime();