From ca5b7d1bff9c865a70dff2386c7df7f26c33921f Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Mon, 21 Nov 2016 13:12:05 +0100 Subject: [PATCH] New Cartographer API, removes SensorBridgeOptions. (#180) --- .../cartographer_ros/node_main.cc | 6 ++-- .../cartographer_ros/node_options.cc | 2 -- .../cartographer_ros/node_options.h | 1 - .../cartographer_ros/sensor_bridge.cc | 33 ++----------------- .../cartographer_ros/sensor_bridge.h | 11 +------ .../configuration_files/backpack_2d.lua | 4 --- .../configuration_files/backpack_3d.lua | 4 --- cartographer_ros/configuration_files/pr2.lua | 4 --- .../configuration_files/revo_lds.lua | 4 --- docs/source/configuration.rst | 6 ---- 10 files changed, 6 insertions(+), 69 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 9cf4923..3e61baf 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/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( - options_.sensor_bridge_options, &tf_bridge_, - map_builder_.GetTrajectoryBuilder(trajectory_id_)); + &tf_bridge_, 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( - options_.sensor_bridge_options, &tf_bridge_, - map_builder_.GetTrajectoryBuilder(trajectory_id_)); + &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); return true; } diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index a3dc7be..03d64d7 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/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"); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 2d2c81f..9461f2f 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/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; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index cb5fb10..851d6fe 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/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(), - covariance); + topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()); } } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 7a7d271..fc91973 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/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_; }; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 6c59d0c..73904a5 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/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", diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 095bced..e209e18 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/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", diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 2873036..42d3c31 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/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", diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 3722396..a2200df 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/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", diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 13f3386..c8b86f3 100644 --- a/docs/source/configuration.rst +++ b/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*