diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index e439b10..aa90033 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -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 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 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 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( 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 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( + 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 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 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; diff --git a/cartographer_ros_msgs/srv/SubmapQuery.srv b/cartographer_ros_msgs/srv/SubmapQuery.srv index d116123..f57bdda 100644 --- a/cartographer_ros_msgs/srv/SubmapQuery.srv +++ b/cartographer_ros_msgs/srv/SubmapQuery.srv @@ -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 diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index b7dada4..e1fc68c 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -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, diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index a59394e..3ef89b0 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -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_; diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 169b253..3239f0b 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -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( - 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; } }