一个子图的更新显示，跨越了从cartographer_rviz->cartographer_ros->cartographer。
其中cartographer_rviz->cartographer_ros之间是ros架构的。
cartographer_ros->cartographer直接调用了cartographer开放的接口。
1-首先cartographer_rviz写了一个rviz的显示插件，即submap_display，并导出：
PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display)
submap_display.cc中的函数update用来进行地图更新的，
// 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;
}
重点是这个函数 it->second->MaybeFetchTexture(&client)，它是来自DrawableSubmap类的函数。这个client是啥呢？我们看他的初始化，可以知道这是一个请求ros服务的客户端，请求的服务名称为”/submap_query“。
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
submap_query_service_property_->getStdString());
我们继续把DrawableSubmap::MaybeFetchTexture函数展开:
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;
}
2-重点是这个函数 ::cartographer_ros::FetchSubmapTextures(id_, client)，id是submap的id，client就是上面传下来的。我们继续在cartographer_ros里展开这个函数。
std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client) {
::cartographer_ros_msgs::SubmapQuery srv;
srv.request.trajectory_id = submap_id.trajectory_id;
srv.request.submap_index = submap_id.submap_index;
if (!client->call(srv) ||
srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) {
return nullptr;
}
if (srv.response.textures.empty()) {
return nullptr;
}
auto response = absl::make_unique<::cartographer::io::SubmapTextures>();
response->version = srv.response.submap_version;
for (const auto& texture : srv.response.textures) {
const std::string compressed_cells(texture.cells.begin(),
texture.cells.end());
response->textures.emplace_back(::cartographer::io::SubmapTexture{
::cartographer::io::UnpackTextureData(compressed_cells, texture.width,
texture.height),
texture.width, texture.height, texture.resolution,
ToRigid3d(texture.slice_pose)});
}
return response;
}
重点是这个函数client->call(srv)，srv.responce就是我们得到的子图数据。那这个客户端请求的服务函数在哪呢？
我们在程序里搜素“submap_query“，可以找到
constexpr char kSubmapQueryServiceName[] = "submap_query";
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
这个服务端的函数是Node::HandleSubmapQuery，我们继续把它展开
bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) {
absl::MutexLock lock(&mutex_);
map_builder_bridge_.HandleSubmapQuery(request, response);
return true;
}
可以看到这个变量继续向下传递，我们继续展开 map_builder_bridge_.HandleSubmapQuery(request, response);
void MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response) {
cartographer::mapping::proto::SubmapQuery::Response response_proto;
cartographer::mapping::SubmapId submap_id{
request.trajectory_id,
request.submap_index};
const std::string error =
map_builder_->SubmapToProto(submap_id, &response_proto);
if (!error.empty()) {
LOG(ERROR) << error;
response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
response.status.message = error;
return;
}
response.submap_version = response_proto.submap_version();
for (const auto& texture_proto : response_proto.textures()) {
response.textures.emplace_back();
auto& texture = response.textures.back();
texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
texture_proto.cells().end());
texture.width = texture_proto.width();
texture.height = texture_proto.height();
texture.resolution = texture_proto.resolution();
texture.slice_pose = ToGeometryMsgPose(
cartographer::transform::ToRigid3(texture_proto.slice_pose()));
}
response.status.message = "Success.";
response.status.code = cartographer_ros_msgs::StatusCode::OK;
}
3-重点是map_builder_->SubmapToProto(submap_id, &response_proto);这句话，这里的map_builder_是cartographer里MapBuilderStub类实例化的对象，我们在cartographer里看这个函数SubmapToProto：
std::string MapBuilderStub::SubmapToProto(
const mapping::SubmapId& submap_id,
mapping::proto::SubmapQuery::Response* submap_query_response) {
proto::GetSubmapRequest request;
submap_id.ToProto(request.mutable_submap_id());
async_grpc::Client<handlers::GetSubmapSignature> client(client_channel_);
CHECK(client.Write(request));
submap_query_response->CopyFrom(client.response().submap_query_response());
return client.response().error_msg();
}
可以看到这里使用async_grpc::Client类实例化的对象通过grpc调用远程的grpc服务器，最终返回的client.response().submap_query_response()赋值给submap_query_response，至此一个submap的请求传递完毕。
4-那我们自己写一个客户端请求getsubmap要怎么写呢？
第1种方法：我认为可以使用async_grpc库的Client，创建一个client_channel_，然后就像上面的例子：
我们看下我们这个要传递查询的数据在proto的那里，在这里
cartographer/mapping/proto/submap_visualization.proto。
来看下这个内容
message SubmapQuery {
message Request {
// Index into 'SubmapList.trajectory(trajectory_id).submap'.
int32 submap_index = 1;
// Index into 'TrajectoryList.trajectory'.
int32 trajectory_id = 2;
}
message Response {
// Version of the given submap, higher means newer.
int32 submap_version = 2;
// Texture that visualizes a grid of a submap.
message SubmapTexture {
// GZipped map data, in row-major order, starting with (0,0). Each cell
// consists of two bytes: value (premultiplied by alpha) and alpha.
bytes cells = 1;
// Dimensions of the grid in cells.
int32 width = 2;
int32 height = 3;
// Size of one cell in meters.
double resolution = 4;
// Pose of the resolution*width x resolution*height rectangle in the
// submap frame.
transform.proto.Rigid3d slice_pose = 5;
}
// When multiple textures are present, high resolution comes first.
repeated SubmapTexture textures = 10;
// Error message in response to malformed requests.
string error_message = 8;
}
Request request = 1;
Response response = 2;
}
我们再看下这个请求rpc的proto在哪里？在这里cartographer/cloud/proto/map_builder_service.proto
// Retrieves a single submap.
rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse);
来看async_grpc::client的构造
async_grpc::Client<handlers::GetSubmapSignature> client(client_channel_);
来看client_channel_的构造
: client_channel_(::grpc::CreateChannel(
server_address, ::grpc::InsecureChannelCredentials())),
来看GetSubmapSignature的构造
DEFINE_HANDLER_SIGNATURE(
GetSubmapSignature, proto::GetSubmapRequest, proto::GetSubmapResponse,
"/cartographer.cloud.proto.MapBuilderService/GetSubmap")
这里使用了DEFINE_HANDLER_SIGNATURE宏，来看看这个宏
#define DEFINE_HANDLER_SIGNATURE(traitsName, incomingType, outgoingType, \
methodName) \
struct traitsName { \
using IncomingType = incomingType; \
using OutgoingType = outgoingType; \
static const char* MethodName() { return methodName; } \
};
第2种方法：我们知道proto的类型，使用常规的写法也可以。
至此整个流程解释完毕。