Implement cartographer_grpc_node. (#632)

master
Christoph Schütte 2018-01-03 15:12:12 +01:00 committed by Wally B. Feed
parent abe4d10de3
commit bf697c2d14
7 changed files with 76 additions and 26 deletions

View File

@ -14,10 +14,12 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer_grpc/mapping/map_builder_stub.h"
#include "cartographer_ros/node.h" #include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h" #include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
DEFINE_string(configuration_directory, "", DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, " "First directory in which configuration files are searched, "
@ -26,7 +28,9 @@ DEFINE_string(configuration_directory, "",
DEFINE_string(configuration_basename, "", DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the " "Basename, i.e. not containing any directory prefix, of the "
"configuration file."); "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( DEFINE_bool(
start_trajectory_with_default_topics, true, start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics."); "Enable to immediately start the first trajectory with default topics.");
@ -39,7 +43,30 @@ namespace cartographer_grpc {
namespace { namespace {
void Run() { 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 } // namespace

View File

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

View File

@ -22,7 +22,8 @@
#include <unordered_map> #include <unordered_map>
#include <unordered_set> #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/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/sensor_bridge.h"
@ -53,7 +54,10 @@ class MapBuilderBridge {
TrajectoryOptions trajectory_options; 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(const MapBuilderBridge&) = delete;
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
@ -89,7 +93,7 @@ class MapBuilderBridge {
const NodeOptions node_options_; const NodeOptions node_options_;
std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>> std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
trajectory_state_data_ GUARDED_BY(mutex_); 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_; tf2_ros::Buffer* const tf_buffer_;
// These are keyed with 'trajectory_id'. // These are keyed with 'trajectory_id'.

View File

@ -78,9 +78,12 @@ namespace carto = ::cartographer;
using carto::transform::Rigid3d; 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), : 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_); carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_ = submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(

View File

@ -25,6 +25,7 @@
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/map_builder_bridge.h"
#include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_constants.h"
@ -46,7 +47,9 @@ namespace cartographer_ros {
// Wires up ROS topics to SLAM. // Wires up ROS topics to SLAM.
class Node { class Node {
public: 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();
Node(const Node&) = delete; Node(const Node&) = delete;

View File

@ -14,6 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h" #include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_log_sink.h"
@ -47,7 +48,10 @@ void Run() {
std::tie(node_options, trajectory_options) = std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); 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()) { if (!FLAGS_map_filename.empty()) {
node.LoadMap(FLAGS_map_filename); node.LoadMap(FLAGS_map_filename);
} }

View File

@ -26,6 +26,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h" #include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.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 // transform. When we finish processing the bag, we will simply drop any
// remaining sensor data that cannot be transformed due to missing transforms. // remaining sensor data that cannot be transformed due to missing transforms.
node_options.lookup_transform_timeout_sec = 0.; 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()) { if (!FLAGS_pbstream_filename.empty()) {
// TODO(jihoonl): LoadMap should be replaced by some better deserialization // TODO(jihoonl): LoadMap should be replaced by some better deserialization
// of full SLAM state as non-frozen trajectories once possible // of full SLAM state as non-frozen trajectories once possible