Implement cartographer_grpc_node. (#632)
parent
abe4d10de3
commit
bf697c2d14
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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'.
|
||||||
|
|
|
@ -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>(
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue