From 9003b19343325c255823f26793c448d8b0f4a577 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Thu, 8 Sep 2016 17:01:21 +0200 Subject: [PATCH] Uses MapBuilder and cleans up some configurations. (#51) --- .../configuration_files/backpack_2d.lua | 24 +- .../configuration_files/backpack_3d.lua | 23 +- .../configuration_files/turtlebot.lua | 23 +- .../src/cartographer_node_main.cc | 375 +++++++++--------- 4 files changed, 233 insertions(+), 212 deletions(-) diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index dc89b3c..df48824 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -12,25 +12,27 @@ -- See the License for the specific language governing permissions and -- limitations under the License. -include "trajectory_builder.lua" -include "sparse_pose_graph.lua" +include "map_builder.lua" options = { - sparse_pose_graph = SPARSE_POSE_GRAPH, - trajectory_builder = TRAJECTORY_BUILDER, + map_builder = MAP_BUILDER, map_frame = "map", - odom_frame = "odom", tracking_frame = "base_link", + odom_frame = "odom", provide_odom_frame = true, - expect_odometry_data = false, + use_odometry_data = false, publish_occupancy_grid = false, - laser_min_range = 0., - laser_max_range = 30., - laser_missing_echo_ray_length = 5., - lookup_transform_timeout = 0.01, + use_horizontal_laser = false, + use_horizontal_multi_echo_laser = true, + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + num_lasers_3d = 0, + lookup_transform_timeout_sec = 0.01, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, - use_multi_echo_laser_scan_2d = true } +options.map_builder.use_trajectory_builder_2d = true + return options diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 8771469..2aef7c6 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -12,27 +12,28 @@ -- See the License for the specific language governing permissions and -- limitations under the License. -include "trajectory_builder_3d.lua" -include "sparse_pose_graph.lua" +include "map_builder.lua" options = { - sparse_pose_graph = SPARSE_POSE_GRAPH, - trajectory_builder = TRAJECTORY_BUILDER_3D, + map_builder = MAP_BUILDER, map_frame = "map", - odom_frame = "odom", tracking_frame = "base_link", + odom_frame = "odom", provide_odom_frame = true, - expect_odometry_data = false, + use_odometry_data = false, publish_occupancy_grid = false, - laser_min_range = 0., - laser_max_range = 30., - laser_missing_echo_ray_length = 5., - lookup_transform_timeout = 0.01, + use_horizontal_laser = false, + use_horizontal_multi_echo_laser = false, + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + num_lasers_3d = 2, + lookup_transform_timeout_sec = 0.01, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, - num_lasers_3d = 2 } +options.map_builder.use_trajectory_builder_3d = true options.sparse_pose_graph.optimize_every_n_scans = 320 options.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03 options.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10 diff --git a/cartographer_ros/configuration_files/turtlebot.lua b/cartographer_ros/configuration_files/turtlebot.lua index ee40614..b1e61d2 100644 --- a/cartographer_ros/configuration_files/turtlebot.lua +++ b/cartographer_ros/configuration_files/turtlebot.lua @@ -12,27 +12,28 @@ -- See the License for the specific language governing permissions and -- limitations under the License. -include "trajectory_builder.lua" -include "sparse_pose_graph.lua" +include "map_builder.lua" options = { - sparse_pose_graph = SPARSE_POSE_GRAPH, - trajectory_builder = TRAJECTORY_BUILDER, + map_builder = MAP_BUILDER, map_frame = "map", - odom_frame = "odom", tracking_frame = "base_link", + odom_frame = "odom", provide_odom_frame = false, - expect_odometry_data = false, + use_odometry_data = false, publish_occupancy_grid = false, - laser_min_range = 0., - laser_max_range = 30., - laser_missing_echo_ray_length = 5., - lookup_transform_timeout = 0.01, + use_horizontal_laser = true, + use_horizontal_multi_echo_laser = false, + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + num_lasers_3d = 0, + lookup_transform_timeout_sec = 0.01, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, - use_laser_scan_2d = true } +options.map_builder.use_trajectory_builder_2d = true options.trajectory_builder.expect_imu_data = false options.trajectory_builder.use_online_correlative_scan_matching = true diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 34d5fa8..b8d2f75 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -32,6 +32,7 @@ #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/proto/submaps.pb.h" #include "cartographer/mapping/sensor_collator.h" #include "cartographer/mapping_2d/global_trajectory_builder.h" @@ -85,7 +86,7 @@ using carto::transform::Rigid3d; using carto::kalman_filter::PoseCovariance; // TODO(hrapp): Support multi trajectory mapping. -constexpr int64 kTrajectoryId = 0; +constexpr int64 kTrajectoryBuilderId = 0; constexpr int kSubscriberQueueSize = 150; constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; @@ -97,12 +98,86 @@ constexpr char kImuTopic[] = "/imu"; constexpr char kOdometryTopic[] = "/odom"; constexpr char kOccupancyGridTopic[] = "/map"; +struct NodeOptions { + carto::mapping::proto::MapBuilderOptions map_builder_options; + string map_frame; + string tracking_frame; + string odom_frame; + bool publish_occupancy_grid; + bool provide_odom_frame; + bool use_odometry_data; + bool use_horizontal_laser; + bool use_horizontal_multi_echo_laser; + double horizontal_laser_min_range; + double horizontal_laser_max_range; + double horizontal_laser_missing_echo_ray_length; + int num_lasers_3d; + double lookup_transform_timeout_sec; + double submap_publish_period_sec; + double pose_publish_period_sec; +}; + +NodeOptions CreateNodeOptions( + carto::common::LuaParameterDictionary* const lua_parameter_dictionary) { + NodeOptions options; + options.map_builder_options = carto::mapping::CreateMapBuilderOptions( + lua_parameter_dictionary->GetDictionary("map_builder").get()); + options.map_frame = lua_parameter_dictionary->GetString("map_frame"); + options.tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); + options.publish_occupancy_grid = + lua_parameter_dictionary->GetBool("publish_occupancy_grid"); + options.provide_odom_frame = + lua_parameter_dictionary->GetBool("provide_odom_frame"); + options.use_odometry_data = + lua_parameter_dictionary->GetBool("use_odometry_data"); + options.use_horizontal_laser = + lua_parameter_dictionary->GetBool("use_horizontal_laser"); + options.use_horizontal_multi_echo_laser = + lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser"); + 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.num_lasers_3d = + lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d"); + options.lookup_transform_timeout_sec = + lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec"); + options.submap_publish_period_sec = + lua_parameter_dictionary->GetDouble("submap_publish_period_sec"); + options.pose_publish_period_sec = + lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); + + CHECK_EQ(options.use_horizontal_laser + + options.use_horizontal_multi_echo_laser + + (options.num_lasers_3d > 0), + 1) + << "Configuration error: 'use_horizontal_laser', " + "'use_horizontal_multi_echo_laser' and 'num_lasers_3d' are " + "mutually exclusive, but one is required."; + CHECK_EQ( + options.map_builder_options.use_trajectory_builder_2d(), + options.use_horizontal_laser || options.use_horizontal_multi_echo_laser); + CHECK_EQ(options.map_builder_options.use_trajectory_builder_3d(), + options.num_lasers_3d > 0); + if (options.publish_occupancy_grid) { + CHECK(options.map_builder_options.use_trajectory_builder_2d()) + << "Publishing OccupancyGrids for 3D data is not yet supported"; + } + return options; +} + // Node that listens to all the sensor data that we are interested in and wires // it up to the SLAM. class Node { public: - Node(); + Node(const NodeOptions& options); ~Node(); + Node(const Node&) = delete; Node& operator=(const Node&) = delete; @@ -121,8 +196,8 @@ class Node { void AddLaserFan3D(int64 timestamp, const string& frame_id, const proto::LaserFan3D& laser_fan_3d); - // Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at - // 'time' or throws tf2::TransformException if it does not exist. + // Returns a transform for 'frame_id' to 'options_.tracking_frame' if it + // exists at 'time' or throws tf2::TransformException if it does not exist. Rigid3d LookupToTrackingTransformOrThrow(carto::common::Time time, const string& frame_id); @@ -130,42 +205,30 @@ class Node { ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response); - void PublishSubmapList(const ros::WallTimerEvent& timer_event); - void PublishPose(const ros::WallTimerEvent& timer_event); + void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); + void PublishPose(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); - // TODO(hrapp): Pull out the common functionality between this and MapWriter - // into an open sourcable MapWriter. - carto::mapping::SensorCollator sensor_collator_; - SensorDataProducer sensor_data_producer_; - ros::NodeHandle node_handle_; - ros::Subscriber imu_subscriber_; - ros::Subscriber laser_subscriber_2d_; - std::vector laser_subscribers_3d_; - ros::Subscriber odometry_subscriber_; - string tracking_frame_; - string odom_frame_; - string map_frame_; - bool provide_odom_frame_; - bool expect_odometry_data_; - double laser_min_range_; - double laser_max_range_; - double laser_missing_echo_ray_length_; - double lookup_transform_timeout_; + const NodeOptions options_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_; - carto::common::ThreadPool thread_pool_; carto::common::Mutex mutex_; - std::unique_ptr - trajectory_builder_ GUARDED_BY(mutex_); - std::deque constant_node_data_ + std::deque constant_data_ GUARDED_BY(mutex_); - std::unique_ptr sparse_pose_graph_; + carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_); + carto::mapping::SensorCollator sensor_collator_ + GUARDED_BY(mutex_); + SensorDataProducer sensor_data_producer_ GUARDED_BY(mutex_); + ::ros::NodeHandle node_handle_; + ::ros::Subscriber imu_subscriber_; + ::ros::Subscriber horizontal_laser_scan_subscriber_; + std::vector<::ros::Subscriber> laser_subscribers_3d_; + ::ros::Subscriber odometry_subscriber_; ::ros::Publisher submap_list_publisher_; ::ros::ServiceServer submap_query_server_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_; tf2_ros::TransformBroadcaster tf_broadcaster_; ::ros::Publisher occupancy_grid_publisher_; @@ -177,17 +240,18 @@ class Node { std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_; std::map> rate_timers_; - // We have to keep the timer handles of ros::WallTimers around, otherwise they - // do not fire. - std::vector wall_timers_; + // We have to keep the timer handles of ::ros::WallTimers around, otherwise + // they do not fire. + std::vector<::ros::WallTimer> wall_timers_; }; -Node::Node() - : sensor_data_producer_(kTrajectoryId, &sensor_collator_), +Node::Node(const NodeOptions& options) + : options_(options), + map_builder_(options.map_builder_options, &constant_data_), + sensor_data_producer_(kTrajectoryBuilderId, &sensor_collator_), node_handle_("~"), - tf_buffer_(ros::Duration(1000)), - tf_(tf_buffer_), - thread_pool_(10) {} + tf_buffer_(::ros::Duration(1000)), + tf_(tf_buffer_) {} Node::~Node() { { @@ -201,15 +265,16 @@ Node::~Node() { Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time, const string& frame_id) { - return ToRigid3d( - tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time), - ros::Duration(lookup_transform_timeout_))); + return ToRigid3d(tf_buffer_.lookupTransform( + options_.tracking_frame, frame_id, ToRos(time), + ::ros::Duration(options_.lookup_transform_timeout_sec))); } void Node::AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, const PoseCovariance& covariance) { const carto::common::Time time = carto::common::FromUniversal(timestamp); - trajectory_builder_->AddOdometerPose(time, pose, covariance); + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->AddOdometerPose(time, pose, covariance); } void Node::AddImu(const int64 timestamp, const string& frame_id, @@ -222,14 +287,15 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, << "The IMU frame must be colocated with the tracking frame. " "Transforming linear accelaration into the tracking frame will " "otherwise be imprecise."; - trajectory_builder_->AddImuData( - time, sensor_to_tracking.rotation() * - carto::transform::ToEigen(imu.linear_acceleration()), - sensor_to_tracking.rotation() * - carto::transform::ToEigen(imu.angular_velocity())); + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->AddImuData(time, + sensor_to_tracking.rotation() * + carto::transform::ToEigen(imu.linear_acceleration()), + sensor_to_tracking.rotation() * + carto::transform::ToEigen(imu.angular_velocity())); } catch (const tf2::TransformException& ex) { - LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ - << ": " << ex.what(); + LOG(WARNING) << "Cannot transform " << frame_id << " -> " + << options_.tracking_frame << ": " << ex.what(); } } void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, @@ -239,16 +305,18 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, const Rigid3d sensor_to_tracking = LookupToTrackingTransformOrThrow(time, frame_id); const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan( - laser_scan, laser_min_range_, laser_max_range_, - laser_missing_echo_ray_length_); + laser_scan, options_.horizontal_laser_min_range, + options_.horizontal_laser_max_range, + options_.horizontal_laser_missing_echo_ray_length); const auto laser_fan_3d = carto::sensor::TransformLaserFan3D( carto::sensor::ToLaserFan3D(laser_fan), sensor_to_tracking.cast()); - trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d); + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->AddHorizontalLaserFan(time, laser_fan_3d); } catch (const tf2::TransformException& ex) { - LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ - << ": " << ex.what(); + LOG(WARNING) << "Cannot transform " << frame_id << " -> " + << options_.tracking_frame << ": " << ex.what(); } } @@ -258,62 +326,24 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, try { const Rigid3d sensor_to_tracking = LookupToTrackingTransformOrThrow(time, frame_id); - trajectory_builder_->AddLaserFan3D( - time, carto::sensor::TransformLaserFan3D( - carto::sensor::FromProto(laser_fan_3d), - sensor_to_tracking.cast())); + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->AddLaserFan3D(time, carto::sensor::TransformLaserFan3D( + carto::sensor::FromProto(laser_fan_3d), + sensor_to_tracking.cast())); } catch (const tf2::TransformException& ex) { - LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ - << ": " << ex.what(); + LOG(WARNING) << "Cannot transform " << frame_id << " -> " + << options_.tracking_frame << ": " << ex.what(); } } void Node::Initialize() { - auto file_resolver = - carto::common::make_unique( - std::vector{FLAGS_configuration_directory}); - const string code = - file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); - carto::common::LuaParameterDictionary lua_parameter_dictionary( - code, std::move(file_resolver), nullptr); - - tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame"); - odom_frame_ = lua_parameter_dictionary.GetString("odom_frame"); - map_frame_ = lua_parameter_dictionary.GetString("map_frame"); - provide_odom_frame_ = lua_parameter_dictionary.GetBool("provide_odom_frame"); - expect_odometry_data_ = - lua_parameter_dictionary.GetBool("expect_odometry_data"); - laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range"); - laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range"); - laser_missing_echo_ray_length_ = - lua_parameter_dictionary.GetDouble("laser_missing_echo_ray_length"); - lookup_transform_timeout_ = - lua_parameter_dictionary.GetDouble("lookup_transform_timeout"); - // Set of all topics we subscribe to. We use the non-remapped default names // which are unique. std::unordered_set expected_sensor_identifiers; - // Subscribe to exactly one laser. - const bool has_laser_scan_2d = - lua_parameter_dictionary.HasKey("use_laser_scan_2d") && - lua_parameter_dictionary.GetBool("use_laser_scan_2d"); - const bool has_multi_echo_laser_scan_2d = - lua_parameter_dictionary.HasKey("use_multi_echo_laser_scan_2d") && - lua_parameter_dictionary.GetBool("use_multi_echo_laser_scan_2d"); - const int num_lasers_3d = - lua_parameter_dictionary.HasKey("num_lasers_3d") - ? lua_parameter_dictionary.GetNonNegativeInt("num_lasers_3d") - : 0; - - CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + - (num_lasers_3d > 0) == - 1) - << "Parameters 'use_laser_scan_2d', 'use_multi_echo_laser_scan_2d' and " - "'num_lasers_3d' are mutually exclusive, but one is required."; - - if (has_laser_scan_2d) { - laser_subscriber_2d_ = node_handle_.subscribe( + // For 2D SLAM, subscribe to exactly one horizontal laser. + if (options_.use_horizontal_laser) { + horizontal_laser_scan_subscriber_ = node_handle_.subscribe( kLaserScanTopic, kSubscriberQueueSize, boost::function( [this](const sensor_msgs::LaserScan::ConstPtr& msg) { @@ -321,8 +351,8 @@ void Node::Initialize() { })); expected_sensor_identifiers.insert(kLaserScanTopic); } - if (has_multi_echo_laser_scan_2d) { - laser_subscriber_2d_ = node_handle_.subscribe( + if (options_.use_horizontal_multi_echo_laser) { + horizontal_laser_scan_subscriber_ = node_handle_.subscribe( kMultiEchoLaserScanTopic, kSubscriberQueueSize, boost::function( [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { @@ -332,28 +362,11 @@ void Node::Initialize() { expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); } - bool expect_imu_data = true; - if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) { - auto sparse_pose_graph_2d = - carto::common::make_unique( - carto::mapping::CreateSparsePoseGraphOptions( - lua_parameter_dictionary.GetDictionary("sparse_pose_graph") - .get()), - &thread_pool_, &constant_node_data_); - auto options = carto::mapping_2d::CreateLocalTrajectoryBuilderOptions( - lua_parameter_dictionary.GetDictionary("trajectory_builder").get()); - submaps_options_ = options.submaps_options(); - expect_imu_data = options.expect_imu_data(); - trajectory_builder_ = - carto::common::make_unique( - options, sparse_pose_graph_2d.get()); - sparse_pose_graph_ = std::move(sparse_pose_graph_2d); - } - - if (num_lasers_3d > 0) { - for (int i = 0; i < num_lasers_3d; ++i) { + // For 3D SLAM, subscribe to all 3D lasers. + if (options_.num_lasers_3d > 0) { + for (int i = 0; i < options_.num_lasers_3d; ++i) { string topic = kPointCloud2Topic; - if (num_lasers_3d > 1) { + if (options_.num_lasers_3d > 1) { topic += "_" + std::to_string(i + 1); } laser_subscribers_3d_.push_back(node_handle_.subscribe( @@ -364,24 +377,14 @@ void Node::Initialize() { }))); expected_sensor_identifiers.insert(topic); } - auto sparse_pose_graph_3d = - carto::common::make_unique( - carto::mapping::CreateSparsePoseGraphOptions( - lua_parameter_dictionary.GetDictionary("sparse_pose_graph") - .get()), - &thread_pool_, &constant_node_data_); - trajectory_builder_ = - carto::common::make_unique( - carto::mapping_3d::CreateLocalTrajectoryBuilderOptions( - lua_parameter_dictionary.GetDictionary("trajectory_builder") - .get()), - sparse_pose_graph_3d.get()); - sparse_pose_graph_ = std::move(sparse_pose_graph_3d); } - CHECK(sparse_pose_graph_ != nullptr); - // Maybe subscribe to the IMU. - if (expect_imu_data) { + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + if (options_.map_builder_options.use_trajectory_builder_3d() || + (options_.map_builder_options.use_trajectory_builder_2d() && + options_.map_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { imu_subscriber_ = node_handle_.subscribe( kImuTopic, kSubscriberQueueSize, boost::function( @@ -391,7 +394,7 @@ void Node::Initialize() { expected_sensor_identifiers.insert(kImuTopic); } - if (expect_odometry_data_) { + if (options_.use_odometry_data) { odometry_subscriber_ = node_handle_.subscribe( kOdometryTopic, kSubscriberQueueSize, boost::function( @@ -401,8 +404,10 @@ void Node::Initialize() { expected_sensor_identifiers.insert(kOdometryTopic); } + // TODO(damonkohler): Add multi-trajectory support. + CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder()); sensor_collator_.AddTrajectory( - kTrajectoryId, expected_sensor_identifiers, + kTrajectoryBuilderId, expected_sensor_identifiers, [this](const int64 timestamp, std::unique_ptr sensor_data) { HandleSensorData(timestamp, std::move(sensor_data)); }); @@ -413,9 +418,7 @@ void Node::Initialize() { submap_query_server_ = node_handle_.advertiseService( kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); - if (lua_parameter_dictionary.GetBool("publish_occupancy_grid")) { - CHECK_EQ(num_lasers_3d, 0) - << "Publishing OccupancyGrids for 3D data is not yet supported"; + if (options_.publish_occupancy_grid) { occupancy_grid_publisher_ = node_handle_.advertise<::nav_msgs::OccupancyGrid>(kOccupancyGridTopic, 1, true); @@ -424,13 +427,11 @@ void Node::Initialize() { } wall_timers_.push_back(node_handle_.createWallTimer( - ros::WallDuration( - lua_parameter_dictionary.GetDouble("submap_publish_period_sec")), + ::ros::WallDuration(options_.submap_publish_period_sec), &Node::PublishSubmapList, this)); wall_timers_.push_back(node_handle_.createWallTimer( - ros::WallDuration( - lua_parameter_dictionary.GetDouble("pose_publish_period_sec")), - &Node::PublishPose, this)); + ::ros::WallDuration(options_.pose_publish_period_sec), &Node::PublishPose, + this)); } bool Node::HandleSubmapQuery( @@ -442,7 +443,8 @@ bool Node::HandleSubmapQuery( carto::common::MutexLocker lock(&mutex_); // TODO(hrapp): return error messages and extract common code from MapBuilder. - carto::mapping::Submaps* submaps = trajectory_builder_->submaps(); + carto::mapping::Submaps* submaps = + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps(); if (request.submap_id < 0 || request.submap_id >= submaps->size()) { return false; } @@ -452,10 +454,10 @@ bool Node::HandleSubmapQuery( response_proto.set_submap_version( submaps->Get(request.submap_id)->end_laser_fan_index); const std::vector submap_transforms = - sparse_pose_graph_->GetSubmapTransforms(*submaps); + map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps); submaps->SubmapToProto(request.submap_id, - sparse_pose_graph_->GetTrajectoryNodes(), + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(), submap_transforms[request.submap_id], &response_proto); response.submap_version = response_proto.submap_version(); @@ -483,11 +485,12 @@ bool Node::HandleSubmapQuery( return true; } -void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) { +void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); - const carto::mapping::Submaps* submaps = trajectory_builder_->submaps(); + const carto::mapping::Submaps* submaps = + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps(); const std::vector submap_transforms = - sparse_pose_graph_->GetSubmapTransforms(*submaps); + map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps); CHECK_EQ(submap_transforms.size(), submaps->size()); ::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory; @@ -499,44 +502,49 @@ void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) { } ::cartographer_ros_msgs::SubmapList ros_submap_list; - ros_submap_list.header.stamp = ros::Time::now(); - ros_submap_list.header.frame_id = map_frame_; + ros_submap_list.header.stamp = ::ros::Time::now(); + ros_submap_list.header.frame_id = options_.map_frame; ros_submap_list.trajectory.push_back(ros_trajectory); submap_list_publisher_.publish(ros_submap_list); } -void Node::PublishPose(const ros::WallTimerEvent& timer_event) { +void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); - const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose; - const carto::mapping::Submaps* submaps = trajectory_builder_->submaps(); + const Rigid3d tracking_to_local = + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->pose_estimate() + .pose; + const carto::mapping::Submaps* submaps = + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps(); const Rigid3d local_to_map = - sparse_pose_graph_->GetLocalToGlobalTransform(*submaps); + map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps); const Rigid3d tracking_to_map = local_to_map * tracking_to_local; - const ros::Time now = ros::Time::now(); + const ::ros::Time now = ::ros::Time::now(); geometry_msgs::TransformStamped stamped_transform; stamped_transform.header.stamp = now; - stamped_transform.header.frame_id = map_frame_; - stamped_transform.child_frame_id = odom_frame_; + stamped_transform.header.frame_id = options_.map_frame; + stamped_transform.child_frame_id = options_.odom_frame; - if (provide_odom_frame_) { + if (options_.provide_odom_frame) { stamped_transform.transform = ToGeometryMsgTransform(local_to_map); tf_broadcaster_.sendTransform(stamped_transform); - stamped_transform.header.frame_id = odom_frame_; - stamped_transform.child_frame_id = tracking_frame_; + stamped_transform.header.frame_id = options_.odom_frame; + stamped_transform.child_frame_id = options_.tracking_frame; stamped_transform.transform = ToGeometryMsgTransform(tracking_to_local); tf_broadcaster_.sendTransform(stamped_transform); } else { try { const Rigid3d tracking_to_odom = - LookupToTrackingTransformOrThrow(FromRos(now), odom_frame_).inverse(); + LookupToTrackingTransformOrThrow(FromRos(now), options_.odom_frame) + .inverse(); const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse(); stamped_transform.transform = ToGeometryMsgTransform(odom_to_map); tf_broadcaster_.sendTransform(stamped_transform); } catch (const tf2::TransformException& ex) { - LOG(WARNING) << "Cannot transform " << tracking_frame_ << " -> " - << odom_frame_ << ": " << ex.what(); + LOG(WARNING) << "Cannot transform " << options_.tracking_frame << " -> " + << options_.odom_frame << ": " << ex.what(); } } } @@ -549,7 +557,8 @@ void Node::SpinOccupancyGridThreadForever() { return; } } - const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes(); + const auto trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); if (trajectory_nodes.empty()) { std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); continue; @@ -571,7 +580,7 @@ void Node::SpinOccupancyGridThreadForever() { ::nav_msgs::OccupancyGrid occupancy_grid; occupancy_grid.header.stamp = ToRos(trajectory_nodes.back().time()); - occupancy_grid.header.frame_id = map_frame_; + occupancy_grid.header.frame_id = options_.map_frame; occupancy_grid.info.map_load_time = occupancy_grid.header.stamp; Eigen::Array2i offset; @@ -661,10 +670,18 @@ void Node::HandleSensorData(const int64 timestamp, LOG(FATAL); } -void Node::SpinForever() { ros::spin(); } +void Node::SpinForever() { ::ros::spin(); } void Run() { - Node node; + auto file_resolver = + carto::common::make_unique( + std::vector{FLAGS_configuration_directory}); + const string code = + file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); + carto::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver), nullptr); + + Node node(CreateNodeOptions(&lua_parameter_dictionary)); node.Initialize(); node.SpinForever(); } @@ -729,11 +746,11 @@ int main(int argc, char** argv) { CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; - ros::init(argc, argv, "cartographer_node"); - ros::start(); + ::ros::init(argc, argv, "cartographer_node"); + ::ros::start(); ::cartographer_ros::ScopedRosLogSink ros_log_sink; ::cartographer_ros::Run(); - ros::shutdown(); + ::ros::shutdown(); return 0; }