Add support for multiple consecutive trajectories. (#151)
When the /finish_trajectory service is called, a new trajectory is started with the same sensor configuration. This enables "pause and resume".master
Wolfgang Hess
GitHub
parent
7d95852192
commit
a1eb540ffa
|
|
@ -29,17 +29,11 @@
|
|||
#include "cartographer/common/thread_pool.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping/map_builder.h"
|
||||
#include "cartographer/mapping/proto/submap_visualization.pb.h"
|
||||
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/sparse_pose_graph.h"
|
||||
#include "cartographer/mapping_3d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||
#include "cartographer/sensor/collator.h"
|
||||
#include "cartographer/sensor/data.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
|
|
@ -87,8 +81,6 @@ namespace proto = carto::sensor::proto;
|
|||
using carto::transform::Rigid3d;
|
||||
using carto::kalman_filter::PoseCovariance;
|
||||
|
||||
// TODO(hrapp): Support multi trajectory mapping.
|
||||
constexpr int64 kTrajectoryBuilderId = 0;
|
||||
constexpr int kInfiniteSubscriberQueueSize = 0;
|
||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||
|
||||
|
|
@ -142,6 +134,11 @@ class Node {
|
|||
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::Subscriber imu_subscriber_;
|
||||
|
|
@ -183,9 +180,7 @@ Node::~Node() {
|
|||
}
|
||||
|
||||
void Node::Initialize() {
|
||||
// Set of all topics we subscribe to. We use the non-remapped default names
|
||||
// which are unique.
|
||||
std::unordered_set<string> expected_sensor_ids;
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
|
||||
// For 2D SLAM, subscribe to exactly one horizontal laser.
|
||||
if (options_.use_horizontal_laser) {
|
||||
|
|
@ -195,7 +190,7 @@ void Node::Initialize() {
|
|||
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
sensor_bridge_->HandleLaserScanMessage(kLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_ids.insert(kLaserScanTopic);
|
||||
expected_sensor_ids_.insert(kLaserScanTopic);
|
||||
}
|
||||
if (options_.use_horizontal_multi_echo_laser) {
|
||||
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
|
||||
|
|
@ -205,7 +200,7 @@ void Node::Initialize() {
|
|||
sensor_bridge_->HandleMultiEchoLaserScanMessage(
|
||||
kMultiEchoLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
|
||||
expected_sensor_ids_.insert(kMultiEchoLaserScanTopic);
|
||||
}
|
||||
|
||||
// For 3D SLAM, subscribe to all 3D lasers.
|
||||
|
|
@ -221,7 +216,7 @@ void Node::Initialize() {
|
|||
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
sensor_bridge_->HandlePointCloud2Message(topic, msg);
|
||||
})));
|
||||
expected_sensor_ids.insert(topic);
|
||||
expected_sensor_ids_.insert(topic);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -237,7 +232,7 @@ void Node::Initialize() {
|
|||
[this](const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
sensor_bridge_->HandleImuMessage(kImuTopic, msg);
|
||||
}));
|
||||
expected_sensor_ids.insert(kImuTopic);
|
||||
expected_sensor_ids_.insert(kImuTopic);
|
||||
}
|
||||
|
||||
if (options_.use_odometry_data) {
|
||||
|
|
@ -247,15 +242,13 @@ void Node::Initialize() {
|
|||
[this](const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
sensor_bridge_->HandleOdometryMessage(kOdometryTopic, msg);
|
||||
}));
|
||||
expected_sensor_ids.insert(kOdometryTopic);
|
||||
expected_sensor_ids_.insert(kOdometryTopic);
|
||||
}
|
||||
|
||||
// TODO(damonkohler): Add multi-trajectory support.
|
||||
CHECK_EQ(kTrajectoryBuilderId,
|
||||
map_builder_.AddTrajectoryBuilder(expected_sensor_ids));
|
||||
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
|
||||
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
|
||||
options_.sensor_bridge_options, &tf_bridge_,
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId));
|
||||
map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
||||
|
||||
submap_list_publisher_ =
|
||||
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
|
||||
|
|
@ -290,28 +283,36 @@ void Node::Initialize() {
|
|||
bool Node::HandleSubmapQuery(
|
||||
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||
::cartographer_ros_msgs::SubmapQuery::Response& response) {
|
||||
if (request.trajectory_id != 0) {
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
|
||||
if (request.trajectory_id < 0 ||
|
||||
request.trajectory_id >= map_builder_.num_trajectory_builders()) {
|
||||
LOG(ERROR) << "Requested submap from trajectory " << request.trajectory_id
|
||||
<< " but there are only "
|
||||
<< map_builder_.num_trajectory_builders() << " trajectories.";
|
||||
return false;
|
||||
}
|
||||
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
// TODO(hrapp): return error messages and extract common code from MapBuilder.
|
||||
const carto::mapping::Submaps* const submaps =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
|
||||
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
|
||||
map_builder_.GetTrajectoryBuilder(request.trajectory_id)->submaps();
|
||||
if (request.submap_index < 0 || request.submap_index >= submaps->size()) {
|
||||
LOG(ERROR) << "Requested submap " << request.submap_index
|
||||
<< " from trajectory " << request.trajectory_id
|
||||
<< " but there are only " << submaps->size()
|
||||
<< " submaps in this trajectory.";
|
||||
return false;
|
||||
}
|
||||
|
||||
carto::mapping::proto::SubmapQuery::Response response_proto;
|
||||
response_proto.set_submap_id(request.submap_id);
|
||||
response_proto.set_submap_version(
|
||||
submaps->Get(request.submap_id)->end_laser_fan_index);
|
||||
submaps->Get(request.submap_index)->end_laser_fan_index);
|
||||
const std::vector<carto::transform::Rigid3d> submap_transforms =
|
||||
map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
|
||||
|
||||
submaps->SubmapToProto(request.submap_id,
|
||||
CHECK_EQ(submap_transforms.size(), submaps->size());
|
||||
submaps->SubmapToProto(request.submap_index,
|
||||
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(),
|
||||
submap_transforms[request.submap_id], &response_proto);
|
||||
submap_transforms[request.submap_index],
|
||||
&response_proto);
|
||||
|
||||
response.submap_version = response_proto.submap_version();
|
||||
response.cells.insert(response.cells.begin(), response_proto.cells().begin(),
|
||||
|
|
@ -340,14 +341,8 @@ bool Node::HandleSubmapQuery(
|
|||
bool Node::HandleFinishTrajectory(
|
||||
::cartographer_ros_msgs::FinishTrajectory::Request& request,
|
||||
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
|
||||
// After shutdown, ROS messages will no longer be received and therefore not
|
||||
// pile up.
|
||||
//
|
||||
// TODO(whess): Continue with a new trajectory instead?
|
||||
::ros::shutdown();
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
// TODO(whess): Add multi-trajectory support.
|
||||
map_builder_.FinishTrajectory(kTrajectoryBuilderId);
|
||||
map_builder_.FinishTrajectory(trajectory_id_);
|
||||
map_builder_.sparse_pose_graph()->RunFinalOptimization();
|
||||
|
||||
const auto trajectory_nodes =
|
||||
|
|
@ -357,48 +352,59 @@ bool Node::HandleFinishTrajectory(
|
|||
return true;
|
||||
}
|
||||
WriteAssets(trajectory_nodes, options_, request.stem);
|
||||
|
||||
// Start a new trajectory.
|
||||
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
|
||||
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
|
||||
options_.sensor_bridge_options, &tf_bridge_,
|
||||
map_builder_.GetTrajectoryBuilder(trajectory_id_));
|
||||
return true;
|
||||
}
|
||||
|
||||
void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) {
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
const carto::mapping::Submaps* submaps =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->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 i = 0; i != submaps->size(); ++i) {
|
||||
::cartographer_ros_msgs::SubmapEntry ros_submap;
|
||||
ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index;
|
||||
ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]);
|
||||
ros_trajectory.submap.push_back(ros_submap);
|
||||
}
|
||||
|
||||
::cartographer_ros_msgs::SubmapList ros_submap_list;
|
||||
ros_submap_list.header.stamp = ::ros::Time::now();
|
||||
ros_submap_list.header.frame_id = options_.map_frame;
|
||||
ros_submap_list.trajectory.push_back(ros_trajectory);
|
||||
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
for (int trajectory_id = 0;
|
||||
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(
|
||||
const ::ros::WallTimerEvent& timer_event) {
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
const carto::mapping::GlobalTrajectoryBuilderInterface::PoseEstimate
|
||||
last_pose_estimate =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
|
||||
->pose_estimate();
|
||||
const carto::mapping::TrajectoryBuilder* trajectory_builder =
|
||||
map_builder_.GetTrajectoryBuilder(trajectory_id_);
|
||||
const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate =
|
||||
trajectory_builder->pose_estimate();
|
||||
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Rigid3d tracking_to_local = last_pose_estimate.pose;
|
||||
const carto::mapping::Submaps* submaps =
|
||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
|
||||
const Rigid3d local_to_map =
|
||||
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps);
|
||||
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
|
||||
*trajectory_builder->submaps());
|
||||
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
|
||||
|
||||
geometry_msgs::TransformStamped stamped_transform;
|
||||
|
|
|
|||
|
|
@ -12,8 +12,8 @@
|
|||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
int32 submap_id
|
||||
int32 trajectory_id
|
||||
int32 submap_index
|
||||
---
|
||||
int32 submap_version
|
||||
uint8[] cells
|
||||
|
|
|
|||
|
|
@ -46,25 +46,28 @@ constexpr double kFadeOutStartDistanceInMeters = 1.;
|
|||
constexpr double kFadeOutDistanceInMeters = 2.;
|
||||
constexpr float kAlphaUpdateThreshold = 0.2f;
|
||||
|
||||
std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) {
|
||||
return std::to_string(trajectory_id) + "-" + std::to_string(submap_id);
|
||||
std::string GetSubmapIdentifier(const int trajectory_id,
|
||||
const int submap_index) {
|
||||
return std::to_string(trajectory_id) + "-" + std::to_string(submap_index);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
|
||||
DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index,
|
||||
Ogre::SceneManager* const scene_manager)
|
||||
: submap_id_(submap_id),
|
||||
trajectory_id_(trajectory_id),
|
||||
: trajectory_id_(trajectory_id),
|
||||
submap_index_(submap_index),
|
||||
scene_manager_(scene_manager),
|
||||
scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
|
||||
manual_object_(scene_manager->createManualObject(
|
||||
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
|
||||
kManualObjectPrefix +
|
||||
GetSubmapIdentifier(trajectory_id, submap_index))),
|
||||
last_query_timestamp_(0) {
|
||||
material_ = Ogre::MaterialManager::getSingleton().getByName(
|
||||
kSubmapSourceMaterialName);
|
||||
material_ = material_->clone(kSubmapMaterialPrefix +
|
||||
GetSubmapIdentifier(trajectory_id_, submap_id));
|
||||
material_ =
|
||||
material_->clone(kSubmapMaterialPrefix +
|
||||
GetSubmapIdentifier(trajectory_id_, submap_index));
|
||||
material_->setReceiveShadows(false);
|
||||
material_->getTechnique(0)->setLightingEnabled(false);
|
||||
material_->setCullingMode(Ogre::CULL_NONE);
|
||||
|
|
@ -121,8 +124,8 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
|
|||
last_query_timestamp_ = now;
|
||||
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
|
||||
::cartographer_ros_msgs::SubmapQuery srv;
|
||||
srv.request.submap_id = submap_id_;
|
||||
srv.request.trajectory_id = trajectory_id_;
|
||||
srv.request.submap_index = submap_index_;
|
||||
if (client->call(srv)) {
|
||||
// We emit a signal to update in the right thread, and pass via the
|
||||
// 'response_' member to simplify the signal-slot connection slightly.
|
||||
|
|
@ -203,7 +206,7 @@ void DrawableSubmap::UpdateSceneNode() {
|
|||
texture_.setNull();
|
||||
}
|
||||
const std::string texture_name =
|
||||
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_);
|
||||
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_index_);
|
||||
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
|
||||
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
|
||||
pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB,
|
||||
|
|
|
|||
|
|
@ -43,9 +43,9 @@ class DrawableSubmap : public QObject {
|
|||
Q_OBJECT
|
||||
|
||||
public:
|
||||
// Each submap is identified by a 'trajectory_id' plus a 'submap_id'.
|
||||
// Each submap is identified by a 'trajectory_id' plus a 'submap_index'.
|
||||
// 'scene_manager' is the Ogre scene manager to which to add a node.
|
||||
DrawableSubmap(int submap_id, int trajectory_id,
|
||||
DrawableSubmap(int trajectory_id, int submap_index,
|
||||
Ogre::SceneManager* scene_manager);
|
||||
~DrawableSubmap() override;
|
||||
DrawableSubmap(const DrawableSubmap&) = delete;
|
||||
|
|
@ -80,8 +80,8 @@ class DrawableSubmap : public QObject {
|
|||
void UpdateTransform();
|
||||
float UpdateAlpha(float target_alpha);
|
||||
|
||||
const int submap_id_;
|
||||
const int trajectory_id_;
|
||||
const int submap_index_;
|
||||
|
||||
::cartographer::common::Mutex mutex_;
|
||||
Ogre::SceneManager* const scene_manager_;
|
||||
|
|
|
|||
|
|
@ -99,14 +99,16 @@ void SubmapsDisplay::processMessage(
|
|||
auto& trajectory = trajectories_[trajectory_id];
|
||||
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
|
||||
msg->trajectory[trajectory_id].submap;
|
||||
for (size_t submap_id = 0; submap_id < submap_entries.size(); ++submap_id) {
|
||||
if (submap_id >= trajectory.size()) {
|
||||
for (size_t submap_index = 0; submap_index < submap_entries.size();
|
||||
++submap_index) {
|
||||
if (submap_index >= trajectory.size()) {
|
||||
trajectory.push_back(
|
||||
::cartographer::common::make_unique<DrawableSubmap>(
|
||||
submap_id, trajectory_id, context_->getSceneManager()));
|
||||
trajectory_id, submap_index, context_->getSceneManager()));
|
||||
}
|
||||
trajectory[submap_id]->Update(msg->header, submap_entries[submap_id],
|
||||
context_->getFrameManager());
|
||||
trajectory[submap_index]->Update(msg->header,
|
||||
submap_entries[submap_index],
|
||||
context_->getFrameManager());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -136,11 +138,11 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
|
|||
++num_ongoing_requests;
|
||||
}
|
||||
}
|
||||
for (int submap_id = trajectory.size() - 1;
|
||||
submap_id >= 0 &&
|
||||
for (int submap_index = trajectory.size() - 1;
|
||||
submap_index >= 0 &&
|
||||
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
|
||||
--submap_id) {
|
||||
if (trajectory[submap_id]->MaybeFetchTexture(&client_)) {
|
||||
--submap_index) {
|
||||
if (trajectory[submap_index]->MaybeFetchTexture(&client_)) {
|
||||
++num_ongoing_requests;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue