cartographer添加自己的grpc服务函数

Posted 2021-12-28 COCO_PEAK_NOODLE

第一步：在cartographer/cloud/proto的map_builder_service.proto文件中添加request和responce数据结构，如下，然后编译生成。

message agv_UpdateTrajectoryRequest
  cartographer.transform.proto.Rigid3d initial_pose = 1;
  string trajectory_id = 2;
  bool use_initial_pose = 3;


message agv_UpdateTrajectoryResponse 
  bool state = 1;
  int32 code = 2;
  string message = 3;

第二步：在cartographer/cloud/internal/handlers下构建回调处理类，我的是
update_trajectory.h和update_trajectory.cc

#ifndef AGV_LOCALIZE_TRAJECTROY_H
#define AGV_LOCALIZE_TRAJECTROY_H

#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"

namespace cartographer 
namespace cloud 
namespace handlers 

DEFINE_HANDLER_SIGNATURE(
    agvUpdateTrajectorySignature, proto::agv_UpdateTrajectoryRequest,
    proto::agv_UpdateTrajectoryResponse,
    "/cartographer.cloud.proto.MapBuilderService/agv_UpdateTrajectory")

class agvUpdateTrajectoryHandler
    : public async_grpc::RpcHandler<agvUpdateTrajectorySignature> 
 public:
  void OnRequest(const proto::agv_UpdateTrajectoryRequest& request) override;
;

  // namespace handlers
  // namespace cloud
  // namespace cartographer

#endif

#include "cartographer/cloud/internal/handlers/update_trajectory.h"

#include "absl/memory/memory.h"
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"

#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/map_builder_interface.h"

namespace cartographer 
namespace cloud 
namespace handlers 

using TrajectoryType = ::cartographer::mapping::MapBuilderInterface::TrajectoryType;

void agvUpdateTrajectoryHandler::OnRequest(
    const proto::agv_UpdateTrajectoryRequest& request) 
  /*      
  if (GetContext<MapBuilderContextInterface>()
          ->map_builder()
          .GetWorkingTrajectoryType() != TrajectoryType::IDLE) 
    Finish(
        ::grpc::Status(::grpc::UNAVAILABLE, "current trajectory unavailable"));
    return;
   
  */
  //reset param;    
  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .agv_order_update_trajectory_.initial_pose =
      transform::ToRigid3(request.initial_pose());

  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .agv_order_update_trajectory_.trajectory_id = request.trajectory_id();

  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .agv_order_update_trajectory_.use_initial_pose =
      request.use_initial_pose();

  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .pose_graph()
      ->Setagv_Order(mapping::PoseGraphInterface::agvOrder::UPDATE);

  int count = 0;
  int wait_time = 100;
  auto response = absl::make_unique<proto::agv_UpdateTrajectoryResponse>();

  mapping::PoseGraphInterface::agvFeedback feedback;
  feedback.state = mapping::PoseGraphInterface::agvState::PROCESSING;

  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .pose_graph()
      ->Setagv_Feedback(feedback);

  for(;;)
  
      mapping::PoseGraphInterface::agvFeedback feedback =  GetContext<MapBuilderContextInterface>()
      ->map_builder()
      .pose_graph()->Getagv_Feedback();

      if(feedback.state == mapping::PoseGraphInterface::agvState::SUCCESS)
      
        LOG(INFO) << "update success!";
        response->set_code(0);
        response->set_state(true);
        break;
      
      else if(feedback.state == mapping::PoseGraphInterface::agvState::PROCESSING)
      
        LOG(INFO) << "Processing ...";
      
      else if(feedback.state == mapping::PoseGraphInterface::agvState::FAIL);
      
        LOG(INFO) << "update fail";
        response->set_code(feedback.code);
        response->set_message(feedback.message);
        response->set_state(false);
        break;
      
      
      absl::SleepFor(absl::Milliseconds(wait_time));
      count++;
      if(count*wait_time > 60*1000)
      
        LOG(INFO) << "Failed to save map because of time out";
        response->set_code(1);
        response->set_message("Failed to save map because of time out");
        response->set_state(false);
        break;
      
  

  Send(std::move(response));


  // namespace handlers
  // namespace cloud
  // namespace cartographer

第三步 在map_builder_server.cc中完成注册

  server_builder.RegisterHandler<handlers::agvUpdateTrajectoryHandler>();

OK，这样就简单实现了在cartographer中新增自己的rpc函数了。

如何使用呢？

第四步：根据自己的map_builder_service.proto生成自己的.grpc.pb.h .grpc.pb.cc
.pb.h .pb.cc 四个文件，然后就可以调用使用了
生成方法
可以参考这篇文章cartographer proto文件使用

然后就可以使用了

    bool UpdateTrajectory(std::string name) 
        // Data we are sending to the server.

        OKagv_UpdateTrajectoryRequest request;
        request.set_trajectory_id(name);
        request.set_use_initial_pose(true);
        request.mutable_initial_pose()->mutable_rotation()->set_w(1.0);
        request.mutable_initial_pose()->mutable_translation()->set_x(0.0);
        request.mutable_initial_pose()->mutable_translation()->set_y(0.0);

        // Container for the data we expect from the server.
        OKagv_UpdateTrajectoryResponse reply;

        // Context for the client. It could be used to convey extra information to
        // the server and/or tweak certain RPC behaviors.
        ClientContext context;

        // The actual RPC.
        //Status status = stub_->GetSubmap(&context, request, &reply);
        Status status = stub_->OKagv_UpdateTrajectory(&context, request, &reply);

        // Act upon its status.
        if (status.ok()) 
            return true;
         else 
            std::cout << RED << status.error_code() << ": " << status.error_message()
                      << std::endl;
            return false;