From 2ec6001f96c9bc678e2ffa6c57e20402ec314cb3 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Tue, 15 Nov 2016 14:47:45 +0100 Subject: [PATCH] New Cartographer API. (#157) --- .../cartographer_ros/sensor_bridge.cc | 16 +++++----------- .../cartographer_ros/sensor_bridge.h | 3 --- .../configuration_files/backpack_2d.lua | 3 --- .../configuration_files/backpack_3d.lua | 3 --- cartographer_ros/configuration_files/pr2.lua | 5 ++--- .../configuration_files/revo_lds.lua | 8 +++++--- docs/source/configuration.rst | 10 ---------- 7 files changed, 12 insertions(+), 36 deletions(-) diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 2178e7d..d78aaaa 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -40,13 +40,6 @@ const string& CheckNoLeadingSlash(const string& frame_id) { SensorBridgeOptions CreateSensorBridgeOptions( carto::common::LuaParameterDictionary* const lua_parameter_dictionary) { SensorBridgeOptions options; - options.horizontal_laser_min_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_min_range"); - options.horizontal_laser_max_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_max_range"); - options.horizontal_laser_missing_echo_ray_length = - lua_parameter_dictionary->GetDouble( - "horizontal_laser_missing_echo_ray_length"); options.constant_odometry_translational_variance = lua_parameter_dictionary->GetDouble( "constant_odometry_translational_variance"); @@ -137,10 +130,11 @@ void SensorBridge::HandlePointCloud2Message( void SensorBridge::HandleLaserScanProto( const string& topic, const carto::common::Time time, const string& frame_id, const carto::sensor::proto::LaserScan& laser_scan) { - const auto laser_fan = carto::sensor::ToLaserFan( - laser_scan, options_.horizontal_laser_min_range, - options_.horizontal_laser_max_range, - options_.horizontal_laser_missing_echo_ray_length); + const carto::sensor::LaserFan laser_fan = { + Eigen::Vector3f::Zero(), + carto::sensor::ToPointCloud(laser_scan), + {}, + {}}; const auto sensor_to_tracking = tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 0840c86..acb6be4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -33,9 +33,6 @@ namespace cartographer_ros { struct SensorBridgeOptions { - double horizontal_laser_min_range; - double horizontal_laser_max_range; - double horizontal_laser_missing_echo_ray_length; double constant_odometry_translational_variance; double constant_odometry_rotational_variance; }; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 89bcf00..6c59d0c 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -17,9 +17,6 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, sensor_bridge = { - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., }, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 5d5282b..095bced 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -17,9 +17,6 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, sensor_bridge = { - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., }, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index a604af2..2873036 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -17,9 +17,6 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, sensor_bridge = { - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., }, @@ -38,10 +35,12 @@ options = { } MAP_BUILDER.use_trajectory_builder_2d = true + TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true TRAJECTORY_BUILDER_2D.use_imu_data = false TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.) + SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2 return options diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index f198fc2..4dc37e6 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -17,9 +17,6 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, sensor_bridge = { - horizontal_laser_min_range = 0.3, - horizontal_laser_max_range = 8., - horizontal_laser_missing_echo_ray_length = 1., constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., }, @@ -38,8 +35,13 @@ options = { } MAP_BUILDER.use_trajectory_builder_2d = true + +TRAJECTORY_BUILDER_2D.laser_min_range = 0.3, +TRAJECTORY_BUILDER_2D.laser_max_range = 8., +TRAJECTORY_BUILDER_2D.laser_missing_echo_ray_length = 1., TRAJECTORY_BUILDER_2D.use_imu_data = false TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true + SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2 return options diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 6c23667..13f3386 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -71,16 +71,6 @@ use_horizontal_multi_echo_laser "echoes" topic. If 2D SLAM is used, either this or *use_horizontal_laser* must be enabled. -sensor_bridge.horizontal_laser_min_range - Range in meters below which laser returns are ignored for the purpose of SLAM. - -sensor_bridge.horizontal_laser_max_range - Range in meters above which laser returns are ignored for the purpose of SLAM. - -sensor_bridge.horizontal_laser_missing_echo_ray_length - Range in meters to use for inserting free space when no laser return was - detected. - num_lasers_3d Number of 3D lasers to subscribe to. Must be a positive value if and only if using 3D SLAM. Subscribes to `sensor_msgs/PointCloud2`_ on the "points2"