RSDK-3589 add wrapper for navigation service

src/viam/api/CMakeLists.txt

@@ -225,6 +225,10 @@ if (VIAMCPPSDK_USE_DYNAMIC_PROTOS)
     ${PROTO_GEN_DIR}/service/motion/v1/motion.grpc.pb.h
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.h
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.h
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.h
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.h
     ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc
@@ -328,6 +332,8 @@ target_sources(viamapi
     ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.grpc.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc
   PUBLIC FILE_SET viamapi_includes TYPE HEADERS
@@ -385,6 +391,8 @@ target_sources(viamapi
       ${PROTO_GEN_DIR}/../../viam/api/service/mlmodel/v1/mlmodel.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/service/motion/v1/motion.grpc.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/service/motion/v1/motion.pb.h
+      ${PROTO_GEN_DIR}/../../viam/api/service/navigation/v1/navigation.grpc.pb.h
+      ${PROTO_GEN_DIR}/../../viam/api/service/navigation/v1/navigation.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/tagger/v1/tagger.pb.h
 )
 

src/viam/sdk/CMakeLists.txt

@@ -119,13 +119,16 @@ target_sources(viamsdk
     services/generic.cpp
     services/mlmodel.cpp
     services/motion.cpp
+    services/navigation.cpp
     services/private/generic_client.cpp
     services/private/generic_server.cpp
     services/private/mlmodel.cpp
     services/private/mlmodel_client.cpp
     services/private/mlmodel_server.cpp
     services/private/motion_client.cpp
     services/private/motion_server.cpp
+    services/private/navigation_client.cpp
+    services/private/navigation_server.cpp
     services/service.cpp
     spatialmath/geometry.cpp
     spatialmath/orientation.cpp
@@ -179,6 +182,7 @@ target_sources(viamsdk
       ../../viam/sdk/services/generic.hpp
       ../../viam/sdk/services/mlmodel.hpp
       ../../viam/sdk/services/motion.hpp
+      ../../viam/sdk/services/navigation.hpp
       ../../viam/sdk/services/service.hpp
       ../../viam/sdk/spatialmath/geometry.hpp
       ../../viam/sdk/spatialmath/orientation.hpp

src/viam/sdk/common/private/proto_utils.hpp

@@ -0,0 +1,59 @@
+/// @file common/proto_utils.hpp
+///
+/// @brief Utils that require generated proto includes. These should be #included
+/// in cpp implementation files, but not in wrapper headers consumed by third party code.
+#pragma once
+
+#include <viam/api/common/v1/common.pb.h>
+
+namespace viam::sdk::impl {
+
+/// @brief Copies elements from a protobuf repeated pointer array into a std::vector. Src type
+/// must have a `to_proto` method.
+template <typename Src, typename Dst>
+void vecToRepeatedPtr(const std::vector<Src>& vec, google::protobuf::RepeatedPtrField<Dst>& dest) {
+    dest.Clear();
+    dest.Reserve(vec.size());
+    for (auto& x : vec) {
+        *dest.Add() = x.to_proto();
+    }
+}
+
+/// @brief Non-member to_proto() version. (necessary for moving generated types out of wrapper
+/// headers). Takes explicit `to_proto`.
+template <typename Src, typename Dst>
+void vecToRepeatedPtr(const std::vector<Src>& vec,
+                      google::protobuf::RepeatedPtrField<Dst>& dest,
+                      Dst to_proto(const Src&)) {
+    dest.Clear();
+    dest.Reserve(vec.size());
+    for (auto& x : vec) {
+        *dest.Add() = to_proto(x);
+    }
+}
+
+/// @brief Copies elements from a std::vector into a protobuf repeated pointer array. Dst type
+/// must have a `from_proto` static method.
+template <typename Src, typename Dst>
+void repeatedPtrToVec(const google::protobuf::RepeatedPtrField<Src>& src, std::vector<Dst>& vec) {
+    vec.clear();
+    vec.reserve(src.size());
+    for (auto& x : src) {
+        vec.push_back(Dst::from_proto(x));
+    }
+}
+
+/// @brief Non-member from_proto() version. (necessary for moving generated types out of wrapper
+/// headers). Takes explicit `from_proto`.
+template <typename Src, typename Dst>
+void repeatedPtrToVec(const google::protobuf::RepeatedPtrField<Src>& src,
+                      std::vector<Dst>& vec,
+                      Dst from_proto(const Src&)) {
+    vec.clear();
+    vec.reserve(src.size());
+    for (auto& x : src) {
+        vec.push_back(from_proto(x));
+    }
+}
+
+}  // namespace viam::sdk::impl

src/viam/sdk/registry/registry.cpp

@@ -48,6 +48,8 @@
 #include <viam/sdk/services/private/mlmodel_server.hpp>
 #include <viam/sdk/services/private/motion_client.hpp>
 #include <viam/sdk/services/private/motion_server.hpp>
+#include <viam/sdk/services/private/navigation_client.hpp>
+#include <viam/sdk/services/private/navigation_server.hpp>
 #include <viam/sdk/services/service.hpp>
 
 namespace viam {
@@ -190,6 +192,7 @@ void register_resources() {
     Registry::register_resource<impl::GenericServiceClient, impl::GenericServiceServer>();
     Registry::register_resource<impl::MLModelServiceClient, impl::MLModelServiceServer>();
     Registry::register_resource<impl::MotionClient, impl::MotionServer>();
+    Registry::register_resource<impl::NavigationClient, impl::NavigationServer>();
 }
 
 void Registry::initialize() {

src/viam/sdk/services/navigation.cpp

@@ -0,0 +1,21 @@
+#include <viam/sdk/services/navigation.hpp>
+
+#include <viam/api/service/navigation/v1/navigation.pb.h>
+#include <viam/sdk/common/private/proto_utils.hpp>
+#include <viam/sdk/common/utils.hpp>
+
+namespace viam {
+namespace sdk {
+
+Navigation::Navigation(std::string name) : Service(std::move(name)){};
+
+API Navigation::api() const {
+    return API::get<Navigation>();
+}
+
+API API::traits<Navigation>::api() {
+    return {kRDK, kService, "navigation"};
+}
+
+}  // namespace sdk
+}  // namespace viam

src/viam/sdk/services/navigation.hpp

@@ -0,0 +1,102 @@
+/// @file services/navigation.hpp
+///
+/// @brief Defines a `Navigation` service.
+#pragma once
+
+#include <string>
+
+#include <viam/sdk/common/pose.hpp>
+#include <viam/sdk/common/utils.hpp>
+#include <viam/sdk/services/service.hpp>
+#include <viam/sdk/spatialmath/geometry.hpp>
+
+namespace viam {
+namespace sdk {
+
+class Navigation : public Service {
+   public:
+    enum class Mode : uint8_t {
+        k_unspecified,
+        k_manual,
+        k_waypoint,
+        k_explore,
+    };
+
+    enum class MapType : uint8_t {
+        k_unspecified,
+        k_none,
+        k_gps,
+    };
+
+    struct LocationResponse {
+        geo_point location;
+        double compass_heading;
+    };
+
+    struct Waypoint {
+        std::string id;
+        geo_point location;
+    };
+
+    struct Path {
+        std::string destination_waypoint_id;
+        std::vector<geo_point> geopoints;
+    };
+
+    API api() const override;
+
+    virtual Mode get_mode(const std::string name, const ProtoStruct& extra) = 0;
+    virtual void set_mode(const std::string name, const Mode mode, const ProtoStruct& extra) = 0;
+    virtual LocationResponse get_location(const std::string name, const ProtoStruct& extra) = 0;
+    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 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<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;
+    virtual MapType get_properties(const std::string) = 0;
+    virtual ProtoStruct do_command(const ProtoStruct& command) = 0;
+
+    // overloads without `extra` param.
+    inline Mode get_mode(const std::string name) {
+        return get_mode(name, {});
+    }
+    inline void set_mode(const std::string name, const Mode mode) {
+        set_mode(name, mode, {});
+    }
+    inline LocationResponse get_location(const std::string name) {
+        return get_location(name, {});
+    }
+    inline std::unique_ptr<std::vector<Waypoint>> get_waypoints(const std::string name) {
+        return get_waypoints(name, {});
+    }
+    inline void add_waypoint(const std::string name, const geo_point& location) {
+        add_waypoint(name, location, {});
+    }
+    inline void remove_waypoint(const std::string name, const std::string id) {
+        remove_waypoint(name, id, {});
+    }
+    inline std::unique_ptr<std::vector<geo_geometry>> get_obstacles(const std::string name) {
+        return get_obstacles(name, {});
+    }
+    inline std::unique_ptr<std::vector<Path>> get_paths(const std::string name) {
+        return get_paths(name, {});
+    }
+
+   protected:
+    explicit Navigation(std::string name);
+};
+
+template <>
+struct API::traits<Navigation> {
+    static API api();
+};
+
+}  // namespace sdk
+}  // namespace viam