-
Notifications
You must be signed in to change notification settings - Fork 22
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
RSDK-3589 add wrapper for navigation service (#323)
- Loading branch information
1 parent
3cd492c
commit 8aed0c9
Showing
15 changed files
with
876 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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::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::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::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::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::vector<geo_geometry> get_obstacles() { | ||
| return get_obstacles({}); | ||
| } | ||
|
|
||
| inline 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 |
Oops, something went wrong.