Adds multi-trajectory support to offline mapping. (#204)
parent
78c14d50d2
commit
42440452b6
|
@ -59,7 +59,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
|
|||
if (trajectory_nodes.empty()) {
|
||||
LOG(WARNING) << "No data was collected and no assets will be written.";
|
||||
} else {
|
||||
LOG(INFO) << "Writing assets to '" << stem << "'...";
|
||||
LOG(INFO) << "Writing assets with stem '" << stem << "'...";
|
||||
cartographer_ros::WriteAssets(trajectory_nodes, options_, stem);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -14,6 +14,7 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
|
@ -37,8 +38,7 @@ DEFINE_string(configuration_directory, "",
|
|||
DEFINE_string(configuration_basename, "",
|
||||
"Basename, i.e. not containing any directory prefix, of the "
|
||||
"configuration file.");
|
||||
// TODO(damonkohler): Support multi-trajectory.
|
||||
DEFINE_string(bag_filename, "", "Bag to process.");
|
||||
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.");
|
||||
|
@ -49,8 +49,17 @@ namespace {
|
|||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||
constexpr char kClockTopic[] = "clock";
|
||||
|
||||
void Run() {
|
||||
// TODO(damonkohler): Pull out this common code across binaries.
|
||||
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;
|
||||
}
|
||||
|
||||
void Run(std::vector<string> bag_filenames) {
|
||||
auto file_resolver = cartographer::common::make_unique<
|
||||
cartographer::common::ConfigurationFileResolver>(
|
||||
std::vector<string>{FLAGS_configuration_directory});
|
||||
|
@ -106,24 +115,27 @@ void Run() {
|
|||
node.node_handle()->resolveName(kOdometryTopic, false /* remap */));
|
||||
}
|
||||
|
||||
// TODO(damonkohler): Support multi-trajectory.
|
||||
const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
|
||||
expected_sensor_ids, options.tracking_frame);
|
||||
|
||||
::ros::Publisher clock_publisher =
|
||||
node.node_handle()->advertise<rosgraph_msgs::Clock>(
|
||||
kClockTopic, kLatestOnlyPublisherQueueSize);
|
||||
|
||||
rosbag::Bag bag;
|
||||
bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
|
||||
for (const rosbag::MessageInstance& msg : rosbag::View(bag)) {
|
||||
for (const string& bag_filename : bag_filenames) {
|
||||
if (!::ros::ok()) {
|
||||
break;
|
||||
return;
|
||||
}
|
||||
|
||||
const string& topic = node.node_handle()->resolveName(msg.getTopic());
|
||||
if (expected_sensor_ids.count(
|
||||
node.node_handle()->resolveName(msg.getTopic())) == 0) {
|
||||
const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
|
||||
expected_sensor_ids, options.tracking_frame);
|
||||
rosbag::Bag bag;
|
||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||
|
||||
for (const rosbag::MessageInstance& msg : rosbag::View(bag)) {
|
||||
if (!::ros::ok()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const string topic = node.node_handle()->resolveName(msg.getTopic());
|
||||
if (expected_sensor_ids.count(topic) == 0) {
|
||||
continue;
|
||||
}
|
||||
if (msg.isType<sensor_msgs::LaserScan>()) {
|
||||
|
@ -148,7 +160,8 @@ void Run() {
|
|||
} else if (msg.isType<nav_msgs::Odometry>()) {
|
||||
node.map_builder_bridge()
|
||||
->sensor_bridge(trajectory_id)
|
||||
->HandleOdometryMessage(topic, msg.instantiate<nav_msgs::Odometry>());
|
||||
->HandleOdometryMessage(topic,
|
||||
msg.instantiate<nav_msgs::Odometry>());
|
||||
}
|
||||
|
||||
rosgraph_msgs::Clock clock;
|
||||
|
@ -157,10 +170,12 @@ void Run() {
|
|||
|
||||
::ros::spinOnce();
|
||||
}
|
||||
bag.close();
|
||||
|
||||
bag.close();
|
||||
node.map_builder_bridge()->FinishTrajectory(trajectory_id);
|
||||
node.map_builder_bridge()->WriteAssets(FLAGS_bag_filename);
|
||||
}
|
||||
|
||||
node.map_builder_bridge()->WriteAssets(bag_filenames.front());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
@ -174,14 +189,15 @@ 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_urdf_filename.empty()) << "-urdf_filename is missing.";
|
||||
|
||||
::ros::init(argc, argv, "cartographer_offline_node");
|
||||
::ros::start();
|
||||
|
||||
cartographer_ros::ScopedRosLogSink ros_log_sink;
|
||||
cartographer_ros::Run();
|
||||
cartographer_ros::Run(
|
||||
cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
|
||||
|
||||
::ros::shutdown();
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue