RSDK-3589 add wrapper for navigation service #323
There are no files selected for viewing
This file contains hidden or 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 hidden or 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 hidden or 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 hidden or 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 hidden or 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 hidden or 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,19 @@ | ||
| #include <viam/sdk/services/navigation.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 hidden or 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,101 @@ | ||
| /// @file services/navigation.hpp | ||
| /// | ||
| /// @brief Defines a `Navigation` service. | ||
| #pragma once | ||
|
|
||
| #include <string> | ||
|
|
||
| #include <viam/api/common/v1/common.pb.h> | ||
| #include <viam/api/service/navigation/v1/navigation.pb.h> | ||
|
|
||
| #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 { | ||
| 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.to_proto(); | ||
| return ret; | ||
| } | ||
|
|
||
| std::string id; | ||
| geo_point location; | ||
| }; | ||
|
|
||
| struct Path { | ||
| 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; | ||
| vecToRepeatedPtr(geopoints, *ret.mutable_geopoints()); | ||
| return ret; | ||
| } | ||
|
|
||
| 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; | ||
abe-winter marked this conversation as resolved.
Outdated
Show resolved
Hide resolved
|
||
| 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; | ||
|
|
||
| protected: | ||
| explicit Navigation(std::string name); | ||
| }; | ||
|
|
||
| template <> | ||
| struct API::traits<Navigation> { | ||
| static API api(); | ||
| }; | ||
|
|
||
| } // namespace sdk | ||
| } // namespace viam | ||
This file contains hidden or 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,141 @@ | ||
| #include <viam/sdk/services/private/navigation_client.hpp> | ||
|
|
||
| #include <math.h> | ||
|
|
||
| #include <grpcpp/support/status.h> | ||
|
|
||
| #include <viam/api/service/navigation/v1/navigation.grpc.pb.h> | ||
| #include <viam/api/service/navigation/v1/navigation.pb.h> | ||
|
|
||
| #include <viam/sdk/common/client_helper.hpp> | ||
| #include <viam/sdk/common/proto_value.hpp> | ||
| #include <viam/sdk/common/utils.hpp> | ||
| #include <viam/sdk/services/navigation.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| namespace impl { | ||
|
|
||
| using namespace viam::service::navigation::v1; | ||
|
|
||
| NavigationClient::NavigationClient(std::string name, std::shared_ptr<grpc::Channel> channel) | ||
| : Navigation(std::move(name)), | ||
| stub_(service::navigation::v1::NavigationService::NewStub(channel)), | ||
| channel_(std::move(channel)){}; | ||
|
|
||
| Navigation::Mode NavigationClient::get_mode(const std::string name, const ProtoStruct& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::GetMode) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) { return Navigation::Mode(response.mode()); }); | ||
| } | ||
|
|
||
| void NavigationClient::set_mode(const std::string name, | ||
| const Navigation::Mode mode, | ||
| const ProtoStruct& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::SetMode) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| request.set_mode(viam::service::navigation::v1::Mode(mode)); | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) {}); | ||
| } | ||
|
|
||
| Navigation::LocationResponse NavigationClient::get_location(const std::string name, | ||
| const ProtoStruct& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::GetLocation) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) { | ||
| return Navigation::LocationResponse{ | ||
| geo_point::from_proto(response.location()), | ||
| response.compass_heading(), | ||
| }; | ||
| }); | ||
| } | ||
|
|
||
| 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) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) { | ||
| auto ret = std::make_unique<std::vector<Navigation::Waypoint>>(); | ||
| repeatedPtrToVec(response.waypoints(), *ret); | ||
| return ret; | ||
| }); | ||
| } | ||
|
|
||
| void NavigationClient::add_waypoint(const std::string name, | ||
| 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.to_proto(); | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) {}); | ||
| } | ||
|
|
||
| void NavigationClient::remove_waypoint(const std::string name, | ||
| const std::string id, | ||
| const ProtoStruct& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::RemoveWaypoint) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| *request.mutable_id() = id; | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) {}); | ||
| } | ||
|
|
||
| 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) { | ||
| auto ret = std::make_unique<std::vector<geo_geometry>>(); | ||
| repeatedPtrToVec(response.obstacles(), *ret); | ||
| return ret; | ||
| }); | ||
| } | ||
|
|
||
| std::unique_ptr<std::vector<NavigationClient::Path>> NavigationClient::get_paths( | ||
| const std::string name, const ProtoStruct& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::GetPaths) | ||
| .with([&](auto& request) { | ||
| *request.mutable_name() = name; | ||
| *request.mutable_extra() = map_to_struct(extra); | ||
| }) | ||
| .invoke([](auto& response) { | ||
| return std::make_unique<std::vector<Path>>(response.paths().begin(), | ||
| response.paths().end()); | ||
| }); | ||
| } | ||
|
|
||
| NavigationClient::MapType NavigationClient::get_properties(const std::string name) { | ||
| return make_client_helper(this, *stub_, &StubType::GetProperties) | ||
| .with([&](auto& request) { *request.mutable_name() = name; }) | ||
| .invoke([](auto& response) { return MapType(response.map_type()); }); | ||
| } | ||
|
|
||
| ProtoStruct NavigationClient::do_command(const ProtoStruct& command) { | ||
| return make_client_helper(this, *stub_, &StubType::DoCommand) | ||
| .with([&](auto& request) { *request.mutable_command() = map_to_struct(command); }) | ||
| .invoke([](auto& response) { return struct_to_map(response.result()); }); | ||
| } | ||
|
|
||
| } // namespace impl | ||
| } // namespace sdk | ||
| } // namespace viam |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Not that we've announced this super loudly but we're currently trying to cut back on API and
google::types in headers, so I think we'll want to delete these helper functions even though they're being used a couple times.As a suggested inline replacement, I think this could just be
You can also do that with the
Addmethod, but note that ifdestalready had stuff then this function would be under-reserving potentially and adding to existing elements which is maybe not what I'd expect from anxToYfunctionAs a suggested replacement for this one, can you just write inline
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
good call -- clearing the destination in d334572 to protect against adding to non-empty collection
I think
Assign(begin, end)doesn't work because the types are different -- tried to get it working with constructor/operator implicit conversion approach, couldn't, guessing even harder now that the constructor/operators have been moved out of common headers