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

Do not forget to finish trajectory if last message is not from a sensor topic (#681) 

Bug introduced in #636.
master
Juraj Oršulić 2018-01-25 16:13:38 +01:00 committed by Wally B. Feed
parent 8d8a86790a
commit 7fc931688c
1 changed files with 26 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
cartographer_ros/cartographer_ros/offline_node.cc
View File

@ -274,34 +274,32 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
}
const std::string topic =
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
if (bag_sensor_topics.at(bag_index).count(topic) == 0) {
continue;
}
if (msg.isType<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::LaserScan>());
}
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.HandleMultiEchoLaserScanMessage(
trajectory_id, topic,
msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (msg.isType<sensor_msgs::PointCloud2>()) {
node.HandlePointCloud2Message(
trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
}
if (msg.isType<sensor_msgs::Imu>()) {
node.HandleImuMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::Imu>());
}
if (msg.isType<nav_msgs::Odometry>()) {
node.HandleOdometryMessage(trajectory_id, topic,
msg.instantiate<nav_msgs::Odometry>());
}
if (msg.isType<sensor_msgs::NavSatFix>()) {
node.HandleNavSatFixMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::NavSatFix>());
if (bag_sensor_topics.at(bag_index).count(topic) > 0) {
if (msg.isType<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::LaserScan>());
}
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.HandleMultiEchoLaserScanMessage(
trajectory_id, topic,
msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (msg.isType<sensor_msgs::PointCloud2>()) {
node.HandlePointCloud2Message(
trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
}
if (msg.isType<sensor_msgs::Imu>()) {
node.HandleImuMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::Imu>());
}
if (msg.isType<nav_msgs::Odometry>()) {
node.HandleOdometryMessage(trajectory_id, topic,
msg.instantiate<nav_msgs::Odometry>());
}
if (msg.isType<sensor_msgs::NavSatFix>()) {
node.HandleNavSatFixMessage(trajectory_id, topic,
msg.instantiate<sensor_msgs::NavSatFix>());
}
}
clock.clock = msg.getTime();
clock_publisher.publish(clock);