diff --git a/src/viam/api/CMakeLists.txt b/src/viam/api/CMakeLists.txt index 1b33312e7..6aab8b7d0 100644 --- a/src/viam/api/CMakeLists.txt +++ b/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 ) diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index 3ed83a7d5..fd38c196e 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -119,6 +119,7 @@ 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 @@ -126,6 +127,8 @@ target_sources(viamsdk 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 diff --git a/src/viam/sdk/common/private/proto_utils.hpp b/src/viam/sdk/common/private/proto_utils.hpp new file mode 100644 index 000000000..1263587f6 --- /dev/null +++ b/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 + +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 +void vecToRepeatedPtr(const std::vector& vec, google::protobuf::RepeatedPtrField& 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 +void vecToRepeatedPtr(const std::vector& vec, + google::protobuf::RepeatedPtrField& 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 +void repeatedPtrToVec(const google::protobuf::RepeatedPtrField& src, std::vector& 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 +void repeatedPtrToVec(const google::protobuf::RepeatedPtrField& src, + std::vector& 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 diff --git a/src/viam/sdk/registry/registry.cpp b/src/viam/sdk/registry/registry.cpp index 4e0819722..e0676af4d 100644 --- a/src/viam/sdk/registry/registry.cpp +++ b/src/viam/sdk/registry/registry.cpp @@ -48,6 +48,8 @@ #include #include #include +#include +#include #include namespace viam { @@ -190,6 +192,7 @@ void register_resources() { Registry::register_resource(); Registry::register_resource(); Registry::register_resource(); + Registry::register_resource(); } void Registry::initialize() { diff --git a/src/viam/sdk/services/motion.hpp b/src/viam/sdk/services/motion.hpp index 61bb718bb..9db70a98b 100644 --- a/src/viam/sdk/services/motion.hpp +++ b/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, diff --git a/src/viam/sdk/services/navigation.cpp b/src/viam/sdk/services/navigation.cpp new file mode 100644 index 000000000..b5d60e069 --- /dev/null +++ b/src/viam/sdk/services/navigation.cpp @@ -0,0 +1,21 @@ +#include + +#include +#include +#include + +namespace viam { +namespace sdk { + +Navigation::Navigation(std::string name) : Service(std::move(name)){}; + +API Navigation::api() const { + return API::get(); +} + +API API::traits::api() { + return {kRDK, kService, "navigation"}; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/navigation.hpp b/src/viam/sdk/services/navigation.hpp new file mode 100644 index 000000000..ae914b068 --- /dev/null +++ b/src/viam/sdk/services/navigation.hpp @@ -0,0 +1,171 @@ +/// @file services/navigation.hpp +/// +/// @brief Defines a `Navigation` service. +#pragma once + +#include + +#include +#include +#include +#include + +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 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 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 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 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 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 get_obstacles() { + return get_obstacles({}); + } + + inline std::vector get_paths() { + return get_paths({}); + } + + protected: + explicit Navigation(std::string name); +}; + +template <> +struct API::traits { + static API api(); +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/private/navigation_client.cpp b/src/viam/sdk/services/private/navigation_client.cpp new file mode 100644 index 000000000..7834d5009 --- /dev/null +++ b/src/viam/sdk/services/private/navigation_client.cpp @@ -0,0 +1,125 @@ +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace viam { +namespace sdk { +namespace impl { + +using namespace viam::service::navigation::v1; + +Navigation::Waypoint from_proto(const viam::service::navigation::v1::Waypoint& proto) { + return Navigation::Waypoint{proto.id(), geo_point::from_proto(proto.location())}; +} + +Navigation::Path from_proto(const viam::service::navigation::v1::Path& proto) { + Navigation::Path ret{proto.destination_waypoint_id()}; + repeatedPtrToVec(proto.geopoints(), ret.geopoints); + return ret; +} + +NavigationClient::NavigationClient(std::string name, std::shared_ptr 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() = map_to_struct(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() = map_to_struct(extra); + }) + .invoke([](auto& response) {}); +} + +Navigation::LocationResponse NavigationClient::get_location(const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::GetLocation) + .with([&](auto& request) { *request.mutable_extra() = map_to_struct(extra); }) + .invoke([](auto& response) { + return Navigation::LocationResponse{ + geo_point::from_proto(response.location()), + response.compass_heading(), + }; + }); +} + +std::vector NavigationClient::get_waypoints(const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::GetWaypoints) + .with([&](auto& request) { *request.mutable_extra() = map_to_struct(extra); }) + .invoke([](auto& response) { + std::vector ret; + repeatedPtrToVec(response.waypoints(), ret, from_proto); + return ret; + }); +} + +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() = location.to_proto(); + *request.mutable_extra() = map_to_struct(extra); + }) + .invoke([](auto& response) {}); +} + +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() = map_to_struct(extra); + }) + .invoke([](auto& response) {}); +} + +std::vector NavigationClient::get_obstacles(const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::GetObstacles) + .with([&](auto& request) { *request.mutable_extra() = map_to_struct(extra); }) + .invoke([](auto& response) { + std::vector ret; + repeatedPtrToVec(response.obstacles(), ret); + return ret; + }); +} + +std::vector NavigationClient::get_paths(const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::GetPaths) + .with([&](auto& request) { *request.mutable_extra() = map_to_struct(extra); }) + .invoke([](auto& response) { + std::vector ret; + repeatedPtrToVec(response.paths(), ret, from_proto); + return ret; + }); +} + +NavigationClient::Properties NavigationClient::get_properties() { + return make_client_helper(this, *stub_, &StubType::GetProperties) + .with([&](auto& request) {}) + .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() = map_to_struct(command); }) + .invoke([](auto& response) { return struct_to_map(response.result()); }); +} + +} // namespace impl +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/private/navigation_client.hpp b/src/viam/sdk/services/private/navigation_client.hpp new file mode 100644 index 000000000..8f32fb38f --- /dev/null +++ b/src/viam/sdk/services/private/navigation_client.hpp @@ -0,0 +1,43 @@ +/// @file services/navigation/client.hpp +/// +/// @brief Implements a gRPC client for the `Navigation` service. +#pragma once + +#include + +#include + +#include + +namespace viam { +namespace sdk { +namespace impl { + +/// @class NavigationClient +/// @brief gRPC client implementation of a `Navigation` service. +/// @ingroup Navigation +class NavigationClient : public Navigation { + public: + using interface_type = Navigation; + NavigationClient(std::string name, std::shared_ptr channel); + + Mode get_mode(const ProtoStruct& extra) override; + void set_mode(const Mode mode, const ProtoStruct& extra) override; + LocationResponse get_location(const ProtoStruct& extra) override; + std::vector get_waypoints(const ProtoStruct& extra) override; + void add_waypoint(const geo_point& location, const ProtoStruct& extra) override; + void remove_waypoint(const std::string id, const ProtoStruct& extra) override; + std::vector get_obstacles(const ProtoStruct& extra) override; + std::vector get_paths(const ProtoStruct& extra) override; + Properties get_properties() override; + ProtoStruct do_command(const ProtoStruct& command) override; + + private: + using StubType = service::navigation::v1::NavigationService::StubInterface; + std::unique_ptr stub_; + std::shared_ptr channel_; +}; + +} // namespace impl +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/private/navigation_server.cpp b/src/viam/sdk/services/private/navigation_server.cpp new file mode 100644 index 000000000..0bfbe7204 --- /dev/null +++ b/src/viam/sdk/services/private/navigation_server.cpp @@ -0,0 +1,132 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace viam { +namespace sdk { +namespace impl { + +using namespace service::navigation::v1; + +viam::service::navigation::v1::Waypoint to_proto(const Navigation::Waypoint& wp) { + viam::service::navigation::v1::Waypoint ret; + *ret.mutable_id() = wp.id; + *ret.mutable_location() = wp.location.to_proto(); + return ret; +} + +viam::service::navigation::v1::Path to_proto(const Navigation::Path& p) { + viam::service::navigation::v1::Path ret; + *ret.mutable_destination_waypoint_id() = p.destination_waypoint_id; + vecToRepeatedPtr(p.geopoints, *ret.mutable_geopoints()); + return ret; +} + +::grpc::Status NavigationServer::GetMode(::grpc::ServerContext*, + const GetModeRequest* request, + GetModeResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetMode", this, request)([&](auto& helper, auto& nav) { + response->set_mode(Mode(nav->get_mode(helper.getExtra()))); + }); +} + +::grpc::Status NavigationServer::SetMode(::grpc::ServerContext*, + const SetModeRequest* request, + SetModeResponse*) noexcept { + return make_service_helper( + "NavigationServer::SetMode", this, request)([&](auto& helper, auto& nav) { + nav->set_mode(Navigation::Mode(request->mode()), helper.getExtra()); + }); +} + +::grpc::Status NavigationServer::GetLocation(::grpc::ServerContext*, + const GetLocationRequest* request, + GetLocationResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetLocation", this, request)([&](auto& helper, auto& nav) { + const auto& loc = nav->get_location(helper.getExtra()); + *response->mutable_location() = loc.location.to_proto(); + response->set_compass_heading(loc.compass_heading); + }); +} + +::grpc::Status NavigationServer::GetWaypoints(::grpc::ServerContext*, + const GetWaypointsRequest* request, + GetWaypointsResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetWaypoints", this, request)([&](auto& helper, auto& nav) { + const auto waypoints = nav->get_waypoints(helper.getExtra()); + vecToRepeatedPtr(waypoints, *response->mutable_waypoints(), to_proto); + }); +} + +::grpc::Status NavigationServer::AddWaypoint(::grpc::ServerContext*, + const AddWaypointRequest* request, + AddWaypointResponse*) noexcept { + return make_service_helper( + "NavigationServer::AddWaypoint", this, request)([&](auto& helper, auto& nav) { + nav->add_waypoint(geo_point::from_proto(request->location()), helper.getExtra()); + }); +} + +::grpc::Status NavigationServer::RemoveWaypoint(::grpc::ServerContext*, + const RemoveWaypointRequest* request, + RemoveWaypointResponse*) noexcept { + return make_service_helper("NavigationServer::RemoveWaypoint", this, request)( + [&](auto& helper, auto& nav) { nav->remove_waypoint(request->id(), helper.getExtra()); }); +} + +::grpc::Status NavigationServer::GetObstacles(::grpc::ServerContext*, + const GetObstaclesRequest* request, + GetObstaclesResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetObstacles", this, request)([&](auto& helper, auto& nav) { + const auto obstacles = nav->get_obstacles(helper.getExtra()); + vecToRepeatedPtr(obstacles, *response->mutable_obstacles()); + }); +} + +::grpc::Status NavigationServer::GetPaths(::grpc::ServerContext*, + const GetPathsRequest* request, + GetPathsResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetPaths", this, request)([&](auto& helper, auto& nav) { + const auto paths = nav->get_paths(helper.getExtra()); + vecToRepeatedPtr(paths, *response->mutable_paths(), to_proto); + }); +} + +::grpc::Status NavigationServer::GetProperties(::grpc::ServerContext*, + const GetPropertiesRequest* request, + GetPropertiesResponse* response) noexcept { + return make_service_helper( + "NavigationServer::GetProperties", this, request)([&](auto&, auto& nav) { + const Navigation::Properties props = nav->get_properties(); + response->set_map_type(MapType(props.map_type)); + }); +} + +::grpc::Status NavigationServer::DoCommand( + ::grpc::ServerContext*, + const ::viam::common::v1::DoCommandRequest* request, + ::viam::common::v1::DoCommandResponse* response) noexcept { + return make_service_helper( + "NavigationServer::DoCommand", this, request)([&](auto&, auto& motion) { + const ProtoStruct result = motion->do_command(struct_to_map(request->command())); + *response->mutable_result() = map_to_struct(result); + }); +}; + +} // namespace impl +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/private/navigation_server.hpp b/src/viam/sdk/services/private/navigation_server.hpp new file mode 100644 index 000000000..208f1c930 --- /dev/null +++ b/src/viam/sdk/services/private/navigation_server.hpp @@ -0,0 +1,66 @@ +/// @file services/private/navigation_server.hpp +/// +/// @brief Implements a gRPC server for the `Navigation` service. +#pragma once + +#include + +#include + +#include +#include +#include +#include + +namespace viam { +namespace sdk { +namespace impl { + +using namespace service::navigation::v1; + +/// @class NavigationServer +/// @brief gRPC server implementation of a `Navigation` service. +/// @ingroup Navigation +class NavigationServer : public ResourceServer, + public viam::service::navigation::v1::NavigationService::Service { + public: + using interface_type = Navigation; + using service_type = service::navigation::v1::NavigationService; + explicit NavigationServer(std::shared_ptr manager) + : ResourceServer(std::move(manager)) {} + + ::grpc::Status GetMode(::grpc::ServerContext* context, + const GetModeRequest* request, + GetModeResponse* response) noexcept override; + ::grpc::Status SetMode(::grpc::ServerContext* context, + const SetModeRequest* request, + SetModeResponse* response) noexcept override; + ::grpc::Status GetLocation(::grpc::ServerContext* context, + const GetLocationRequest* request, + GetLocationResponse* response) noexcept override; + ::grpc::Status GetWaypoints(::grpc::ServerContext* context, + const GetWaypointsRequest* request, + GetWaypointsResponse* response) noexcept override; + ::grpc::Status AddWaypoint(::grpc::ServerContext* context, + const AddWaypointRequest* request, + AddWaypointResponse* response) noexcept override; + ::grpc::Status RemoveWaypoint(::grpc::ServerContext* context, + const RemoveWaypointRequest* request, + RemoveWaypointResponse* response) noexcept override; + ::grpc::Status GetObstacles(::grpc::ServerContext* context, + const GetObstaclesRequest* request, + GetObstaclesResponse* response) noexcept override; + ::grpc::Status GetPaths(::grpc::ServerContext* context, + const GetPathsRequest* request, + GetPathsResponse* response) noexcept override; + ::grpc::Status GetProperties(::grpc::ServerContext* context, + const GetPropertiesRequest* request, + GetPropertiesResponse* response) noexcept override; + ::grpc::Status DoCommand(::grpc::ServerContext* context, + const ::viam::common::v1::DoCommandRequest* request, + ::viam::common::v1::DoCommandResponse* response) noexcept override; +}; + +} // namespace impl +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index 92122150c..8c16e0fdc 100644 --- a/src/viam/sdk/tests/CMakeLists.txt +++ b/src/viam/sdk/tests/CMakeLists.txt @@ -30,6 +30,7 @@ target_sources(viamsdk_test mocks/mock_motor.cpp mocks/mock_motion.cpp mocks/mock_movement_sensor.cpp + mocks/mock_navigation.cpp mocks/mock_pose_tracker.cpp mocks/mock_power_sensor.cpp mocks/mock_sensor.cpp @@ -55,6 +56,7 @@ viamcppsdk_add_boost_test(test_mlmodel.cpp) viamcppsdk_add_boost_test(test_motor.cpp) viamcppsdk_add_boost_test(test_motion.cpp) viamcppsdk_add_boost_test(test_movement_sensor.cpp) +viamcppsdk_add_boost_test(test_navigation.cpp) viamcppsdk_add_boost_test(test_pose_tracker.cpp) viamcppsdk_add_boost_test(test_power_sensor.cpp) viamcppsdk_add_boost_test(test_proto_value.cpp) diff --git a/src/viam/sdk/tests/mocks/mock_navigation.cpp b/src/viam/sdk/tests/mocks/mock_navigation.cpp new file mode 100644 index 000000000..c0eb37c75 --- /dev/null +++ b/src/viam/sdk/tests/mocks/mock_navigation.cpp @@ -0,0 +1,67 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace viam { +namespace sdktests { +namespace navigation { + +using namespace viam::sdk; + +MockNav::MockNav(std::string name) : Navigation(name) {} + +Navigation::Mode MockNav::get_mode(const ProtoStruct& extra) { + return mode; +} + +void MockNav::set_mode(const Mode mode, const ProtoStruct& extra) { + this->mode = mode; +} + +Navigation::LocationResponse MockNav::get_location(const ProtoStruct& extra) { + return loc; +} + +std::vector MockNav::get_waypoints(const ProtoStruct& extra) { + return waypoints; +} + +void MockNav::add_waypoint(const geo_point& location, const ProtoStruct& extra) { + waypoints.push_back(Waypoint{std::to_string(next_waypoint_id++), location}); +} + +void MockNav::remove_waypoint(const std::string id, const ProtoStruct& extra) { + for (auto it = waypoints.begin(); it != waypoints.end(); ++it) { + if (it->id == id) { + waypoints.erase(it); + break; + } + } +} + +std::vector MockNav::get_obstacles(const ProtoStruct& extra) { + return obstacles; +} + +std::vector MockNav::get_paths(const ProtoStruct& extra) { + return paths; +} + +Navigation::Properties MockNav::get_properties() { + return Properties{map_type}; +} + +ProtoStruct MockNav::do_command(const ProtoStruct& command) { + return ProtoStruct{}; +} + +} // namespace navigation +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/mocks/mock_navigation.hpp b/src/viam/sdk/tests/mocks/mock_navigation.hpp new file mode 100644 index 000000000..be01201cf --- /dev/null +++ b/src/viam/sdk/tests/mocks/mock_navigation.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +namespace viam { +namespace sdktests { +namespace navigation { + +using namespace sdk; + +class MockNav : public sdk::Navigation { + public: + MockNav(std::string); + Mode get_mode(const ProtoStruct& extra) override; + void set_mode(const Mode mode, const ProtoStruct& extra) override; + LocationResponse get_location(const ProtoStruct& extra) override; + std::vector get_waypoints(const ProtoStruct& extra) override; + void add_waypoint(const geo_point& location, const ProtoStruct& extra) override; + void remove_waypoint(const std::string id, const ProtoStruct& extra) override; + std::vector get_obstacles(const ProtoStruct& extra) override; + std::vector get_paths(const ProtoStruct& extra) override; + Properties get_properties() override; + ProtoStruct do_command(const ProtoStruct& command) override; + + Mode mode; + LocationResponse loc = LocationResponse{}; + std::vector waypoints; + int next_waypoint_id = 0; + std::vector obstacles; + std::vector paths; + MapType map_type = MapType::k_unspecified; +}; + +} // namespace navigation +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/test_navigation.cpp b/src/viam/sdk/tests/test_navigation.cpp new file mode 100644 index 000000000..94c146b22 --- /dev/null +++ b/src/viam/sdk/tests/test_navigation.cpp @@ -0,0 +1,134 @@ +#define BOOST_TEST_MODULE test module test_navigation + +#include + +#include +#include +#include + +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Navigation::Mode) +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Navigation::Path) +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Navigation::LocationResponse) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Navigation::MapType) + +namespace viam { +namespace sdktests { +namespace navigation { + +using namespace viam::sdk; + +BOOST_AUTO_TEST_CASE(nav_mode) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + client.set_mode(Navigation::Mode::k_manual); + BOOST_CHECK_EQUAL(client.get_mode(), Navigation::Mode::k_manual); + client.set_mode(Navigation::Mode::k_explore); + BOOST_CHECK_EQUAL(client.get_mode(), Navigation::Mode::k_explore); + }); +} + +BOOST_AUTO_TEST_CASE(nav_get_location) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + auto loc = client.get_location(); + BOOST_CHECK_EQUAL(loc, Navigation::LocationResponse{}); + mock->loc.compass_heading = 1; + loc = client.get_location(); + BOOST_CHECK_CLOSE(loc.compass_heading, 1., 1e-4); + }); +} + +// weak backport of std::transform. +template +auto mapOver(const std::vector& source, Func f) { + using Result = decltype(f(source.front())); + std::vector ret; + for (const auto& x : source) { + ret.push_back(f(x)); + } + return ret; +} + +BOOST_AUTO_TEST_CASE(nav_waypoints) { + // get, add, remove + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + // confirm empty + auto waypoints = client.get_waypoints(); + BOOST_CHECK_EQUAL(waypoints.size(), 0); + + // add 3 and confirm size + for (int i = 0; i < 3; i++) { + client.add_waypoint(geo_point{0, double(i)}); + } + waypoints = client.get_waypoints(); + BOOST_CHECK_EQUAL(waypoints.size(), 3); + + // remove 1, check size and IDs + client.remove_waypoint(waypoints[1].id); + const std::vector expected_ids = {waypoints[0].id, waypoints[2].id}; + waypoints = client.get_waypoints(); + BOOST_CHECK_EQUAL(waypoints.size(), 2); + const auto actual_ids = + mapOver(waypoints, [](const Navigation::Waypoint& wp) { return wp.id; }); + BOOST_CHECK_EQUAL(expected_ids, actual_ids); + }); +} + +BOOST_AUTO_TEST_CASE(nav_obstacles) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + // empty case + auto obstacles = client.get_obstacles(); + BOOST_CHECK_EQUAL(obstacles.size(), 0); + + // one element + mock->obstacles.push_back({}); + obstacles = client.get_obstacles(); + BOOST_CHECK_EQUAL(obstacles.size(), 1); + + // one element with one sub-element + mock->obstacles.back().geometries.push_back({}); + obstacles = client.get_obstacles(); + BOOST_CHECK_EQUAL(obstacles.size(), 1); + BOOST_CHECK_EQUAL(obstacles.back().geometries.size(), 1); + }); +} + +BOOST_AUTO_TEST_CASE(nav_paths) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + // empty + auto paths = client.get_paths(); + BOOST_CHECK_EQUAL(paths.size(), 0); + + // 1 element with 1 sub-element + mock->paths.push_back({"2", {{}}}); + paths = client.get_paths(); + std::vector expected = {Navigation::Path{"2", {{}}}}; + BOOST_CHECK_EQUAL(paths, expected); + }); +} + +BOOST_AUTO_TEST_CASE(props) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + const auto props = client.get_properties(); + BOOST_CHECK_EQUAL(props.map_type, Navigation::MapType::k_unspecified); + }); +} + +BOOST_AUTO_TEST_CASE(do_command) { + auto mock = std::make_shared("mock_nav"); + client_to_mock_pipeline(mock, [&](Navigation& client) { + const auto ret = client.do_command({}); + BOOST_CHECK_EQUAL(ret.size(), 0); + }); +} + +} // namespace navigation +} // namespace sdktests +} // namespace viam