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

Add support for subdividing laser scan messages. (#428) 

This adds an option to subdivide sensor_msgs/LaserScan and
sensor_msgs/MultiEchoLaserScan messages into multiple point
clouds. For slow spinning laser scanners this allows unwarping
using googlecartographer/cartographer#408.
master
Wolfgang Hess 2017-07-19 14:15:11 +02:00 committed by GitHub
parent b12e32d6fc
commit c58a262d56
13 changed files with 71 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer_ros/cartographer_ros/configuration_files_test.cc
View File

@ -20,6 +20,7 @@
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h"
#include "gtest/gtest.h"
#include "ros/package.h"

1
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -50,6 +50,7 @@ int MapBuilderBridge::AddTrajectory(
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] =
cartographer::common::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_.GetTrajectoryBuilder(trajectory_id));

1
cartographer_ros/cartographer_ros/node_options.h
View File

@ -21,7 +21,6 @@
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/trajectory_options.h"
namespace cartographer_ros {

36
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -39,10 +39,11 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
} // namespace
SensorBridge::SensorBridge(
const string& tracking_frame, const double lookup_transform_timeout_sec,
tf2_ros::Buffer* const tf_buffer,
const int num_subdivisions_per_laser_scan, const string& tracking_frame,
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilder* const trajectory_builder)
: tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {}
void SensorBridge::HandleOdometryMessage(
@ -87,15 +88,17 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
void SensorBridge::HandleLaserScanMessage(
const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg).points);
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg).points,
msg->time_increment);
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg).points);
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg).points,
msg->time_increment);
}
void SensorBridge::HandlePointCloud2Message(
@ -112,6 +115,25 @@ void SensorBridge::HandlePointCloud2Message(
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
void SensorBridge::HandleLaserScan(const string& sensor_id,
const carto::common::Time start_time,
const string& frame_id,
const carto::sensor::PointCloud& points,
const double seconds_between_points) {
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
const carto::sensor::PointCloud subdivision(points.begin() + start_index,
points.begin() + end_index);
const carto::common::Time subdivision_time =
start_time + carto::common::FromSeconds((start_index + end_index) / 2. *
seconds_between_points);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
void SensorBridge::HandleRangefinder(const string& sensor_id,
const carto::common::Time time,
const string& frame_id,

12
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -36,8 +36,8 @@ namespace cartographer_ros {
class SensorBridge {
public:
explicit SensorBridge(
const string& tracking_frame, double lookup_transform_timeout_sec,
tf2_ros::Buffer* tf_buffer,
int num_subdivisions_per_laser_scan, const string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
@ -58,11 +58,17 @@ class SensorBridge {
const TfBridge& tf_bridge() const;
private:
void HandleLaserScan(const string& sensor_id,
::cartographer::common::Time start_time,
const string& frame_id,
const ::cartographer::sensor::PointCloud& points,
double seconds_between_points);
void HandleRangefinder(const string& sensor_id,
const ::cartographer::common::Time time,
::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::PointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
};

32
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

@ -14,12 +14,26 @@
* limitations under the License.
*/
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h"
#include "glog/logging.h"
namespace cartographer_ros {
namespace {
void CheckTrajectoryOptions(const TrajectoryOptions& options) {
CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
1)
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
}
} // namespace
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* const
lua_parameter_dictionary) {
@ -38,16 +52,12 @@ TrajectoryOptions CreateTrajectoryOptions(
options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
options.use_multi_echo_laser_scan =
lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
options.num_subdivisions_per_laser_scan =
lua_parameter_dictionary->GetNonNegativeInt(
"num_subdivisions_per_laser_scan");
options.num_point_clouds =
lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
1)
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
CheckTrajectoryOptions(options);
return options;
}
@ -60,12 +70,15 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
options->use_odometry = msg.use_odometry;
options->use_laser_scan = msg.use_laser_scan;
options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
options->num_subdivisions_per_laser_scan =
msg.num_subdivisions_per_laser_scan;
options->num_point_clouds = msg.num_point_clouds;
if (!options->trajectory_builder_options.ParseFromString(
msg.trajectory_builder_options_proto)) {
LOG(ERROR) << "Failed to parse protobuf";
return false;
}
CheckTrajectoryOptions(*options);
return true;
}
@ -79,6 +92,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
msg.use_odometry = options.use_odometry;
msg.use_laser_scan = options.use_laser_scan;
msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan;
msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
msg.num_point_clouds = options.num_point_clouds;
options.trajectory_builder_options.SerializeToString(
&msg.trajectory_builder_options_proto);

1
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -36,6 +36,7 @@ struct TrajectoryOptions {
bool use_odometry;
bool use_laser_scan;
bool use_multi_echo_laser_scan;
int num_subdivisions_per_laser_scan;
int num_point_clouds;
};

2
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -26,6 +26,7 @@ options = {
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = true,
num_subdivisions_per_laser_scan = 10,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
@ -34,5 +35,6 @@ options = {
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.scans_per_accumulation = 10
return options

1
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -26,6 +26,7 @@ options = {
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = false,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 2,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

1
cartographer_ros/configuration_files/pr2.lua
View File

@ -26,6 +26,7 @@ options = {
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

1
cartographer_ros/configuration_files/revo_lds.lua
View File

@ -26,6 +26,7 @@ options = {
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

1
cartographer_ros/configuration_files/taurob_tracker.lua
View File

@ -26,6 +26,7 @@ options = {
use_odometry = true,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,

1
cartographer_ros_msgs/msg/TrajectoryOptions.msg
View File

@ -19,6 +19,7 @@ bool provide_odom_frame
bool use_odometry
bool use_laser_scan
bool use_multi_echo_laser_scan
int32 num_subdivisions_per_laser_scan
int32 num_point_clouds
# This is a binary-encoded