viam-cpp-sdk/src/viam/sdk/services/private/navigation_client.cpp at 5cb8aad104e6b0d1845a78f5bb3d4792a16ab7fe · acmorrow/viam-cpp-sdk · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#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/private/repeated_ptr_convert.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 proto_convert_details {

template <>
struct from_proto_impl<service::navigation::v1::Path> {
    Navigation::Path operator()(const service::navigation::v1::Path* proto) const {
        return {proto->destination_waypoint_id(), impl::from_repeated_field(proto->geopoints())};
    }
};

template <>
struct from_proto_impl<service::navigation::v1::Waypoint> {
    Navigation::Waypoint operator()(const service::navigation::v1::Waypoint* proto) const {
        return {proto->id(), from_proto(proto->location())};
    }
};

}  // namespace proto_convert_details

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 ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::GetMode)
        .with([&](auto& request) { *request.mutable_extra() = to_proto(extra); })
        .invoke([](auto& response) { return Navigation::Mode(response.mode()); });
}

void NavigationClient::set_mode(const Navigation::Mode mode, const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::SetMode)
        .with([&](auto& request) {
            request.set_mode(viam::service::navigation::v1::Mode(mode));
            *request.mutable_extra() = to_proto(extra);
        })
        .invoke([](auto&) {});
}

Navigation::LocationResponse NavigationClient::get_location(const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::GetLocation)
        .with([&](auto& request) { *request.mutable_extra() = to_proto(extra); })
        .invoke([](auto& response) {
            return Navigation::LocationResponse{
                from_proto(response.location()),
                response.compass_heading(),
            };
        });
}

std::vector<Navigation::Waypoint> NavigationClient::get_waypoints(const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::GetWaypoints)
        .with([&](auto& request) { *request.mutable_extra() = to_proto(extra); })
        .invoke([](auto& response) { return impl::from_repeated_field(response.waypoints()); });
}

void NavigationClient::add_waypoint(const geo_point& location, const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::AddWaypoint)
        .with([&](auto& request) {
            *request.mutable_location() = to_proto(location);
            *request.mutable_extra() = to_proto(extra);
        })
        .invoke([](auto&) {});
}

void NavigationClient::remove_waypoint(const std::string id, const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::RemoveWaypoint)
        .with([&](auto& request) {
            *request.mutable_id() = id;
            *request.mutable_extra() = to_proto(extra);
        })
        .invoke([](auto&) {});
}

std::vector<geo_geometry> NavigationClient::get_obstacles(const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::GetObstacles)
        .with([&](auto& request) { *request.mutable_extra() = to_proto(extra); })
        .invoke([](auto& response) { return impl::from_repeated_field(response.obstacles()); });
}

std::vector<NavigationClient::Path> NavigationClient::get_paths(const ProtoStruct& extra) {
    return make_client_helper(this, *stub_, &StubType::GetPaths)
        .with([&](auto& request) { *request.mutable_extra() = to_proto(extra); })
        .invoke([](auto& response) { return impl::from_repeated_field(response.paths()); });
}

NavigationClient::Properties NavigationClient::get_properties() {
    return make_client_helper(this, *stub_, &StubType::GetProperties)
        .with([&](auto&) {})
        .invoke([](auto& response) { return Properties{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() = to_proto(command); })
        .invoke([](auto& response) { return from_proto(response.result()); });
}

}  // namespace impl
}  // namespace sdk
}  // namespace viam