cartographer

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函数中，这句话就是更新的子图的位置

trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);

继续跟进这个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(); } }

其中比较重要的一个函数，

it->second->MaybeFetchTexture(&client_)

继续跟进这个函数，它获取了子图的纹理，然后通过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得到。