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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/configuration_files/backpack_2d.lua
View File

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

22
cartographer_ros/src/cartographer_node_main.cc
View File

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