use builtin types instead of protos

Author: abe-winter

src/viam/sdk/services/navigation.hpp

@@ -9,12 +9,13 @@
 #include <viam/api/service/navigation/v1/navigation.pb.h>
 
 #include <viam/sdk/services/service.hpp>
+#include <viam/sdk/spatialmath/geometry.hpp>
+#include <viam/sdk/common/pose.hpp>
+#include <viam/sdk/common/utils.hpp>
 
 namespace viam {
 namespace sdk {
 
-using namespace viam::common::v1;
-
 class Navigation : public Service {
    public:
     enum class Mode : uint8_t {
@@ -31,47 +32,40 @@ class Navigation : public Service {
     };
 
     struct LocationResponse {
-       public:
-        GeoPoint location;
+        geo_point location;
         double compass_heading;
     };
 
     struct Waypoint {
-        Waypoint(const viam::service::navigation::v1::Waypoint& proto) {
-            this->id = proto.id();
-            this->location = proto.location();
-        }
+        Waypoint(const viam::service::navigation::v1::Waypoint& proto)
+            : id(proto.id()), location(geo_point::from_proto(proto.location())) {}
 
         operator viam::service::navigation::v1::Waypoint() {
             viam::service::navigation::v1::Waypoint ret;
             *ret.mutable_id() = id;
-            *ret.mutable_location() = location;
+            *ret.mutable_location() = location.to_proto();
             return ret;
         }
 
         std::string id;
-        GeoPoint location;
+        geo_point location;
     };
 
     struct Path {
-        Path(const viam::service::navigation::v1::Path& proto) {
-            this->destination_waypoint_id = proto.destination_waypoint_id();
-            this->geopoints =
-                std::vector<GeoPoint>(proto.geopoints().begin(), proto.geopoints().end());
-        }
+        Path(const viam::service::navigation::v1::Path& proto)
+            : destination_waypoint_id(proto.destination_waypoint_id()) {
+                repeatedPtrToVec(proto.geopoints(), geopoints);
+            }
 
         operator viam::service::navigation::v1::Path() {
             viam::service::navigation::v1::Path ret;
             *ret.mutable_destination_waypoint_id() = destination_waypoint_id;
-            auto dest_points = ret.mutable_geopoints();
-            for (const auto& x : geopoints) {
-                *dest_points->Add() = x;
-            }
+            vecToRepeatedPtr(geopoints, *ret.mutable_geopoints());
             return ret;
         }
 
         std::string destination_waypoint_id;
-        std::vector<GeoPoint> geopoints;
+        std::vector<geo_point> geopoints;
     };
 
     API api() const override;
@@ -82,12 +76,12 @@ class Navigation : public Service {
     virtual std::unique_ptr<std::vector<Waypoint>> get_waypoints(const std::string name,
                                                                  const ProtoStruct& extra) = 0;
     virtual void add_waypoint(const std::string name,
-                              const GeoPoint& location,
+                              const geo_point& location,
                               const ProtoStruct& extra) = 0;
     virtual void remove_waypoint(const std::string name,
                                  const std::string id,
                                  const ProtoStruct& extra) = 0;
-    virtual std::unique_ptr<std::vector<GeoGeometry>> get_obstacles(const std::string name,
+    virtual std::unique_ptr<std::vector<geo_geometry>> get_obstacles(const std::string name,
                                                                     const ProtoStruct& extra) = 0;
     virtual std::unique_ptr<std::vector<Path>> get_paths(const std::string name,
                                                          const ProtoStruct& extra) = 0;

src/viam/sdk/services/private/navigation_client.cpp

@@ -53,25 +53,12 @@ Navigation::LocationResponse NavigationClient::get_location(const std::string na
         })
         .invoke([](auto& response) {
             return Navigation::LocationResponse{
-                response.location(),
+                geo_point::from_proto(response.location()),
                 response.compass_heading(),
             };
         });
 }
 
-// helper; unpacks a repeated pointer to a vector of destination type.
-// Dst must have a constructor that takes Src*.
-template <typename Src, typename Dst>
-std::unique_ptr<std::vector<Dst>> repeatedPtrToVector(
-    const ::google::protobuf::RepeatedPtrField<Src>& items) {
-    auto vec = std::make_unique<std::vector<Dst>>();
-    vec->reserve(items.size());
-    for (auto& x : items) {
-        vec->push_back(x);
-    }
-    return vec;
-}
-
 std::unique_ptr<std::vector<Navigation::Waypoint>> NavigationClient::get_waypoints(
     const std::string name, const ProtoStruct& extra) {
     return make_client_helper(this, *stub_, &StubType::GetWaypoints)
@@ -80,18 +67,19 @@ std::unique_ptr<std::vector<Navigation::Waypoint>> NavigationClient::get_waypoin
             *request.mutable_extra() = map_to_struct(extra);
         })
         .invoke([](auto& response) {
-            return repeatedPtrToVector<viam::service::navigation::v1::Waypoint,
-                                       Navigation::Waypoint>(response.waypoints());
+            auto ret = std::make_unique<std::vector<Navigation::Waypoint>>();
+            repeatedPtrToVec(response.waypoints(), *ret);
+            return ret;
         });
 }
 
 void NavigationClient::add_waypoint(const std::string name,
-                                    const GeoPoint& location,
+                                    const geo_point& location,
                                     const ProtoStruct& extra) {
     return make_client_helper(this, *stub_, &StubType::AddWaypoint)
         .with([&](auto& request) {
             *request.mutable_name() = name;
-            *request.mutable_location() = location;
+            *request.mutable_location() = location.to_proto();
             *request.mutable_extra() = map_to_struct(extra);
         })
         .invoke([](auto& response) {});
@@ -109,16 +97,17 @@ void NavigationClient::remove_waypoint(const std::string name,
         .invoke([](auto& response) {});
 }
 
-std::unique_ptr<std::vector<GeoGeometry>> NavigationClient::get_obstacles(
+std::unique_ptr<std::vector<geo_geometry>> NavigationClient::get_obstacles(
     const std::string name, const ProtoStruct& extra) {
     return make_client_helper(this, *stub_, &StubType::GetObstacles)
         .with([&](auto& request) {
             *request.mutable_name() = name;
             *request.mutable_extra() = map_to_struct(extra);
         })
         .invoke([](auto& response) {
-            return std::make_unique<std::vector<GeoGeometry>>(response.obstacles().begin(),
-                                                              response.obstacles().end());
+            auto ret = std::make_unique<std::vector<geo_geometry>>();
+            repeatedPtrToVec(response.obstacles(), *ret);
+            return ret;
         });
 }
 

src/viam/sdk/services/private/navigation_client.hpp

@@ -27,12 +27,12 @@ class NavigationClient : public Navigation {
     std::unique_ptr<std::vector<Waypoint>> get_waypoints(const std::string name,
                                                          const ProtoStruct& extra) override;
     void add_waypoint(const std::string name,
-                      const GeoPoint& location,
+                      const geo_point& location,
                       const ProtoStruct& extra) override;
     void remove_waypoint(const std::string name,
                          const std::string id,
                          const ProtoStruct& extra) override;
-    std::unique_ptr<std::vector<GeoGeometry>> get_obstacles(const std::string name,
+    std::unique_ptr<std::vector<geo_geometry>> get_obstacles(const std::string name,
                                                             const ProtoStruct& extra) override;
     std::unique_ptr<std::vector<Path>> get_paths(const std::string name,
                                                  const ProtoStruct& extra) override;

src/viam/sdk/services/private/navigation_server.cpp

@@ -40,27 +40,18 @@ ::grpc::Status NavigationServer::GetLocation(::grpc::ServerContext*,
     return make_service_helper<Navigation>(
         "NavigationServer::GetLocation", this, request)([&](auto& helper, auto& nav) {
         const auto& loc = nav->get_location(request->name(), helper.getExtra());
-        *response->mutable_location() = loc.location;
+        *response->mutable_location() = loc.location.to_proto();
         response->set_compass_heading(loc.compass_heading);
     });
 }
 
-// helper; copies vec<Src> into RepeatedPtrField<Dst>.
-template <typename Src, typename Dst>
-void vecToRepeatedPtr(std::vector<Src>& vec, google::protobuf::RepeatedPtrField<Dst>* dest) {
-    dest->Reserve(vec.size());
-    for (auto& x : vec) {
-        *dest->Add() = x;
-    }
-}
-
 ::grpc::Status NavigationServer::GetWaypoints(::grpc::ServerContext*,
                                               const GetWaypointsRequest* request,
                                               GetWaypointsResponse* response) noexcept {
     return make_service_helper<Navigation>(
         "NavigationServer::GetWaypoints", this, request)([&](auto& helper, auto& nav) {
         const auto& waypoints = nav->get_waypoints(request->name(), helper.getExtra());
-        vecToRepeatedPtr(*waypoints, response->mutable_waypoints());
+        vecToRepeatedPtr(*waypoints, *response->mutable_waypoints());
     });
 }
 
@@ -69,7 +60,7 @@ ::grpc::Status NavigationServer::AddWaypoint(::grpc::ServerContext*,
                                              AddWaypointResponse*) noexcept {
     return make_service_helper<Navigation>(
         "NavigationServer::AddWaypoint", this, request)([&](auto& helper, auto& nav) {
-        nav->add_waypoint(request->name(), request->location(), helper.getExtra());
+        nav->add_waypoint(request->name(), geo_point::from_proto(request->location()), helper.getExtra());
     });
 }
 
@@ -88,7 +79,7 @@ ::grpc::Status NavigationServer::GetObstacles(::grpc::ServerContext*,
     return make_service_helper<Navigation>(
         "NavigationServer::GetObstacles", this, request)([&](auto& helper, auto& nav) {
         const auto& obstacles = nav->get_obstacles(request->name(), helper.getExtra());
-        vecToRepeatedPtr(*obstacles, response->mutable_obstacles());
+        vecToRepeatedPtr(*obstacles, *response->mutable_obstacles());
     });
 }
 
@@ -98,7 +89,7 @@ ::grpc::Status NavigationServer::GetPaths(::grpc::ServerContext*,
     return make_service_helper<Navigation>(
         "NavigationServer::GetPaths", this, request)([&](auto& helper, auto& nav) {
         const auto& paths = nav->get_paths(request->name(), helper.getExtra());
-        vecToRepeatedPtr(*paths, response->mutable_paths());
+        vecToRepeatedPtr(*paths, *response->mutable_paths());
     });
 }