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,63 @@
+/// @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 {
+namespace sdk {
+namespace 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 impl
+}  // namespace sdk
+}  // namespace viam

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/motion.hpp

@@ -247,7 +247,7 @@ class Motion : public Service {
     /// @param world_state Obstacles to avoid and transforms to add to the robot for the duration of
     /// the move.
     /// @param constraints Constraints to apply to how the robot will move.
-    /// @extra Any additional arguments to the method.
+    /// @param extra Any additional arguments to the method.
     /// @return Whether or not the move was successful.
     virtual bool move(const pose_in_frame& destination,
                       const Name& name,

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,171 @@
+/// @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 Mode
+    /// @brief Enum affecting this nav service's goal.
+    /// @ingroup Navigation
+    enum class Mode : uint8_t {
+        k_unspecified,
+        k_manual,
+        k_waypoint,
+        k_explore,
+    };
+
+    /// @enum MapType
+    /// @brief Is the map navigating in GPS or a custom map.
+    /// @ingroup Navigation
+    enum class MapType : uint8_t {
+        k_unspecified,
+        k_none,
+        k_gps,
+    };
+
+    /// @struct LocationResponse
+    /// @brief Location and direction.
+    /// @ingroup Navigation
+    struct LocationResponse {
+        geo_point location;
+        double compass_heading;
+
+        bool operator==(const LocationResponse& rhs) const {
+            return compass_heading == rhs.compass_heading && location == rhs.location;
+        }
+    };
+
+    /// @struct Properties
+    /// @brief A set of attributes for this nav service.
+    /// @ingroup Navigation
+    struct Properties {
+        MapType map_type;
+    };
+
+    /// @struct Waypoint
+    /// @brief A location with an `id` handle that can be used to uniquely identify and remove it.
+    /// @ingroup Navigation
+    struct Waypoint {
+        std::string id;
+        geo_point location;
+    };
+
+    /// @struct Path
+    /// @brief A user-provided destination and a set of geopoints to get there.
+    /// @ingroup Navigation
+    struct Path {
+        std::string destination_waypoint_id;
+        std::vector<geo_point> geopoints;
+
+        bool operator==(const Path& rhs) const {
+            return destination_waypoint_id == rhs.destination_waypoint_id &&
+                   geopoints == rhs.geopoints;
+        }
+    };
+
+    API api() const override;
+
+    /// @brief Get the current mode.
+    /// @param extra Any additional arguments to the method.
+    /// @return Current mode.
+    virtual Mode get_mode(const ProtoStruct& extra) = 0;
+
+    /// @brief Set the current mode.
+    /// @param mode Desired mode.
+    /// @param extra Any additional arguments to the method.
+    virtual void set_mode(const Mode mode, const ProtoStruct& extra) = 0;
+
+    /// @brief Get the current location.
+    /// @param extra Any additional arguments to the method.
+    /// @return Current location.
+    virtual LocationResponse get_location(const ProtoStruct& extra) = 0;
+
+    /// @brief Get the waypoints this nav service knows about.
+    /// @param extra Any additional arguments to the method.
+    /// @return List of waypoints.
+    virtual std::unique_ptr<std::vector<Waypoint>> get_waypoints(const ProtoStruct& extra) = 0;
+
+    /// @brief Add a waypoint.
+    /// @param location Coordinate of the new waypoint.
+    virtual void add_waypoint(const geo_point& location, const ProtoStruct& extra) = 0;
+
+    /// @brief Remove a waypoint by ID.
+    /// @param id The string ID of the waypoint to remove.
+    /// @param extra Any additional arguments to the method.
+    virtual void remove_waypoint(const std::string id, const ProtoStruct& extra) = 0;
+
+    /// @brief Get the obstacles this nav service knows about.
+    /// @param extra Any additional arguments to the method.
+    /// @return List of shapes.
+    virtual std::unique_ptr<std::vector<geo_geometry>> get_obstacles(const ProtoStruct& extra) = 0;
+
+    /// @brief Get the paths this nav service knows about.
+    /// @param extra Any additional arguments to the method.
+    /// @return List of paths.
+    virtual std::unique_ptr<std::vector<Path>> get_paths(const ProtoStruct& extra) = 0;
+
+    /// @brief Get this nav service's properties.
+    /// @return Properties.
+    virtual Properties get_properties() = 0;
+
+    /// @brief Do an arbitrary command.
+    /// @param command Freeform fields that are service-specific.
+    /// @return Freeform result of the command.
+    virtual ProtoStruct do_command(const ProtoStruct& command) = 0;
+
+    // overloads without `extra` param:
+
+    inline Mode get_mode() {
+        return get_mode({});
+    }
+
+    inline void set_mode(const Mode mode) {
+        set_mode(mode, {});
+    }
+
+    inline LocationResponse get_location() {
+        return get_location({});
+    }
+
+    inline std::unique_ptr<std::vector<Waypoint>> get_waypoints() {
+        return get_waypoints({});
+    }
+
+    inline void add_waypoint(const geo_point& location) {
+        add_waypoint(location, {});
+    }
+
+    inline void remove_waypoint(const std::string id) {
+        remove_waypoint(id, {});
+    }
+
+    inline std::unique_ptr<std::vector<geo_geometry>> get_obstacles() {
+        return get_obstacles({});
+    }
+
+    inline std::unique_ptr<std::vector<Path>> get_paths() {
+        return get_paths({});
+    }
+
+   protected:
+    explicit Navigation(std::string name);
+};
+
+template <>
+struct API::traits<Navigation> {
+    static API api();
+};
+
+}  // namespace sdk
+}  // namespace viam