Implement cartographer_grpc_node. (#632)

cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc

@@ -14,10 +14,12 @@
  * limitations under the License.
  */
 
+#include "cartographer_grpc/mapping/map_builder_stub.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "gflags/gflags.h"
+#include "tf2_ros/transform_listener.h"
 
 DEFINE_string(configuration_directory, "",
               "First directory in which configuration files are searched, "
@@ -26,7 +28,9 @@ DEFINE_string(configuration_directory, "",
 DEFINE_string(configuration_basename, "",
               "Basename, i.e. not containing any directory prefix, of the "
               "configuration file.");
-DEFINE_string(map_filename, "", "If non-empty, filename of a map to load.");
+DEFINE_string(server_address, "localhost:50051",
+              "gRPC server address to "
+              "stream the sensor data to.");
 DEFINE_bool(
     start_trajectory_with_default_topics, true,
     "Enable to immediately start the first trajectory with default topics.");
@@ -39,7 +43,30 @@ namespace cartographer_grpc {
 namespace {
 
 void Run() {
-  // TODO(cschuet): Implement this.
+  constexpr double kTfBufferCacheTimeInSeconds = 1e6;
+  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
+  tf2_ros::TransformListener tf(tf_buffer);
+  NodeOptions node_options;
+  TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) =
+      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
+
+  auto map_builder = cartographer::common::make_unique<
+      ::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address);
+  Node node(node_options, std::move(map_builder), &tf_buffer);
+
+  if (FLAGS_start_trajectory_with_default_topics) {
+    node.StartTrajectoryWithDefaultTopics(trajectory_options);
+  }
+
+  ::ros::spin();
+
+  node.FinishAllTrajectories();
+  node.RunFinalOptimization();
+
+  if (!FLAGS_save_map_filename.empty()) {
+    node.SerializeState(FLAGS_save_map_filename);
+  }
 }
 
 }  // namespace

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -16,8 +16,10 @@
 
 #include "cartographer_ros/map_builder_bridge.h"
 
+#include "cartographer/common/make_unique.h"
 #include "cartographer/io/color.h"
 #include "cartographer/io/proto_stream.h"
+#include "cartographer/mapping/pose_graph.h"
 #include "cartographer_ros/msg_conversion.h"
 
 namespace cartographer_ros {
@@ -62,22 +64,24 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker,
 
 }  // namespace
 
-MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
-                                   tf2_ros::Buffer* const tf_buffer)
+MapBuilderBridge::MapBuilderBridge(
+    const NodeOptions& node_options,
+    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
-      map_builder_(node_options.map_builder_options),
+      map_builder_(std::move(map_builder)),
       tf_buffer_(tf_buffer) {}
 
 void MapBuilderBridge::LoadMap(const std::string& map_filename) {
   LOG(INFO) << "Loading map '" << map_filename << "'...";
   cartographer::io::ProtoStreamReader stream(map_filename);
-  map_builder_.LoadMap(&stream);
+  map_builder_->LoadMap(&stream);
 }
 
 int MapBuilderBridge::AddTrajectory(
     const std::unordered_set<std::string>& expected_sensor_ids,
     const TrajectoryOptions& trajectory_options) {
-  const int trajectory_id = map_builder_.AddTrajectoryBuilder(
+  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
       expected_sensor_ids, trajectory_options.trajectory_builder_options,
       ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                   ::std::placeholders::_1, ::std::placeholders::_2,
@@ -92,7 +96,7 @@ int MapBuilderBridge::AddTrajectory(
           trajectory_options.num_subdivisions_per_laser_scan,
           trajectory_options.tracking_frame,
           node_options_.lookup_transform_timeout_sec, tf_buffer_,
-          map_builder_.GetTrajectoryBuilder(trajectory_id));
+          map_builder_->GetTrajectoryBuilder(trajectory_id));
   auto emplace_result =
       trajectory_options_.emplace(trajectory_id, trajectory_options);
   CHECK(emplace_result.second == true);
@@ -104,18 +108,18 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
 
   // Make sure there is a trajectory with 'trajectory_id'.
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
-  map_builder_.FinishTrajectory(trajectory_id);
+  map_builder_->FinishTrajectory(trajectory_id);
   sensor_bridges_.erase(trajectory_id);
 }
 
 void MapBuilderBridge::RunFinalOptimization() {
   LOG(INFO) << "Running final trajectory optimization...";
-  map_builder_.pose_graph()->RunFinalOptimization();
+  map_builder_->pose_graph()->RunFinalOptimization();
 }
 
 void MapBuilderBridge::SerializeState(const std::string& filename) {
   cartographer::io::ProtoStreamWriter writer(filename);
-  map_builder_.SerializeState(&writer);
+  map_builder_->SerializeState(&writer);
   CHECK(writer.Close()) << "Could not write state.";
 }
 
@@ -126,7 +130,7 @@ bool MapBuilderBridge::HandleSubmapQuery(
   cartographer::mapping::SubmapId submap_id{request.trajectory_id,
                                             request.submap_index};
   const std::string error =
-      map_builder_.SubmapToProto(submap_id, &response_proto);
+      map_builder_->SubmapToProto(submap_id, &response_proto);
   if (!error.empty()) {
     LOG(ERROR) << error;
     return false;
@@ -155,7 +159,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   submap_list.header.stamp = ::ros::Time::now();
   submap_list.header.frame_id = node_options_.map_frame;
   for (const auto& submap_id_data :
-       map_builder_.pose_graph()->GetAllSubmapData()) {
+       map_builder_->pose_graph()->GetAllSubmapData()) {
     cartographer_ros_msgs::SubmapEntry submap_entry;
     submap_entry.trajectory_id = submap_id_data.id.trajectory_id;
     submap_entry.submap_index = submap_id_data.id.submap_index;
@@ -186,7 +190,7 @@ MapBuilderBridge::GetTrajectoryStates() {
     CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
     trajectory_states[trajectory_id] = {
         local_slam_data,
-        map_builder_.pose_graph()->GetLocalToGlobalTransform(trajectory_id),
+        map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
         sensor_bridge.tf_bridge().LookupToTracking(
             local_slam_data->time,
             trajectory_options_[trajectory_id].published_frame),
@@ -197,7 +201,7 @@ MapBuilderBridge::GetTrajectoryStates() {
 
 visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
   visualization_msgs::MarkerArray trajectory_node_list;
-  const auto nodes = map_builder_.pose_graph()->GetTrajectoryNodes();
+  const auto nodes = map_builder_->pose_graph()->GetTrajectoryNodes();
   for (const int trajectory_id : nodes.trajectory_ids()) {
     visualization_msgs::Marker marker =
         CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
@@ -253,9 +257,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
   residual_inter_marker.ns = "Inter residuals";
   residual_inter_marker.pose.position.z = 0.1;
 
-  const auto trajectory_nodes = map_builder_.pose_graph()->GetTrajectoryNodes();
-  const auto submap_data = map_builder_.pose_graph()->GetAllSubmapData();
-  const auto constraints = map_builder_.pose_graph()->constraints();
+  const auto trajectory_nodes =
+      map_builder_->pose_graph()->GetTrajectoryNodes();
+  const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData();
+  const auto constraints = map_builder_->pose_graph()->constraints();
 
   for (const auto& constraint : constraints) {
     visualization_msgs::Marker *constraint_marker, *residual_marker;

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -22,7 +22,8 @@
 #include <unordered_map>
 #include <unordered_set>
 
-#include "cartographer/mapping/map_builder.h"
+#include "cartographer/common/mutex.h"
+#include "cartographer/mapping/map_builder_interface.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/sensor_bridge.h"
@@ -53,7 +54,10 @@ class MapBuilderBridge {
     TrajectoryOptions trajectory_options;
   };
 
-  MapBuilderBridge(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
+  MapBuilderBridge(
+      const NodeOptions& node_options,
+      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+      tf2_ros::Buffer* tf_buffer);
 
   MapBuilderBridge(const MapBuilderBridge&) = delete;
   MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
@@ -89,7 +93,7 @@ class MapBuilderBridge {
   const NodeOptions node_options_;
   std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
       trajectory_state_data_ GUARDED_BY(mutex_);
-  cartographer::mapping::MapBuilder map_builder_;
+  std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
   tf2_ros::Buffer* const tf_buffer_;
 
   // These are keyed with 'trajectory_id'.

cartographer_ros/cartographer_ros/node.cc

@@ -78,9 +78,12 @@ namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
 
-Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
+Node::Node(
+    const NodeOptions& node_options,
+    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
-      map_builder_bridge_(node_options_, tf_buffer) {
+      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
   carto::common::MutexLocker lock(&mutex_);
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(

cartographer_ros/cartographer_ros/node.h

@@ -25,6 +25,7 @@
 
 #include "cartographer/common/fixed_ratio_sampler.h"
 #include "cartographer/common/mutex.h"
+#include "cartographer/mapping/map_builder_interface.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer_ros/map_builder_bridge.h"
 #include "cartographer_ros/node_constants.h"
@@ -46,7 +47,9 @@ namespace cartographer_ros {
 // Wires up ROS topics to SLAM.
 class Node {
  public:
-  Node(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
+  Node(const NodeOptions& node_options,
+       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+       tf2_ros::Buffer* tf_buffer);
   ~Node();
 
   Node(const Node&) = delete;

cartographer_ros/cartographer_ros/node_main.cc

@@ -14,6 +14,7 @@
  * limitations under the License.
  */
 
+#include "cartographer/mapping/map_builder.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
@@ -47,7 +48,10 @@ void Run() {
   std::tie(node_options, trajectory_options) =
       LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
 
-  Node node(node_options, &tf_buffer);
+  auto map_builder =
+      cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
+          node_options.map_builder_options);
+  Node node(node_options, std::move(map_builder), &tf_buffer);
   if (!FLAGS_map_filename.empty()) {
     node.LoadMap(FLAGS_map_filename);
   }

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -26,6 +26,7 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/map_builder.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
@@ -90,7 +91,10 @@ void Run(const std::vector<std::string>& bag_filenames) {
   // transform. When we finish processing the bag, we will simply drop any
   // remaining sensor data that cannot be transformed due to missing transforms.
   node_options.lookup_transform_timeout_sec = 0.;
-  Node node(node_options, &tf_buffer);
+  auto map_builder =
+      cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
+          node_options.map_builder_options);
+  Node node(node_options, std::move(map_builder), &tf_buffer);
   if (!FLAGS_pbstream_filename.empty()) {
     // TODO(jihoonl): LoadMap should be replaced by some better deserialization
     // of full SLAM state as non-frozen trajectories once possible