New Cartographer API, removes SensorBridgeOptions. (#180)

cartographer_ros/cartographer_ros/node_main.cc

@@ -240,8 +240,7 @@ void Node::Initialize() {
 
   trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
   sensor_bridge_ = carto::common::make_unique<SensorBridge>(
-      options_.sensor_bridge_options, &tf_bridge_,
+      &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
-      map_builder_.GetTrajectoryBuilder(trajectory_id_));
 
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
@@ -315,8 +314,7 @@ bool Node::HandleFinishTrajectory(
   // Start a new trajectory.
   trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
   sensor_bridge_ = carto::common::make_unique<SensorBridge>(
-      options_.sensor_bridge_options, &tf_bridge_,
+      &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
-      map_builder_.GetTrajectoryBuilder(trajectory_id_));
   return true;
 }
 

cartographer_ros/cartographer_ros/node_options.cc

@@ -27,8 +27,6 @@ NodeOptions CreateNodeOptions(
   options.map_builder_options =
       ::cartographer::mapping::CreateMapBuilderOptions(
           lua_parameter_dictionary->GetDictionary("map_builder").get());
-  options.sensor_bridge_options = CreateSensorBridgeOptions(
-      lua_parameter_dictionary->GetDictionary("sensor_bridge").get());
   options.map_frame = lua_parameter_dictionary->GetString("map_frame");
   options.tracking_frame =
       lua_parameter_dictionary->GetString("tracking_frame");

cartographer_ros/cartographer_ros/node_options.h

@@ -29,7 +29,6 @@ namespace cartographer_ros {
 // Top-level options of Cartographer's ROS integration.
 struct NodeOptions {
   ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
-  SensorBridgeOptions sensor_bridge_options;
   string map_frame;
   string tracking_frame;
   string published_frame;

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -37,46 +37,19 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
 
 }  // namespace
 
-SensorBridgeOptions CreateSensorBridgeOptions(
-    carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
-  SensorBridgeOptions options;
-  options.constant_odometry_translational_variance =
-      lua_parameter_dictionary->GetDouble(
-          "constant_odometry_translational_variance");
-  options.constant_odometry_rotational_variance =
-      lua_parameter_dictionary->GetDouble(
-          "constant_odometry_rotational_variance");
-  return options;
-}
-
 SensorBridge::SensorBridge(
-    const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
+    const TfBridge* const tf_bridge,
     carto::mapping::TrajectoryBuilder* const trajectory_builder)
-    : options_(options),
+    : tf_bridge_(tf_bridge), trajectory_builder_(trajectory_builder) {}
-      tf_bridge_(tf_bridge),
-      trajectory_builder_(trajectory_builder) {}
 
 void SensorBridge::HandleOdometryMessage(
     const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
   const carto::common::Time time = FromRos(msg->header.stamp);
-  const Eigen::Matrix3d translational =
-      Eigen::Matrix3d::Identity() *
-      options_.constant_odometry_translational_variance;
-  const Eigen::Matrix3d rotational =
-      Eigen::Matrix3d::Identity() *
-      options_.constant_odometry_rotational_variance;
-  // clang-format off
-  carto::kalman_filter::PoseCovariance covariance;
-  covariance <<
-      translational, Eigen::Matrix3d::Zero(),
-      Eigen::Matrix3d::Zero(), rotational;
-  // clang-format on
   const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
       time, CheckNoLeadingSlash(msg->child_frame_id));
   if (sensor_to_tracking != nullptr) {
     trajectory_builder_->AddOdometerData(
-        topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
+        topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse());
-        covariance);
   }
 }
 

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -32,19 +32,11 @@
 
 namespace cartographer_ros {
 
-struct SensorBridgeOptions {
-  double constant_odometry_translational_variance;
-  double constant_odometry_rotational_variance;
-};
-
-SensorBridgeOptions CreateSensorBridgeOptions(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
-
 // Converts ROS messages into SensorData in tracking frame for the MapBuilder.
 class SensorBridge {
  public:
   explicit SensorBridge(
-      const SensorBridgeOptions& options, const TfBridge* tf_bridge,
+      const TfBridge* tf_bridge,
       ::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
 
   SensorBridge(const SensorBridge&) = delete;
@@ -68,7 +60,6 @@ class SensorBridge {
                          const string& frame_id,
                          const ::cartographer::sensor::PointCloud& ranges);
 
-  const SensorBridgeOptions options_;
   const TfBridge* const tf_bridge_;
   ::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
 };

cartographer_ros/configuration_files/backpack_2d.lua

@@ -16,10 +16,6 @@ include "map_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
-  sensor_bridge = {
-    constant_odometry_translational_variance = 0.,
-    constant_odometry_rotational_variance = 0.,
-  },
   map_frame = "map",
   tracking_frame = "base_link",
   published_frame = "base_link",

cartographer_ros/configuration_files/backpack_3d.lua

@@ -16,10 +16,6 @@ include "map_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
-  sensor_bridge = {
-    constant_odometry_translational_variance = 0.,
-    constant_odometry_rotational_variance = 0.,
-  },
   map_frame = "map",
   tracking_frame = "base_link",
   published_frame = "base_link",

cartographer_ros/configuration_files/pr2.lua

@@ -16,10 +16,6 @@ include "map_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
-  sensor_bridge = {
-    constant_odometry_translational_variance = 0.,
-    constant_odometry_rotational_variance = 0.,
-  },
   map_frame = "map",
   tracking_frame = "base_footprint",
   published_frame = "base_footprint",

cartographer_ros/configuration_files/revo_lds.lua

@@ -16,10 +16,6 @@ include "map_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
-  sensor_bridge = {
-    constant_odometry_translational_variance = 0.,
-    constant_odometry_rotational_variance = 0.,
-  },
   map_frame = "map",
   tracking_frame = "horizontal_laser_link",
   published_frame = "horizontal_laser_link",

docs/source/configuration.rst

@@ -55,12 +55,6 @@ use_odometry_data
   If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry
   must be provided in this case, and the information will be included in SLAM.
 
-sensor_bridge.constant_odometry_translational_variance
-  The variance to use for the translational component of odometry.
-
-sensor_bridge.constant_odometry_rotational_variance
-  The variance to use for the rotational component of odometry.
-
 use_horizontal_laser
   If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan"
   topic. If 2D SLAM is used, either this or *use_horizontal_multi_echo_laser*