Partially pulls out MapBuilderBridge. (#186)
parent
516c86fa5a
commit
500b83ff0d
|
@ -10,6 +10,20 @@ google_library(assets_writer
|
||||||
occupancy_grid
|
occupancy_grid
|
||||||
)
|
)
|
||||||
|
|
||||||
|
google_library(map_builder_bridge
|
||||||
|
USES_CARTOGRAPHER
|
||||||
|
SRCS
|
||||||
|
map_builder_bridge.cc
|
||||||
|
HDRS
|
||||||
|
map_builder_bridge.h
|
||||||
|
DEPENDS
|
||||||
|
assets_writer
|
||||||
|
msg_conversion
|
||||||
|
node_options
|
||||||
|
sensor_bridge
|
||||||
|
tf_bridge
|
||||||
|
)
|
||||||
|
|
||||||
google_library(map_writer
|
google_library(map_writer
|
||||||
USES_GLOG
|
USES_GLOG
|
||||||
USES_YAMLCPP
|
USES_YAMLCPP
|
||||||
|
@ -128,7 +142,7 @@ google_binary(cartographer_node
|
||||||
SRCS
|
SRCS
|
||||||
node_main.cc
|
node_main.cc
|
||||||
DEPENDS
|
DEPENDS
|
||||||
assets_writer
|
map_builder_bridge
|
||||||
msg_conversion
|
msg_conversion
|
||||||
node_options
|
node_options
|
||||||
occupancy_grid
|
occupancy_grid
|
||||||
|
|
|
@ -0,0 +1,118 @@
|
||||||
|
/*
|
||||||
|
* Copyright 2016 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "cartographer_ros/map_builder_bridge.h"
|
||||||
|
|
||||||
|
#include "cartographer_ros/assets_writer.h"
|
||||||
|
#include "cartographer_ros/msg_conversion.h"
|
||||||
|
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
|
||||||
|
MapBuilderBridge::MapBuilderBridge(
|
||||||
|
const NodeOptions& options,
|
||||||
|
const std::unordered_set<string>& expected_sensor_ids,
|
||||||
|
tf2_ros::Buffer* const tf_buffer)
|
||||||
|
: options_(options),
|
||||||
|
map_builder_(options.map_builder_options, &constant_data_),
|
||||||
|
expected_sensor_ids_(expected_sensor_ids),
|
||||||
|
trajectory_id_(map_builder_.AddTrajectoryBuilder(expected_sensor_ids_)),
|
||||||
|
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
|
||||||
|
tf_buffer) {
|
||||||
|
sensor_bridge_ = cartographer::common::make_unique<SensorBridge>(
|
||||||
|
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
||||||
|
}
|
||||||
|
|
||||||
|
MapBuilderBridge::~MapBuilderBridge() {
|
||||||
|
map_builder_.FinishTrajectory(trajectory_id_);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MapBuilderBridge::HandleSubmapQuery(
|
||||||
|
cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
|
cartographer_ros_msgs::SubmapQuery::Response& response) {
|
||||||
|
cartographer::mapping::proto::SubmapQuery::Response response_proto;
|
||||||
|
const std::string error = map_builder_.SubmapToProto(
|
||||||
|
request.trajectory_id, request.submap_index, &response_proto);
|
||||||
|
if (!error.empty()) {
|
||||||
|
LOG(ERROR) << error;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
response.submap_version = response_proto.submap_version();
|
||||||
|
response.cells.insert(response.cells.begin(), response_proto.cells().begin(),
|
||||||
|
response_proto.cells().end());
|
||||||
|
response.width = response_proto.width();
|
||||||
|
response.height = response_proto.height();
|
||||||
|
response.resolution = response_proto.resolution();
|
||||||
|
response.slice_pose = ToGeometryMsgPose(
|
||||||
|
cartographer::transform::ToRigid3(response_proto.slice_pose()));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MapBuilderBridge::HandleFinishTrajectory(
|
||||||
|
cartographer_ros_msgs::FinishTrajectory::Request& request,
|
||||||
|
cartographer_ros_msgs::FinishTrajectory::Response& response) {
|
||||||
|
LOG(INFO) << "Finishing trajectory...";
|
||||||
|
|
||||||
|
const int previous_trajectory_id = trajectory_id_;
|
||||||
|
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
|
||||||
|
sensor_bridge_ = cartographer::common::make_unique<SensorBridge>(
|
||||||
|
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
||||||
|
|
||||||
|
map_builder_.FinishTrajectory(previous_trajectory_id);
|
||||||
|
map_builder_.sparse_pose_graph()->RunFinalOptimization();
|
||||||
|
|
||||||
|
const auto trajectory_nodes =
|
||||||
|
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
|
||||||
|
if (trajectory_nodes.empty()) {
|
||||||
|
LOG(WARNING) << "No data collected and no assets will be written.";
|
||||||
|
} else {
|
||||||
|
LOG(INFO) << "Writing assets...";
|
||||||
|
WriteAssets(trajectory_nodes, options_, request.stem);
|
||||||
|
}
|
||||||
|
|
||||||
|
LOG(INFO) << "New trajectory started.";
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
|
||||||
|
cartographer_ros_msgs::SubmapList submap_list;
|
||||||
|
submap_list.header.stamp = ::ros::Time::now();
|
||||||
|
submap_list.header.frame_id = options_.map_frame;
|
||||||
|
for (int trajectory_id = 0;
|
||||||
|
trajectory_id < map_builder_.num_trajectory_builders();
|
||||||
|
++trajectory_id) {
|
||||||
|
const cartographer::mapping::Submaps* submaps =
|
||||||
|
map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
|
||||||
|
const std::vector<cartographer::transform::Rigid3d> submap_transforms =
|
||||||
|
map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
|
||||||
|
CHECK_EQ(submap_transforms.size(), submaps->size());
|
||||||
|
|
||||||
|
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
|
||||||
|
for (int submap_index = 0; submap_index != submaps->size();
|
||||||
|
++submap_index) {
|
||||||
|
cartographer_ros_msgs::SubmapEntry submap_entry;
|
||||||
|
submap_entry.submap_version =
|
||||||
|
submaps->Get(submap_index)->end_laser_fan_index;
|
||||||
|
submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
|
||||||
|
trajectory_submap_list.submap.push_back(submap_entry);
|
||||||
|
}
|
||||||
|
submap_list.trajectory.push_back(trajectory_submap_list);
|
||||||
|
}
|
||||||
|
return submap_list;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cartographer_ros
|
|
@ -0,0 +1,69 @@
|
||||||
|
/*
|
||||||
|
* Copyright 2016 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <unordered_set>
|
||||||
|
|
||||||
|
#include "cartographer/mapping/map_builder.h"
|
||||||
|
#include "cartographer_ros/node_options.h"
|
||||||
|
#include "cartographer_ros/sensor_bridge.h"
|
||||||
|
#include "cartographer_ros/tf_bridge.h"
|
||||||
|
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
||||||
|
#include "cartographer_ros_msgs/SubmapEntry.h"
|
||||||
|
#include "cartographer_ros_msgs/SubmapList.h"
|
||||||
|
#include "cartographer_ros_msgs/SubmapQuery.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
|
||||||
|
class MapBuilderBridge {
|
||||||
|
public:
|
||||||
|
MapBuilderBridge(const NodeOptions& options,
|
||||||
|
const std::unordered_set<string>& expected_sensor_ids,
|
||||||
|
tf2_ros::Buffer* tf_buffer);
|
||||||
|
~MapBuilderBridge();
|
||||||
|
|
||||||
|
MapBuilderBridge(const MapBuilderBridge&) = delete;
|
||||||
|
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
|
||||||
|
|
||||||
|
bool HandleSubmapQuery(
|
||||||
|
cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
|
cartographer_ros_msgs::SubmapQuery::Response& response);
|
||||||
|
bool HandleFinishTrajectory(
|
||||||
|
cartographer_ros_msgs::FinishTrajectory::Request& request,
|
||||||
|
cartographer_ros_msgs::FinishTrajectory::Response& response);
|
||||||
|
|
||||||
|
cartographer_ros_msgs::SubmapList GetSubmapList();
|
||||||
|
|
||||||
|
SensorBridge* sensor_bridge() { return sensor_bridge_.get(); }
|
||||||
|
|
||||||
|
// TODO(damonkohler): Remove these accessors.
|
||||||
|
int trajectory_id() const { return trajectory_id_; }
|
||||||
|
TfBridge* tf_bridge() { return &tf_bridge_; }
|
||||||
|
cartographer::mapping::MapBuilder* map_builder() { return &map_builder_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
const NodeOptions options_;
|
||||||
|
|
||||||
|
std::deque<cartographer::mapping::TrajectoryNode::ConstantData>
|
||||||
|
constant_data_;
|
||||||
|
cartographer::mapping::MapBuilder map_builder_;
|
||||||
|
std::unordered_set<string> expected_sensor_ids_;
|
||||||
|
int trajectory_id_ = -1;
|
||||||
|
TfBridge tf_bridge_;
|
||||||
|
std::unique_ptr<SensorBridge> sensor_bridge_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cartographer_ros
|
|
@ -33,7 +33,7 @@
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
#include "cartographer_ros/assets_writer.h"
|
#include "cartographer_ros/map_builder_bridge.h"
|
||||||
#include "cartographer_ros/msg_conversion.h"
|
#include "cartographer_ros/msg_conversion.h"
|
||||||
#include "cartographer_ros/node_options.h"
|
#include "cartographer_ros/node_options.h"
|
||||||
#include "cartographer_ros/occupancy_grid.h"
|
#include "cartographer_ros/occupancy_grid.h"
|
||||||
|
@ -71,10 +71,10 @@ namespace {
|
||||||
namespace carto = ::cartographer;
|
namespace carto = ::cartographer;
|
||||||
|
|
||||||
using carto::transform::Rigid3d;
|
using carto::transform::Rigid3d;
|
||||||
using carto::kalman_filter::PoseCovariance;
|
|
||||||
|
|
||||||
constexpr int kInfiniteSubscriberQueueSize = 0;
|
constexpr int kInfiniteSubscriberQueueSize = 0;
|
||||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||||
|
constexpr double kTfBufferCacheTimeInSeconds = 1e6;
|
||||||
|
|
||||||
// Unique default topic names. Expected to be remapped as needed.
|
// Unique default topic names. Expected to be remapped as needed.
|
||||||
constexpr char kLaserScanTopic[] = "scan";
|
constexpr char kLaserScanTopic[] = "scan";
|
||||||
|
@ -119,18 +119,9 @@ class Node {
|
||||||
tf2_ros::Buffer tf_buffer_;
|
tf2_ros::Buffer tf_buffer_;
|
||||||
tf2_ros::TransformListener tf_;
|
tf2_ros::TransformListener tf_;
|
||||||
tf2_ros::TransformBroadcaster tf_broadcaster_;
|
tf2_ros::TransformBroadcaster tf_broadcaster_;
|
||||||
TfBridge tf_bridge_;
|
|
||||||
|
|
||||||
carto::common::Mutex mutex_;
|
carto::common::Mutex mutex_;
|
||||||
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
|
std::unique_ptr<MapBuilderBridge> map_builder_bridge_ GUARDED_BY(mutex_);
|
||||||
GUARDED_BY(mutex_);
|
|
||||||
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
|
|
||||||
std::unique_ptr<SensorBridge> sensor_bridge_ GUARDED_BY(mutex_);
|
|
||||||
int trajectory_id_ = -1 GUARDED_BY(mutex_);
|
|
||||||
|
|
||||||
// Set of all topics we subscribe to. We use the non-remapped default names
|
|
||||||
// which are unique.
|
|
||||||
std::unordered_set<string> expected_sensor_ids_;
|
|
||||||
|
|
||||||
::ros::NodeHandle node_handle_;
|
::ros::NodeHandle node_handle_;
|
||||||
::ros::Subscriber imu_subscriber_;
|
::ros::Subscriber imu_subscriber_;
|
||||||
|
@ -155,17 +146,13 @@ class Node {
|
||||||
|
|
||||||
Node::Node(const NodeOptions& options)
|
Node::Node(const NodeOptions& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
tf_buffer_(::ros::Duration(1000)),
|
tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)),
|
||||||
tf_(tf_buffer_),
|
tf_(tf_buffer_) {}
|
||||||
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
|
|
||||||
&tf_buffer_),
|
|
||||||
map_builder_(options.map_builder_options, &constant_data_) {}
|
|
||||||
|
|
||||||
Node::~Node() {
|
Node::~Node() {
|
||||||
{
|
{
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
terminating_ = true;
|
terminating_ = true;
|
||||||
map_builder_.FinishTrajectory(trajectory_id_);
|
|
||||||
}
|
}
|
||||||
if (occupancy_grid_thread_.joinable()) {
|
if (occupancy_grid_thread_.joinable()) {
|
||||||
occupancy_grid_thread_.join();
|
occupancy_grid_thread_.join();
|
||||||
|
@ -174,6 +161,7 @@ Node::~Node() {
|
||||||
|
|
||||||
void Node::Initialize() {
|
void Node::Initialize() {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
|
std::unordered_set<string> expected_sensor_ids;
|
||||||
|
|
||||||
// For 2D SLAM, subscribe to exactly one horizontal laser.
|
// For 2D SLAM, subscribe to exactly one horizontal laser.
|
||||||
if (options_.use_laser_scan) {
|
if (options_.use_laser_scan) {
|
||||||
|
@ -181,19 +169,21 @@ void Node::Initialize() {
|
||||||
kLaserScanTopic, kInfiniteSubscriberQueueSize,
|
kLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||||
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
||||||
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
|
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||||
sensor_bridge_->HandleLaserScanMessage(kLaserScanTopic, msg);
|
map_builder_bridge_->sensor_bridge()->HandleLaserScanMessage(
|
||||||
|
kLaserScanTopic, msg);
|
||||||
}));
|
}));
|
||||||
expected_sensor_ids_.insert(kLaserScanTopic);
|
expected_sensor_ids.insert(kLaserScanTopic);
|
||||||
}
|
}
|
||||||
if (options_.use_multi_echo_laser_scan) {
|
if (options_.use_multi_echo_laser_scan) {
|
||||||
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
|
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
|
||||||
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
|
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||||
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
||||||
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||||
sensor_bridge_->HandleMultiEchoLaserScanMessage(
|
map_builder_bridge_->sensor_bridge()
|
||||||
kMultiEchoLaserScanTopic, msg);
|
->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
|
||||||
|
msg);
|
||||||
}));
|
}));
|
||||||
expected_sensor_ids_.insert(kMultiEchoLaserScanTopic);
|
expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
|
||||||
}
|
}
|
||||||
|
|
||||||
// For 3D SLAM, subscribe to all point clouds topics.
|
// For 3D SLAM, subscribe to all point clouds topics.
|
||||||
|
@ -207,9 +197,10 @@ void Node::Initialize() {
|
||||||
topic, kInfiniteSubscriberQueueSize,
|
topic, kInfiniteSubscriberQueueSize,
|
||||||
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||||
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||||
sensor_bridge_->HandlePointCloud2Message(topic, msg);
|
map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message(
|
||||||
|
topic, msg);
|
||||||
})));
|
})));
|
||||||
expected_sensor_ids_.insert(topic);
|
expected_sensor_ids.insert(topic);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -223,9 +214,10 @@ void Node::Initialize() {
|
||||||
kImuTopic, kInfiniteSubscriberQueueSize,
|
kImuTopic, kInfiniteSubscriberQueueSize,
|
||||||
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
||||||
[this](const sensor_msgs::Imu::ConstPtr& msg) {
|
[this](const sensor_msgs::Imu::ConstPtr& msg) {
|
||||||
sensor_bridge_->HandleImuMessage(kImuTopic, msg);
|
map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic,
|
||||||
|
msg);
|
||||||
}));
|
}));
|
||||||
expected_sensor_ids_.insert(kImuTopic);
|
expected_sensor_ids.insert(kImuTopic);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (options_.use_odometry) {
|
if (options_.use_odometry) {
|
||||||
|
@ -233,14 +225,14 @@ void Node::Initialize() {
|
||||||
kOdometryTopic, kInfiniteSubscriberQueueSize,
|
kOdometryTopic, kInfiniteSubscriberQueueSize,
|
||||||
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
|
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
|
||||||
[this](const nav_msgs::Odometry::ConstPtr& msg) {
|
[this](const nav_msgs::Odometry::ConstPtr& msg) {
|
||||||
sensor_bridge_->HandleOdometryMessage(kOdometryTopic, msg);
|
map_builder_bridge_->sensor_bridge()->HandleOdometryMessage(
|
||||||
|
kOdometryTopic, msg);
|
||||||
}));
|
}));
|
||||||
expected_sensor_ids_.insert(kOdometryTopic);
|
expected_sensor_ids.insert(kOdometryTopic);
|
||||||
}
|
}
|
||||||
|
|
||||||
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
|
map_builder_bridge_ = carto::common::make_unique<MapBuilderBridge>(
|
||||||
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
|
options_, expected_sensor_ids, &tf_buffer_);
|
||||||
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
|
||||||
|
|
||||||
submap_list_publisher_ =
|
submap_list_publisher_ =
|
||||||
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
|
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
|
||||||
|
@ -276,89 +268,27 @@ bool Node::HandleSubmapQuery(
|
||||||
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
::cartographer_ros_msgs::SubmapQuery::Response& response) {
|
::cartographer_ros_msgs::SubmapQuery::Response& response) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
|
return map_builder_bridge_->HandleSubmapQuery(request, response);
|
||||||
carto::mapping::proto::SubmapQuery::Response response_proto;
|
|
||||||
const std::string error = map_builder_.SubmapToProto(
|
|
||||||
request.trajectory_id, request.submap_index, &response_proto);
|
|
||||||
if (!error.empty()) {
|
|
||||||
LOG(ERROR) << error;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
response.submap_version = response_proto.submap_version();
|
|
||||||
response.cells.insert(response.cells.begin(), response_proto.cells().begin(),
|
|
||||||
response_proto.cells().end());
|
|
||||||
response.width = response_proto.width();
|
|
||||||
response.height = response_proto.height();
|
|
||||||
response.resolution = response_proto.resolution();
|
|
||||||
response.slice_pose = ToGeometryMsgPose(
|
|
||||||
carto::transform::ToRigid3(response_proto.slice_pose()));
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Node::HandleFinishTrajectory(
|
bool Node::HandleFinishTrajectory(
|
||||||
::cartographer_ros_msgs::FinishTrajectory::Request& request,
|
::cartographer_ros_msgs::FinishTrajectory::Request& request,
|
||||||
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
|
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
|
||||||
LOG(INFO) << "Finishing trajectory...";
|
|
||||||
|
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
const int previous_trajectory_id = trajectory_id_;
|
return map_builder_bridge_->HandleFinishTrajectory(request, response);
|
||||||
|
|
||||||
trajectory_id_ =
|
|
||||||
map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
|
|
||||||
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
|
|
||||||
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
|
||||||
|
|
||||||
map_builder_.FinishTrajectory(previous_trajectory_id);
|
|
||||||
map_builder_.sparse_pose_graph()->RunFinalOptimization();
|
|
||||||
|
|
||||||
const auto trajectory_nodes =
|
|
||||||
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
|
|
||||||
if (trajectory_nodes.empty()) {
|
|
||||||
LOG(WARNING) << "No data collected and no assets will be written.";
|
|
||||||
} else {
|
|
||||||
LOG(INFO) << "Writing assets...";
|
|
||||||
WriteAssets(trajectory_nodes, options_, request.stem);
|
|
||||||
}
|
|
||||||
|
|
||||||
LOG(INFO) << "New trajectory started.";
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) {
|
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
|
||||||
::cartographer_ros_msgs::SubmapList ros_submap_list;
|
|
||||||
ros_submap_list.header.stamp = ::ros::Time::now();
|
|
||||||
ros_submap_list.header.frame_id = options_.map_frame;
|
|
||||||
|
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
for (int trajectory_id = 0;
|
submap_list_publisher_.publish(map_builder_bridge_->GetSubmapList());
|
||||||
trajectory_id < map_builder_.num_trajectory_builders();
|
|
||||||
++trajectory_id) {
|
|
||||||
const carto::mapping::Submaps* submaps =
|
|
||||||
map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
|
|
||||||
const std::vector<carto::transform::Rigid3d> submap_transforms =
|
|
||||||
map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
|
|
||||||
CHECK_EQ(submap_transforms.size(), submaps->size());
|
|
||||||
|
|
||||||
::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory;
|
|
||||||
for (int submap_index = 0; submap_index != submaps->size();
|
|
||||||
++submap_index) {
|
|
||||||
::cartographer_ros_msgs::SubmapEntry ros_submap;
|
|
||||||
ros_submap.submap_version =
|
|
||||||
submaps->Get(submap_index)->end_laser_fan_index;
|
|
||||||
ros_submap.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
|
|
||||||
ros_trajectory.submap.push_back(ros_submap);
|
|
||||||
}
|
|
||||||
ros_submap_list.trajectory.push_back(ros_trajectory);
|
|
||||||
}
|
|
||||||
submap_list_publisher_.publish(ros_submap_list);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::PublishPoseAndScanMatchedPointCloud(
|
void Node::PublishPoseAndScanMatchedPointCloud(
|
||||||
const ::ros::WallTimerEvent& timer_event) {
|
const ::ros::WallTimerEvent& timer_event) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
const carto::mapping::TrajectoryBuilder* trajectory_builder =
|
const carto::mapping::TrajectoryBuilder* trajectory_builder =
|
||||||
map_builder_.GetTrajectoryBuilder(trajectory_id_);
|
map_builder_bridge_->map_builder()->GetTrajectoryBuilder(
|
||||||
|
map_builder_bridge_->trajectory_id());
|
||||||
const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate =
|
const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate =
|
||||||
trajectory_builder->pose_estimate();
|
trajectory_builder->pose_estimate();
|
||||||
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
|
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
|
||||||
|
@ -367,8 +297,9 @@ void Node::PublishPoseAndScanMatchedPointCloud(
|
||||||
|
|
||||||
const Rigid3d tracking_to_local = last_pose_estimate.pose;
|
const Rigid3d tracking_to_local = last_pose_estimate.pose;
|
||||||
const Rigid3d local_to_map =
|
const Rigid3d local_to_map =
|
||||||
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
|
map_builder_bridge_->map_builder()
|
||||||
*trajectory_builder->submaps());
|
->sparse_pose_graph()
|
||||||
|
->GetLocalToGlobalTransform(*trajectory_builder->submaps());
|
||||||
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
|
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
|
||||||
|
|
||||||
geometry_msgs::TransformStamped stamped_transform;
|
geometry_msgs::TransformStamped stamped_transform;
|
||||||
|
@ -390,7 +321,8 @@ void Node::PublishPoseAndScanMatchedPointCloud(
|
||||||
stamped_transform.header.stamp = ros::Time::now();
|
stamped_transform.header.stamp = ros::Time::now();
|
||||||
}
|
}
|
||||||
|
|
||||||
const auto published_to_tracking = tf_bridge_.LookupToTracking(
|
const auto published_to_tracking =
|
||||||
|
map_builder_bridge_->tf_bridge()->LookupToTracking(
|
||||||
last_pose_estimate.time, options_.published_frame);
|
last_pose_estimate.time, options_.published_frame);
|
||||||
if (published_to_tracking != nullptr) {
|
if (published_to_tracking != nullptr) {
|
||||||
if (options_.provide_odom_frame) {
|
if (options_.provide_odom_frame) {
|
||||||
|
@ -430,8 +362,9 @@ void Node::SpinOccupancyGridThreadForever() {
|
||||||
if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
|
if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
const auto trajectory_nodes =
|
const auto trajectory_nodes = map_builder_bridge_->map_builder()
|
||||||
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
|
->sparse_pose_graph()
|
||||||
|
->GetTrajectoryNodes();
|
||||||
if (trajectory_nodes.empty()) {
|
if (trajectory_nodes.empty()) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue