Optionally read /tf from bag in place of a URDF. (#213)
Damon Kohler
GitHub
parent
c3a319cabf
commit
cff972ddf2
|
|
@ -10,6 +10,15 @@ google_library(assets_writer
|
|||
occupancy_grid
|
||||
)
|
||||
|
||||
google_library(bag_reader
|
||||
USES_CARTOGRAPHER
|
||||
USES_GLOG
|
||||
SRCS
|
||||
bag_reader.cc
|
||||
HDRS
|
||||
bag_reader.h
|
||||
)
|
||||
|
||||
google_library(map_builder_bridge
|
||||
USES_CARTOGRAPHER
|
||||
SRCS
|
||||
|
|
@ -158,6 +167,7 @@ google_binary(cartographer_assets_writer
|
|||
SRCS
|
||||
assets_writer_main.cc
|
||||
DEPENDS
|
||||
bag_reader
|
||||
msg_conversion
|
||||
time_conversion
|
||||
urdf_reader
|
||||
|
|
@ -188,6 +198,7 @@ google_binary(cartographer_offline_node
|
|||
SRCS
|
||||
offline_node_main.cc
|
||||
DEPENDS
|
||||
bag_reader
|
||||
node
|
||||
ros_log_sink
|
||||
urdf_reader
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include "cartographer/io/points_processor.h"
|
||||
#include "cartographer/io/points_processor_pipeline_builder.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"
|
||||
|
|
@ -34,6 +35,7 @@
|
|||
#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"
|
||||
|
||||
|
|
@ -57,20 +59,43 @@ namespace {
|
|||
|
||||
namespace carto = ::cartographer;
|
||||
|
||||
void HandlePointCloud2Message(
|
||||
const sensor_msgs::PointCloud2::ConstPtr& msg,
|
||||
const string& tracking_frame,
|
||||
const tf2_ros::Buffer& tf_buffer,
|
||||
const carto::transform::TransformInterpolationBuffer&
|
||||
transform_interpolation_buffer,
|
||||
const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline) {
|
||||
const carto::common::Time time = FromRos(msg->header.stamp);
|
||||
if (!transform_interpolation_buffer.Has(time)) {
|
||||
return;
|
||||
}
|
||||
|
||||
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, msg->header.frame_id,
|
||||
msg->header.stamp));
|
||||
const carto::transform::Rigid3f sensor_to_map =
|
||||
(tracking_to_map * sensor_to_tracking).cast<float>();
|
||||
|
||||
auto batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||
batch->time = time;
|
||||
batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
|
||||
batch->frame_id = msg->header.frame_id;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
||||
for (const auto& point : pcl_point_cloud) {
|
||||
batch->points.push_back(sensor_to_map *
|
||||
Eigen::Vector3f(point.x, point.y, point.z));
|
||||
}
|
||||
pipeline.back()->Process(std::move(batch));
|
||||
}
|
||||
|
||||
void Run(const string& trajectory_filename, const string& bag_filename,
|
||||
const string& configuration_directory,
|
||||
const string& configuration_basename, const string& urdf_filename) {
|
||||
std::ifstream stream(trajectory_filename.c_str());
|
||||
carto::mapping::proto::Trajectory trajectory_proto;
|
||||
CHECK(trajectory_proto.ParseFromIstream(&stream));
|
||||
|
||||
auto transform_interpolation_buffer =
|
||||
carto::transform::TransformInterpolationBuffer::FromTrajectory(
|
||||
trajectory_proto);
|
||||
|
||||
carto::io::PointsProcessorPipelineBuilder builder;
|
||||
carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
|
||||
|
||||
auto file_resolver =
|
||||
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
|
||||
std::vector<string>{configuration_directory});
|
||||
|
|
@ -78,50 +103,31 @@ void Run(const string& trajectory_filename, const string& bag_filename,
|
|||
file_resolver->GetFileContentOrDie(configuration_basename);
|
||||
carto::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||
code, std::move(file_resolver));
|
||||
const string tracking_frame =
|
||||
lua_parameter_dictionary.GetString("tracking_frame");
|
||||
|
||||
std::ifstream stream(trajectory_filename.c_str());
|
||||
carto::mapping::proto::Trajectory trajectory_proto;
|
||||
CHECK(trajectory_proto.ParseFromIstream(&stream));
|
||||
|
||||
carto::io::PointsProcessorPipelineBuilder builder;
|
||||
carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
|
||||
std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
|
||||
builder.CreatePipeline(
|
||||
lua_parameter_dictionary.GetDictionary("pipeline").get());
|
||||
|
||||
tf2_ros::Buffer buffer;
|
||||
// TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to
|
||||
// support non-rigid sensor configurations.
|
||||
auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
|
||||
if (!urdf_filename.empty()) {
|
||||
ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
|
||||
ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get());
|
||||
} else {
|
||||
LOG(INFO) << "Pre-loading transforms from bag...";
|
||||
tf_buffer = ReadTransformsFromBag(bag_filename);
|
||||
}
|
||||
|
||||
auto handle_point_cloud_2_message = [&](
|
||||
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
const carto::common::Time time = FromRos(msg->header.stamp);
|
||||
if (!transform_interpolation_buffer->Has(time)) {
|
||||
return;
|
||||
}
|
||||
carto::transform::Rigid3d tracking_to_map =
|
||||
transform_interpolation_buffer->Lookup(time);
|
||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
||||
|
||||
const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d(
|
||||
buffer.lookupTransform(tracking_frame, msg->header.frame_id,
|
||||
msg->header.stamp));
|
||||
|
||||
const carto::transform::Rigid3f sensor_to_map =
|
||||
(tracking_to_map * sensor_to_tracking).cast<float>();
|
||||
|
||||
auto batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||
batch->time = time;
|
||||
batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
|
||||
batch->frame_id = msg->header.frame_id;
|
||||
|
||||
for (const auto& point : pcl_point_cloud) {
|
||||
batch->points.push_back(sensor_to_map *
|
||||
Eigen::Vector3f(point.x, point.y, point.z));
|
||||
}
|
||||
pipeline.back()->Process(std::move(batch));
|
||||
};
|
||||
|
||||
const string tracking_frame =
|
||||
lua_parameter_dictionary.GetString("tracking_frame");
|
||||
const auto transform_interpolation_buffer =
|
||||
carto::transform::TransformInterpolationBuffer::FromTrajectory(
|
||||
trajectory_proto);
|
||||
LOG(INFO) << "Processing pipeline...";
|
||||
do {
|
||||
rosbag::Bag bag;
|
||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||
|
|
@ -131,7 +137,9 @@ void Run(const string& trajectory_filename, const string& bag_filename,
|
|||
|
||||
for (const rosbag::MessageInstance& msg : view) {
|
||||
if (msg.isType<sensor_msgs::PointCloud2>()) {
|
||||
handle_point_cloud_2_message(msg.instantiate<sensor_msgs::PointCloud2>());
|
||||
HandlePointCloud2Message(msg.instantiate<sensor_msgs::PointCloud2>(),
|
||||
tracking_frame, *tf_buffer,
|
||||
*transform_interpolation_buffer, pipeline);
|
||||
}
|
||||
LOG_EVERY_N(INFO, 100000)
|
||||
<< "Processed " << (msg.getTime() - begin_time).toSec() << " of "
|
||||
|
|
@ -146,6 +154,7 @@ void Run(const string& trajectory_filename, const string& bag_filename,
|
|||
} // namespace cartographer_ros
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
FLAGS_alsologtostderr = true;
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
|
|
@ -156,7 +165,6 @@ int main(int argc, char** argv) {
|
|||
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
|
||||
CHECK(!FLAGS_trajectory_filename.empty())
|
||||
<< "-trajectory_filename is missing.";
|
||||
CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
|
||||
|
||||
::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename,
|
||||
FLAGS_configuration_directory,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,64 @@
|
|||
/*
|
||||
* 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";
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
|
||||
const string& bag_filename) {
|
||||
rosbag::Bag bag;
|
||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||
rosbag::View view(bag);
|
||||
|
||||
auto tf_buffer =
|
||||
::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
|
||||
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();
|
||||
return tf_buffer;
|
||||
}
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
/*
|
||||
* 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 {
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
|
||||
const string& bag_filename);
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_BAG_READER_H_
|
||||
|
|
@ -22,6 +22,7 @@
|
|||
#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"
|
||||
|
|
@ -29,6 +30,7 @@
|
|||
#include "rosbag/bag.h"
|
||||
#include "rosbag/view.h"
|
||||
#include "rosgraph_msgs/Clock.h"
|
||||
#include "tf2_msgs/TFMessage.h"
|
||||
#include "urdf/model.h"
|
||||
|
||||
DEFINE_string(configuration_directory, "",
|
||||
|
|
@ -48,6 +50,7 @@ namespace {
|
|||
|
||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||
constexpr char kClockTopic[] = "clock";
|
||||
constexpr char kTfStaticTopic[] = "/tf_static";
|
||||
|
||||
std::vector<string> SplitString(const string& input, const char delimiter) {
|
||||
std::stringstream stream(input);
|
||||
|
|
@ -68,11 +71,23 @@ void Run(std::vector<string> bag_filenames) {
|
|||
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||
code, std::move(file_resolver));
|
||||
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
tf_buffer.setUsingDedicatedThread(true);
|
||||
ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
|
||||
const auto options = CreateNodeOptions(&lua_parameter_dictionary);
|
||||
Node node(options, &tf_buffer);
|
||||
auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
|
||||
if (!FLAGS_urdf_filename.empty()) {
|
||||
ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get());
|
||||
} else {
|
||||
LOG(INFO) << "Pre-loading transforms from bag...";
|
||||
// TODO(damonkohler): Support multi-trajectory.
|
||||
CHECK_EQ(bag_filenames.size(), 1);
|
||||
tf_buffer = ReadTransformsFromBag(bag_filenames.back());
|
||||
}
|
||||
tf_buffer->setUsingDedicatedThread(true);
|
||||
|
||||
auto options = CreateNodeOptions(&lua_parameter_dictionary);
|
||||
// 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.Initialize();
|
||||
|
||||
std::unordered_set<string> expected_sensor_ids;
|
||||
|
|
@ -138,6 +153,7 @@ void Run(std::vector<string> bag_filenames) {
|
|||
return;
|
||||
}
|
||||
|
||||
// TODO(damonkohler): Republish non-conflicting tf messages.
|
||||
const string topic = node.node_handle()->resolveName(msg.getTopic());
|
||||
if (expected_sensor_ids.count(topic) == 0) {
|
||||
continue;
|
||||
|
|
@ -147,21 +163,25 @@ void Run(std::vector<string> bag_filenames) {
|
|||
->sensor_bridge(trajectory_id)
|
||||
->HandleLaserScanMessage(topic,
|
||||
msg.instantiate<sensor_msgs::LaserScan>());
|
||||
} else if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
}
|
||||
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
node.map_builder_bridge()
|
||||
->sensor_bridge(trajectory_id)
|
||||
->HandleMultiEchoLaserScanMessage(
|
||||
topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
|
||||
} else if (msg.isType<sensor_msgs::PointCloud2>()) {
|
||||
}
|
||||
if (msg.isType<sensor_msgs::PointCloud2>()) {
|
||||
node.map_builder_bridge()
|
||||
->sensor_bridge(trajectory_id)
|
||||
->HandlePointCloud2Message(
|
||||
topic, msg.instantiate<sensor_msgs::PointCloud2>());
|
||||
} else if (msg.isType<sensor_msgs::Imu>()) {
|
||||
}
|
||||
if (msg.isType<sensor_msgs::Imu>()) {
|
||||
node.map_builder_bridge()
|
||||
->sensor_bridge(trajectory_id)
|
||||
->HandleImuMessage(topic, msg.instantiate<sensor_msgs::Imu>());
|
||||
} else if (msg.isType<nav_msgs::Odometry>()) {
|
||||
}
|
||||
if (msg.isType<nav_msgs::Odometry>()) {
|
||||
node.map_builder_bridge()
|
||||
->sensor_bridge(trajectory_id)
|
||||
->HandleOdometryMessage(topic,
|
||||
|
|
@ -190,6 +210,7 @@ void Run(std::vector<string> bag_filenames) {
|
|||
} // namespace cartographer_ros
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
FLAGS_alsologtostderr = true;
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
|
|
@ -198,7 +219,6 @@ int main(int argc, char** argv) {
|
|||
CHECK(!FLAGS_configuration_basename.empty())
|
||||
<< "-configuration_basename is missing.";
|
||||
CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
|
||||
CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
|
||||
|
||||
::ros::init(argc, argv, "cartographer_offline_node");
|
||||
::ros::start();
|
||||
|
|
|
|||
Loading…
Reference in New Issue