Do not drop data when falling behind. (#68)

Dropping data significantly harms quality of SLAM. This change means that
all data will be kept. If the system falls behind only for a short period
of time this is preferable. Publishing of the pose and submaps does not
work in this case and needs improvement.
master
Wolfgang Hess 2016-09-26 16:06:58 +02:00 committed by GitHub
parent 87a2437d04
commit 9103b846ff
2 changed files with 13 additions and 11 deletions

View File

@ -37,6 +37,6 @@ options = {
pose_publish_period_sec = 5e-3, pose_publish_period_sec = 5e-3,
} }
options.map_builder.use_trajectory_builder_2d = true MAP_BUILDER.use_trajectory_builder_2d = true
return options return options

View File

@ -87,7 +87,8 @@ using carto::kalman_filter::PoseCovariance;
// TODO(hrapp): Support multi trajectory mapping. // TODO(hrapp): Support multi trajectory mapping.
constexpr int64 kTrajectoryBuilderId = 0; constexpr int64 kTrajectoryBuilderId = 0;
constexpr int kSubscriberQueueSize = 150; constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
// Unique default topic names. Expected to be remapped as needed. // Unique default topic names. Expected to be remapped as needed.
@ -384,7 +385,7 @@ void Node::Initialize() {
// For 2D SLAM, subscribe to exactly one horizontal laser. // For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_horizontal_laser) { if (options_.use_horizontal_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe( horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kLaserScanTopic, kSubscriberQueueSize, kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) { [this](const sensor_msgs::LaserScan::ConstPtr& msg) {
sensor_data_producer_.AddLaserScanMessage(kLaserScanTopic, msg); sensor_data_producer_.AddLaserScanMessage(kLaserScanTopic, msg);
@ -393,7 +394,7 @@ void Node::Initialize() {
} }
if (options_.use_horizontal_multi_echo_laser) { if (options_.use_horizontal_multi_echo_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe( horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kSubscriberQueueSize, kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
sensor_data_producer_.AddMultiEchoLaserScanMessage( sensor_data_producer_.AddMultiEchoLaserScanMessage(
@ -410,7 +411,7 @@ void Node::Initialize() {
topic += "_" + std::to_string(i + 1); topic += "_" + std::to_string(i + 1);
} }
laser_subscribers_3d_.push_back(node_handle_.subscribe( laser_subscribers_3d_.push_back(node_handle_.subscribe(
topic, kSubscriberQueueSize, topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>( boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
sensor_data_producer_.AddPointCloud2Message(topic, msg); sensor_data_producer_.AddPointCloud2Message(topic, msg);
@ -426,7 +427,7 @@ void Node::Initialize() {
options_.map_builder_options.trajectory_builder_2d_options() options_.map_builder_options.trajectory_builder_2d_options()
.use_imu_data())) { .use_imu_data())) {
imu_subscriber_ = node_handle_.subscribe( imu_subscriber_ = node_handle_.subscribe(
kImuTopic, kSubscriberQueueSize, kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>( boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](const sensor_msgs::Imu::ConstPtr& msg) { [this](const sensor_msgs::Imu::ConstPtr& msg) {
sensor_data_producer_.AddImuMessage(kImuTopic, msg); sensor_data_producer_.AddImuMessage(kImuTopic, msg);
@ -436,7 +437,7 @@ void Node::Initialize() {
if (options_.use_odometry_data) { if (options_.use_odometry_data) {
odometry_subscriber_ = node_handle_.subscribe( odometry_subscriber_ = node_handle_.subscribe(
kOdometryTopic, kSubscriberQueueSize, kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>( boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) { [this](const nav_msgs::Odometry::ConstPtr& msg) {
sensor_data_producer_.AddOdometryMessage(kOdometryTopic, msg); sensor_data_producer_.AddOdometryMessage(kOdometryTopic, msg);
@ -454,21 +455,22 @@ void Node::Initialize() {
submap_list_publisher_ = submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, 10); kSubmapListTopic, kLatestOnlyPublisherQueueSize);
submap_query_server_ = node_handle_.advertiseService( submap_query_server_ = node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
if (options_.publish_occupancy_grid) { if (options_.publish_occupancy_grid) {
occupancy_grid_publisher_ = occupancy_grid_publisher_ =
node_handle_.advertise<::nav_msgs::OccupancyGrid>(kOccupancyGridTopic, node_handle_.advertise<::nav_msgs::OccupancyGrid>(
1, true); kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
true /* latched */);
occupancy_grid_thread_ = occupancy_grid_thread_ =
std::thread(&Node::SpinOccupancyGridThreadForever, this); std::thread(&Node::SpinOccupancyGridThreadForever, this);
} }
scan_matched_point_cloud_publisher_ = scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>( node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, 10); kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(options_.submap_publish_period_sec), ::ros::WallDuration(options_.submap_publish_period_sec),