From bf697c2d147ac2cb8e64e859c1d5e2eb308db3d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Wed, 3 Jan 2018 15:12:12 +0100 Subject: [PATCH] Implement cartographer_grpc_node. (#632) --- .../cartographer_grpc/node_grpc_main.cc | 31 +++++++++++++++- .../cartographer_ros/map_builder_bridge.cc | 37 +++++++++++-------- .../cartographer_ros/map_builder_bridge.h | 10 +++-- cartographer_ros/cartographer_ros/node.cc | 7 +++- cartographer_ros/cartographer_ros/node.h | 5 ++- .../cartographer_ros/node_main.cc | 6 ++- .../cartographer_ros/offline_node_main.cc | 6 ++- 7 files changed, 76 insertions(+), 26 deletions(-) diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc index 7d92137..eb2d6db 100644 --- a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc +++ b/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 diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 4b6239f..932a40a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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 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& 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; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index cd14e2c..13f373c 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -22,7 +22,8 @@ #include #include -#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 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> trajectory_state_data_ GUARDED_BY(mutex_); - cartographer::mapping::MapBuilder map_builder_; + std::unique_ptr map_builder_; tf2_ros::Buffer* const tf_buffer_; // These are keyed with 'trajectory_id'. diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 3d4399b..bc13be9 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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 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>( diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index c101c5d..5fbd2b2 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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 map_builder, + tf2_ros::Buffer* tf_buffer); ~Node(); Node(const Node&) = delete; diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 904d5ab..94516ab 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/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( + 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); } diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index d9668c7..76b7eba 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/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& 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( + 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