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

Adds multi-trajectory support to offline mapping. (#204)

master
Damon Kohler 2016-11-30 16:48:03 +01:00 committed by GitHub
parent 78c14d50d2
commit 42440452b6
2 changed files with 67 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -59,7 +59,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
if (trajectory_nodes.empty()) { if (trajectory_nodes.empty()) {
LOG(WARNING) << "No data was collected and no assets will be written."; LOG(WARNING) << "No data was collected and no assets will be written.";
} else { } else {
LOG(INFO) << "Writing assets to '" << stem << "'..."; LOG(INFO) << "Writing assets with stem '" << stem << "'...";
cartographer_ros::WriteAssets(trajectory_nodes, options_, stem); cartographer_ros::WriteAssets(trajectory_nodes, options_, stem);
} }
} }

116
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -14,6 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include <sstream>
#include <string> #include <string>
#include <vector> #include <vector>
@ -37,8 +38,7 @@ DEFINE_string(configuration_directory, "",
DEFINE_string(configuration_basename, "", DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the " "Basename, i.e. not containing any directory prefix, of the "
"configuration file."); "configuration file.");
// TODO(damonkohler): Support multi-trajectory. DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
DEFINE_string(bag_filename, "", "Bag to process.");
DEFINE_string( DEFINE_string(
urdf_filename, "", urdf_filename, "",
"URDF file that contains static links for your sensor configuration."); "URDF file that contains static links for your sensor configuration.");
@ -49,8 +49,17 @@ namespace {
constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr char kClockTopic[] = "clock"; constexpr char kClockTopic[] = "clock";
void Run() { std::vector<string> SplitString(const string& input, const char delimiter) {
// TODO(damonkohler): Pull out this common code across binaries. 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< auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>( cartographer::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory}); std::vector<string>{FLAGS_configuration_directory});
@ -106,61 +115,67 @@ void Run() {
node.node_handle()->resolveName(kOdometryTopic, false /* remap */)); 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 = ::ros::Publisher clock_publisher =
node.node_handle()->advertise<rosgraph_msgs::Clock>( node.node_handle()->advertise<rosgraph_msgs::Clock>(
kClockTopic, kLatestOnlyPublisherQueueSize); kClockTopic, kLatestOnlyPublisherQueueSize);
rosbag::Bag bag; for (const string& bag_filename : bag_filenames) {
bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
for (const rosbag::MessageInstance& msg : rosbag::View(bag)) {
if (!::ros::ok()) { if (!::ros::ok()) {
break; return;
} }
const string& topic = node.node_handle()->resolveName(msg.getTopic()); const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
if (expected_sensor_ids.count( expected_sensor_ids, options.tracking_frame);
node.node_handle()->resolveName(msg.getTopic())) == 0) { rosbag::Bag bag;
continue; bag.open(bag_filename, rosbag::bagmode::Read);
}
if (msg.isType<sensor_msgs::LaserScan>()) { for (const rosbag::MessageInstance& msg : rosbag::View(bag)) {
node.map_builder_bridge() if (!::ros::ok()) {
->sensor_bridge(trajectory_id) return;
->HandleLaserScanMessage(topic, }
msg.instantiate<sensor_msgs::LaserScan>());
} else if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) { const string topic = node.node_handle()->resolveName(msg.getTopic());
node.map_builder_bridge() if (expected_sensor_ids.count(topic) == 0) {
->sensor_bridge(trajectory_id) continue;
->HandleMultiEchoLaserScanMessage( }
topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>()); if (msg.isType<sensor_msgs::LaserScan>()) {
} else if (msg.isType<sensor_msgs::PointCloud2>()) { node.map_builder_bridge()
node.map_builder_bridge() ->sensor_bridge(trajectory_id)
->sensor_bridge(trajectory_id) ->HandleLaserScanMessage(topic,
->HandlePointCloud2Message( msg.instantiate<sensor_msgs::LaserScan>());
topic, msg.instantiate<sensor_msgs::PointCloud2>()); } else if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
} else if (msg.isType<sensor_msgs::Imu>()) { node.map_builder_bridge()
node.map_builder_bridge() ->sensor_bridge(trajectory_id)
->sensor_bridge(trajectory_id) ->HandleMultiEchoLaserScanMessage(
->HandleImuMessage(topic, msg.instantiate<sensor_msgs::Imu>()); topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
} else if (msg.isType<nav_msgs::Odometry>()) { } else if (msg.isType<sensor_msgs::PointCloud2>()) {
node.map_builder_bridge() node.map_builder_bridge()
->sensor_bridge(trajectory_id) ->sensor_bridge(trajectory_id)
->HandleOdometryMessage(topic, msg.instantiate<nav_msgs::Odometry>()); ->HandlePointCloud2Message(
topic, msg.instantiate<sensor_msgs::PointCloud2>());
} else 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>()) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleOdometryMessage(topic,
msg.instantiate<nav_msgs::Odometry>());
}
rosgraph_msgs::Clock clock;
clock.clock = msg.getTime();
clock_publisher.publish(clock);
::ros::spinOnce();
} }
rosgraph_msgs::Clock clock; bag.close();
clock.clock = msg.getTime(); node.map_builder_bridge()->FinishTrajectory(trajectory_id);
clock_publisher.publish(clock);
::ros::spinOnce();
} }
bag.close();
node.map_builder_bridge()->FinishTrajectory(trajectory_id); node.map_builder_bridge()->WriteAssets(bag_filenames.front());
node.map_builder_bridge()->WriteAssets(FLAGS_bag_filename);
} }
} // namespace } // namespace
@ -174,14 +189,15 @@ int main(int argc, char** argv) {
<< "-configuration_directory is missing."; << "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty()) CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing."; << "-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."; CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
::ros::init(argc, argv, "cartographer_offline_node"); ::ros::init(argc, argv, "cartographer_offline_node");
::ros::start(); ::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run(); cartographer_ros::Run(
cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
::ros::shutdown(); ::ros::shutdown();
} }