cartographer寫了一個插件，其中的類函數是SubmapsDisplay，這個類中有2個比較重要的函數，一個是processMessage，用來接收每個子圖之間的位置關系，一個是update，用來更新每個子圖的紋理
SubmapsDisplay::processMessage（）：
void SubmapsDisplay::processMessage(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
absl::MutexLock locker(&mutex_);
map_frame_ = absl::make_unique<std::string>(msg->header.frame_id);
// In case Cartographer node is relaunched, destroy trajectories from the
// previous instance.
for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
const size_t trajectory_id = submap_entry.trajectory_id;
if (trajectories_.count(trajectory_id) == 0) {
continue;
}
const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
const auto it = trajectory_submaps.find(submap_entry.submap_index);
if (it != trajectory_submaps.end() &&
it->second->version() > submap_entry.submap_version) {
// Versions should only increase unless Cartographer restarted.
trajectories_.clear();
break;
}
}
using ::cartographer::mapping::SubmapId;
std::set<SubmapId> listed_submaps;
std::set<int> listed_trajectories;
for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
listed_submaps.insert(id);
listed_trajectories.insert(submap_entry.trajectory_id);
if (trajectories_.count(id.trajectory_id) == 0) {
trajectories_.insert(std::make_pair(
id.trajectory_id,
absl::make_unique<Trajectory>(
absl::make_unique<::rviz::BoolProperty>(
QString("Trajectory %1").arg(id.trajectory_id),
visibility_all_enabled_->getBool(),
QString(
"List of all submaps in Trajectory %1. The checkbox "
"controls whether all submaps in this trajectory should "
"be displayed or not.")
.arg(id.trajectory_id),
trajectories_category_),
pose_markers_all_enabled_->getBool())));
}
auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
auto& pose_markers_visibility =
trajectories_[id.trajectory_id]->pose_markers_visibility;
if (trajectory_submaps.count(id.submap_index) == 0) {
// TODO(ojura): Add RViz properties for adjusting submap pose axes
constexpr float kSubmapPoseAxesLength = 0.3f;
constexpr float kSubmapPoseAxesRadius = 0.06f;
trajectory_submaps.emplace(
id.submap_index,
absl::make_unique<DrawableSubmap>(
id, context_, map_node_, trajectory_visibility.get(),
trajectory_visibility->getBool(),
pose_markers_visibility->getBool(), kSubmapPoseAxesLength,
kSubmapPoseAxesRadius));
trajectory_submaps.at(id.submap_index)
->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool());
trajectory_submaps.at(id.submap_index)
->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool());
}
trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
}
// Remove all deleted trajectories not mentioned in the SubmapList.
for (auto it = trajectories_.begin(); it != trajectories_.end();) {
if (listed_trajectories.count(it->first) == 0) {
it = trajectories_.erase(it);
} else {
++it;
}
}
// Remove all submaps not mentioned in the SubmapList.
for (const auto& trajectory_by_id : trajectories_) {
const int trajectory_id = trajectory_by_id.first;
auto& trajectory_submaps = trajectory_by_id.second->submaps;
for (auto it = trajectory_submaps.begin();
it != trajectory_submaps.end();) {
if (listed_submaps.count(
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
it = trajectory_submaps.erase(it);
} else {
++it;
}
}
}
}
processMessage函數中，這句話就是更新的子圖的位置
繼續跟進這個Update，它把子圖的位置指派給了submap_node_，submap_node_是繪圖類orge的一個圖元。
void DrawableSubmap::Update(
const ::std_msgs::Header& header,
const ::cartographer_ros_msgs::SubmapEntry& metadata) {
absl::MutexLock locker(&mutex_);
metadata_version_ = metadata.submap_version;
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
submap_node_->setPosition(ToOgre(pose_.translation()));
submap_node_->setOrientation(ToOgre(pose_.rotation()));
display_context_->queueRender();
visibility_->setName(
QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
visibility_->setDescription(
QString("Toggle visibility of this individual submap.<br><br>"
"Trajectory %1, submap %2, submap version %3")
.arg(id_.trajectory_id)
.arg(id_.submap_index)
.arg(metadata_version_));
}
繼續看這個函數，SubmapsDisplay::update（）：，它更新了子圖的紋理
void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
absl::MutexLock locker(&mutex_);
// Schedule fetching of new submap textures.
for (const auto& trajectory_by_id : trajectories_) {
int num_ongoing_requests = 0;
for (const auto& submap_entry : trajectory_by_id.second->submaps) {
if (submap_entry.second->QueryInProgress()) {
++num_ongoing_requests;
}
}
for (auto it = trajectory_by_id.second->submaps.rbegin();
it != trajectory_by_id.second->submaps.rend() &&
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
++it) {
if (it->second->MaybeFetchTexture(&client_)) {
++num_ongoing_requests;
}
}
}
if (map_frame_ == nullptr) {
return;
}
// Update the fading by z distance.
const ros::Time kLatest(0);
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(
*map_frame_, tracking_frame_property_->getStdString(), kLatest);
for (auto& trajectory_by_id : trajectories_) {
for (auto& submap_entry : trajectory_by_id.second->submaps) {
submap_entry.second->SetAlpha(
transform_stamped.transform.translation.z,
fade_out_start_distance_in_meters_->getFloat());
}
}
} catch (const tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what());
}
// Update the map frame to fixed frame transform.
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position,
orientation)) {
map_node_->setPosition(position);
map_node_->setOrientation(orientation);
context_->queueRender();
}
}
其中比較重要的一個函數，
繼續跟進這個函數，它擷取了子圖的紋理，然後通過EMIT RequestSucceeded，讓UpdateSceneNode繼續處理接收到的子圖
bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
absl::MutexLock locker(&mutex_);
// Received metadata version can also be lower if we restarted Cartographer.
const bool newer_version_available =
submap_textures_ == nullptr ||
submap_textures_->version != metadata_version_;
const std::chrono::milliseconds now =
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch());
const bool recently_queried =
last_query_timestamp_ + kMinQueryDelayInMs > now;
if (!newer_version_available || recently_queried || query_in_progress_) {
return false;
}
query_in_progress_ = true;
last_query_timestamp_ = now;
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures =
::cartographer_ros::FetchSubmapTextures(id_, client);
absl::MutexLock locker(&mutex_);
query_in_progress_ = false;
if (submap_textures != nullptr) {
// We emit a signal to update in the right thread, and pass via the
// 'submap_texture_' member to simplify the signal-slot connection
// slightly.
submap_textures_ = std::move(submap_textures);
Q_EMIT RequestSucceeded();
}
});
return true;
}
來看一下UpdateSceneNode這個函數，ogre_slices_更新紋理，其中它的成員變量slice_node_是上一步submap_node_的子圖元，他們之間沒有坐标偏移，可以認為他們是同一坐标系。
void DrawableSubmap::UpdateSceneNode() {
absl::MutexLock locker(&mutex_);
for (size_t slice_index = 0; slice_index < ogre_slices_.size() &&
slice_index < submap_textures_->textures.size();
++slice_index) {
ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]);
}
display_context_->queueRender();
}
至此我們知道了cartographer_rviz的繪制方式，就是submap_node_控制一個子圖的繪制，每個子圖有一個子圖原點，這個是submap_query中slice_pose。而每個子圖之間是有微弱旋轉的，而控制旋轉的中心也是這個slice_pose。而每個子圖之間的旋轉和平移可以通過/submap_list的pose得到。
![cartographer架構設計圖[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)