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

Support multiple trajectories in the asset_writer_main (#395)

master
Holger Rapp 2017-06-26 12:40:38 +02:00 committed by GitHub
parent 04df159d82
commit ecc2f08828
7 changed files with 148 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -94,7 +94,7 @@ void Write3DAssets(
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->time = node.time();
points_batch->origin = range_data.origin;
points_batch->trajectory_index = trajectory_id;
points_batch->trajectory_id = trajectory_id;
points_batch->points = range_data.returns;
ply_writing_points_processor.Process(std::move(points_batch));
}

107
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -31,6 +31,7 @@
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/split_string.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/urdf_reader.h"
#include "gflags/gflags.h"
@ -54,7 +55,9 @@ DEFINE_string(configuration_basename, "",
DEFINE_string(
urdf_filename, "",
"URDF file that contains static links for your sensor configuration.");
DEFINE_string(bag_filename, "", "Bag to process.");
DEFINE_string(bag_filenames, "",
"Bags to process, must be in the same order as the trajectories "
"in 'pose_graph_filename'.");
DEFINE_string(
pose_graph_filename, "",
"Proto containing the pose graph written by /write_assets service.");
@ -100,17 +103,15 @@ void ReadTransformsFromBag(const string& bag_filename,
}
template <typename T>
void HandleMessage(
std::unique_ptr<carto::io::PointsBatch> HandleMessage(
const T& message, 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) {
transform_interpolation_buffer) {
const carto::common::Time time = FromRos(message.header.stamp);
if (!transform_interpolation_buffer.Has(time)) {
return;
return nullptr;
}
const carto::transform::Rigid3d tracking_to_map =
transform_interpolation_buffer.Lookup(time);
const carto::transform::Rigid3d sensor_to_tracking =
@ -119,23 +120,24 @@ void HandleMessage(
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 = message.header.frame_id;
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->time = time;
points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
points_batch->frame_id = message.header.frame_id;
carto::sensor::PointCloudWithIntensities point_cloud =
ToPointCloudWithIntensities(message);
CHECK(point_cloud.intensities.size() == point_cloud.points.size());
for (size_t i = 0; i < point_cloud.points.size(); ++i) {
batch->points.push_back(sensor_to_map * point_cloud.points[i]);
batch->intensities.push_back(point_cloud.intensities[i]);
points_batch->points.push_back(sensor_to_map * point_cloud.points[i]);
points_batch->intensities.push_back(point_cloud.intensities[i]);
}
pipeline.back()->Process(std::move(batch));
return points_batch;
}
void Run(const string& pose_graph_filename, const string& bag_filename,
void Run(const string& pose_graph_filename,
const std::vector<string>& bag_filenames,
const string& configuration_directory,
const string& configuration_basename, const string& urdf_filename) {
auto file_resolver =
@ -149,26 +151,38 @@ void Run(const string& pose_graph_filename, const string& bag_filename,
std::ifstream stream(pose_graph_filename.c_str());
carto::mapping::proto::SparsePoseGraph pose_graph_proto;
CHECK(pose_graph_proto.ParseFromIstream(&stream));
CHECK_EQ(pose_graph_proto.trajectory_size(), 1)
<< "Only pose graphs containing a single trajectory are supported.";
const carto::mapping::proto::Trajectory& trajectory_proto =
pose_graph_proto.trajectory(0);
CHECK_GT(trajectory_proto.node_size(), 0)
<< "Trajectory does not contain any nodes.";
CHECK_EQ(pose_graph_proto.trajectory_size(), bag_filenames.size())
<< "Pose graphs contains " << pose_graph_proto.trajectory_size()
<< " trajectories while " << bag_filenames.size()
<< " bags were provided. This tool requires one bag for each "
"trajectory in the same order as the correponding trajectories in the "
"pose graph proto.";
const auto file_writer_factory = [](const string& filename) {
return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
};
carto::io::PointsProcessorPipelineBuilder builder;
carto::io::RegisterBuiltInPointsProcessors(trajectory_proto,
// TODO(hrapp): Pass all trajectories to the PointsProcessors.
carto::io::RegisterBuiltInPointsProcessors(pose_graph_proto.trajectory(0),
file_writer_factory, &builder);
std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get());
const string tracking_frame =
lua_parameter_dictionary.GetString("tracking_frame");
do {
for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size();
++trajectory_id) {
const carto::mapping::proto::Trajectory& trajectory_proto =
pose_graph_proto.trajectory(trajectory_id);
const string& bag_filename = bag_filenames[trajectory_id];
LOG(INFO) << "Processing " << bag_filename << "...";
if (trajectory_proto.node_size() == 0) {
continue;
}
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);
@ -178,40 +192,41 @@ void Run(const string& pose_graph_filename, const string& bag_filename,
ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
}
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);
rosbag::View view(bag);
const ::ros::Time begin_time = view.getBeginTime();
const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
const double duration_in_seconds =
(view.getEndTime() - begin_time).toSec();
for (const rosbag::MessageInstance& message : view) {
std::unique_ptr<carto::io::PointsBatch> points_batch;
if (message.isType<sensor_msgs::PointCloud2>()) {
HandleMessage(*message.instantiate<sensor_msgs::PointCloud2>(),
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
points_batch = HandleMessage(
*message.instantiate<sensor_msgs::PointCloud2>(), tracking_frame,
tf_buffer, *transform_interpolation_buffer);
} else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
points_batch = HandleMessage(
*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
tracking_frame, tf_buffer, *transform_interpolation_buffer);
} else if (message.isType<sensor_msgs::LaserScan>()) {
points_batch = HandleMessage(
*message.instantiate<sensor_msgs::LaserScan>(), tracking_frame,
tf_buffer, *transform_interpolation_buffer);
}
if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
HandleMessage(*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
}
if (message.isType<sensor_msgs::LaserScan>()) {
HandleMessage(*message.instantiate<sensor_msgs::LaserScan>(),
tracking_frame, tf_buffer,
*transform_interpolation_buffer, pipeline);
if (points_batch != nullptr) {
points_batch->trajectory_id = trajectory_id;
pipeline.back()->Process(std::move(points_batch));
}
LOG_EVERY_N(INFO, 100000)
<< "Processed " << (message.getTime() - begin_time).toSec() << " of "
<< duration_in_seconds << " bag time seconds...";
<< "Processed " << (message.getTime() - begin_time).toSec()
<< " of " << duration_in_seconds << " bag time seconds...";
}
bag.close();
}
} while (pipeline.back()->Flush() ==
carto::io::PointsProcessor::FlushResult::kRestartStream);
}
@ -228,11 +243,13 @@ int main(int argc, char** argv) {
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
CHECK(!FLAGS_pose_graph_filename.empty())
<< "-pose_graph_filename is missing.";
::cartographer_ros::Run(FLAGS_pose_graph_filename, FLAGS_bag_filename,
FLAGS_configuration_directory,
FLAGS_configuration_basename, FLAGS_urdf_filename);
::cartographer_ros::Run(
FLAGS_pose_graph_filename,
cartographer_ros::SplitString(FLAGS_bag_filenames, ','),
FLAGS_configuration_directory, FLAGS_configuration_basename,
FLAGS_urdf_filename);
}

11
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -26,6 +26,7 @@
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.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"
@ -62,16 +63,6 @@ volatile std::sig_atomic_t sigint_triggered = 0;
void SigintHandler(int) { sigint_triggered = 1; }
std::vector<string> SplitString(const string& input, const char delimiter) {
std::stringstream stream(input);
string token;
std::vector<string> tokens;
while (std::getline(stream, token, delimiter)) {
tokens.push_back(token);
}
return tokens;
}
// TODO(hrapp): This is duplicated in node_main.cc. Pull out into a config
// unit.
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions() {

34
cartographer_ros/cartographer_ros/split_string.cc Normal file
View File

@ -0,0 +1,34 @@
/*
* 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/split_string.h"
#include <sstream>
namespace cartographer_ros {
std::vector<std::string> SplitString(const std::string& input,
const char delimiter) {
std::istringstream stream(input);
std::string token;
std::vector<std::string> tokens;
while (std::getline(stream, token, delimiter)) {
tokens.push_back(token);
}
return tokens;
}
} // namespace cartographer_ros

30
cartographer_ros/cartographer_ros/split_string.h Normal file
View File

@ -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_SPLIT_STRING_H_
#define CARTOGRAPHER_ROS_SPLIT_STRING_H_
#include <string>
#include <vector>
namespace cartographer_ros {
// Split 'input' at 'delimiter'.
std::vector<std::string> SplitString(const std::string& input, char delimiter);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_SPLIT_STRING_H_

2
cartographer_ros/launch/assets_writer_backpack_2d.launch
View File

@ -20,7 +20,7 @@
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename assets_writer_backpack_2d.lua
-urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
-bag_filename $(arg bag_filenames)
-bag_filenames $(arg bag_filenames)
-pose_graph_filename $(arg pose_graph_filename)"
output="screen">
</node>

2
cartographer_ros/launch/assets_writer_backpack_3d.launch
View File

@ -20,7 +20,7 @@
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename assets_writer_backpack_3d.lua
-urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
-bag_filename $(arg bag_filenames)
-bag_filenames $(arg bag_filenames)
-pose_graph_filename $(arg pose_graph_filename)"
output="screen">
</node>