From e92e44916a7cf0c789a0c980509ec3fe13fa52ac Mon Sep 17 00:00:00 2001 From: Ethan Date: Fri, 18 Aug 2023 14:25:49 -0400 Subject: [PATCH] RSDK-3531 - Motion service wrappers (#141) --- codesamples/apis.json | 6 + src/viam/api/CMakeLists.txt | 8 + src/viam/sdk/CMakeLists.txt | 10 + src/viam/sdk/common/pose.cpp | 48 +++++ src/viam/sdk/common/pose.hpp | 44 ++++ src/viam/sdk/common/proto_type.cpp | 4 + src/viam/sdk/common/world_state.cpp | 98 +++++++++ src/viam/sdk/common/world_state.hpp | 48 +++++ src/viam/sdk/resource/resource_api.cpp | 25 ++- src/viam/sdk/resource/resource_api.hpp | 10 +- src/viam/sdk/robot/client.cpp | 3 + src/viam/sdk/services/motion/client.cpp | 150 +++++++++++++ src/viam/sdk/services/motion/client.hpp | 51 +++++ src/viam/sdk/services/motion/motion.cpp | 227 ++++++++++++++++++++ src/viam/sdk/services/motion/motion.hpp | 182 ++++++++++++++++ src/viam/sdk/services/motion/server.cpp | 199 ++++++++++++++++++ src/viam/sdk/services/motion/server.hpp | 46 ++++ src/viam/sdk/spatialmath/geometry.cpp | 255 ++++++++++++++++++----- src/viam/sdk/spatialmath/geometry.hpp | 57 ++++- src/viam/sdk/tests/CMakeLists.txt | 2 + src/viam/sdk/tests/mocks/mock_motion.cpp | 117 +++++++++++ src/viam/sdk/tests/mocks/mock_motion.hpp | 74 +++++++ src/viam/sdk/tests/test_motion.cpp | 207 ++++++++++++++++++ src/viam/sdk/tests/test_utils.cpp | 25 ++- 24 files changed, 1820 insertions(+), 76 deletions(-) create mode 100644 src/viam/sdk/common/pose.cpp create mode 100644 src/viam/sdk/common/pose.hpp create mode 100644 src/viam/sdk/common/world_state.cpp create mode 100644 src/viam/sdk/common/world_state.hpp create mode 100644 src/viam/sdk/services/motion/client.cpp create mode 100644 src/viam/sdk/services/motion/client.hpp create mode 100644 src/viam/sdk/services/motion/motion.cpp create mode 100644 src/viam/sdk/services/motion/motion.hpp create mode 100644 src/viam/sdk/services/motion/server.cpp create mode 100644 src/viam/sdk/services/motion/server.hpp create mode 100644 src/viam/sdk/tests/mocks/mock_motion.cpp create mode 100644 src/viam/sdk/tests/mocks/mock_motion.hpp create mode 100644 src/viam/sdk/tests/test_motion.cpp diff --git a/codesamples/apis.json b/codesamples/apis.json index 33dbc9c84..b496a511b 100644 --- a/codesamples/apis.json +++ b/codesamples/apis.json @@ -15,6 +15,12 @@ "func": "get_properties", "args": [] }, + "motion": { + "client": "MotionClient", + "func": "get_pose", + "args": ["{{\"rdk\", \"component\", \"fake\"}, \"\", \"component\"}", "", "{}", "nullptr"] + "comment": "Update with correct component name, destination frame, supplemental transforms, and extra params" + }, "motor": { "client": "MotorClient", "func": "is_moving", diff --git a/src/viam/api/CMakeLists.txt b/src/viam/api/CMakeLists.txt index d398384d9..98ad42e73 100644 --- a/src/viam/api/CMakeLists.txt +++ b/src/viam/api/CMakeLists.txt @@ -160,6 +160,10 @@ if (VIAMCPPSDK_USE_DYNAMIC_PROTOS) ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.grpc.pb.h ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.pb.cc ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.pb.h + ${PROTO_GEN_DIR}/service/motion/v1/motion.grpc.pb.cc + ${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}/tagger/v1/tagger.grpc.pb.cc ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.h ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc @@ -234,6 +238,8 @@ target_sources(viamapi ${PROTO_GEN_DIR}/robot/v1/robot.pb.cc ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.grpc.pb.cc ${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}/tagger/v1/tagger.grpc.pb.cc ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc PUBLIC FILE_SET viamapi_includes TYPE HEADERS @@ -268,6 +274,8 @@ target_sources(viamapi ${PROTO_GEN_DIR}/../../viam/api/tagger/v1/tagger.grpc.pb.h ${PROTO_GEN_DIR}/../../viam/api/service/mlmodel/v1/mlmodel.grpc.pb.h ${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/tagger/v1/tagger.pb.h ) diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index b46770d83..98db10d08 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -44,8 +44,10 @@ target_sources(viamsdk # be initialized within main before anything else happens? registry/registry.cpp common/linear_algebra.cpp + common/pose.cpp common/proto_type.cpp common/utils.cpp + common/world_state.cpp components/base/base.cpp components/base/client.cpp components/base/server.cpp @@ -83,6 +85,9 @@ target_sources(viamsdk services/mlmodel/mlmodel.cpp services/mlmodel/private/proto.cpp services/mlmodel/server.cpp + services/motion/client.cpp + services/motion/motion.cpp + services/motion/server.cpp services/service.cpp spatialmath/geometry.cpp spatialmath/orientation_types.cpp @@ -92,8 +97,10 @@ target_sources(viamsdk ../.. FILES ../../viam/sdk/common/linear_algebra.hpp + ../../viam/sdk/common/pose.hpp ../../viam/sdk/common/proto_type.hpp ../../viam/sdk/common/utils.hpp + ../../viam/sdk/common/world_state.hpp ../../viam/sdk/components/base/base.hpp ../../viam/sdk/components/base/client.hpp ../../viam/sdk/components/base/server.hpp @@ -131,6 +138,9 @@ target_sources(viamsdk ../../viam/sdk/services/mlmodel/client.hpp ../../viam/sdk/services/mlmodel/mlmodel.hpp ../../viam/sdk/services/mlmodel/server.hpp + ../../viam/sdk/services/motion/client.hpp + ../../viam/sdk/services/motion/motion.hpp + ../../viam/sdk/services/motion/server.hpp ../../viam/sdk/services/service.hpp ../../viam/sdk/spatialmath/geometry.hpp ../../viam/sdk/spatialmath/orientation_types.hpp diff --git a/src/viam/sdk/common/pose.cpp b/src/viam/sdk/common/pose.cpp new file mode 100644 index 000000000..521d3127e --- /dev/null +++ b/src/viam/sdk/common/pose.cpp @@ -0,0 +1,48 @@ +#include + +#include + +namespace viam { +namespace sdk { + +common::v1::PoseInFrame pose_in_frame::to_proto() const { + common::v1::PoseInFrame pif; + *pif.mutable_reference_frame() = reference_frame; + common::v1::Pose proto_pose; + proto_pose.set_x(pose.coordinates.x); + proto_pose.set_y(pose.coordinates.y); + proto_pose.set_z(pose.coordinates.z); + proto_pose.set_o_x(pose.orientation.o_x); + proto_pose.set_o_y(pose.orientation.o_y); + proto_pose.set_o_z(pose.orientation.o_z); + proto_pose.set_theta(pose.theta); + *pif.mutable_pose() = std::move(proto_pose); + return pif; +}; + +pose_in_frame pose_in_frame::from_proto(const common::v1::PoseInFrame& proto) { + pose_in_frame pif; + pif.reference_frame = proto.reference_frame(); + const auto& proto_pose = proto.pose(); + pif.pose.orientation.o_x = proto_pose.o_x(); + pif.pose.orientation.o_y = proto_pose.o_y(); + pif.pose.orientation.o_z = proto_pose.o_z(); + pif.pose.coordinates.x = proto_pose.x(); + pif.pose.coordinates.y = proto_pose.y(); + pif.pose.coordinates.z = proto_pose.z(); + pif.pose.theta = proto_pose.theta(); + + return pif; +} + +bool operator==(const pose_in_frame& lhs, const pose_in_frame& rhs) { + return lhs.pose == rhs.pose && lhs.reference_frame == rhs.reference_frame; +} +std::ostream& operator<<(std::ostream& os, const pose_in_frame& v) { + os << "{ pose: " << v.pose << ",\n" + << " reference_frame: " << v.reference_frame << "}"; + return os; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/common/pose.hpp b/src/viam/sdk/common/pose.hpp new file mode 100644 index 000000000..b97353dc8 --- /dev/null +++ b/src/viam/sdk/common/pose.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +namespace viam { +namespace sdk { + +struct coordinates { + double x, y, z; + friend bool operator==(const coordinates& lhs, const coordinates& rhs); +}; + +struct pose_orientation { + double o_x, o_y, o_z; + friend bool operator==(const pose_orientation& lhs, const pose_orientation& rhs); +}; + +struct pose { + struct coordinates coordinates; + pose_orientation orientation; + double theta; + + static pose from_proto(const viam::common::v1::Pose& proto); + viam::common::v1::Pose to_proto() const; + + friend bool operator==(const pose& lhs, const pose& rhs); + friend std::ostream& operator<<(std::ostream& os, const pose& v); +}; + +struct pose_in_frame { + viam::common::v1::PoseInFrame to_proto() const; + static pose_in_frame from_proto(const viam::common::v1::PoseInFrame& proto); + pose_in_frame(std::string reference_frame_, struct pose pose_) + : reference_frame(std::move(reference_frame_)), pose(std::move(pose_)) {} + pose_in_frame() {} + + std::string reference_frame; + struct pose pose; + friend bool operator==(const pose_in_frame& lhs, const pose_in_frame& rhs); + friend std::ostream& operator<<(std::ostream& os, const pose_in_frame& v); +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/common/proto_type.cpp b/src/viam/sdk/common/proto_type.cpp index 1fa6f0340..01a6fc879 100644 --- a/src/viam/sdk/common/proto_type.cpp +++ b/src/viam/sdk/common/proto_type.cpp @@ -21,6 +21,10 @@ using google::protobuf::Value; // NOLINTNEXTLINE(misc-no-recursion) Struct map_to_struct(AttributeMap dict) { Struct s; + if (!dict) { + return s; + } + for (const auto& key_and_value : *dict) { const std::string key = key_and_value.first; const Value value = key_and_value.second->proto_value(); diff --git a/src/viam/sdk/common/world_state.cpp b/src/viam/sdk/common/world_state.cpp new file mode 100644 index 000000000..bd1fa5ca6 --- /dev/null +++ b/src/viam/sdk/common/world_state.cpp @@ -0,0 +1,98 @@ +#include + +#include + +#include + +namespace viam { +namespace sdk { + +WorldState::geometries_in_frame WorldState::geometries_in_frame::from_proto( + const common::v1::GeometriesInFrame& proto) { + geometries_in_frame gif; + for (const auto& geo : proto.geometries()) { + gif.geometries.push_back(GeometryConfig::from_proto(geo)); + } + gif.reference_frame = proto.reference_frame(); + + return gif; +} + +common::v1::GeometriesInFrame WorldState::geometries_in_frame::to_proto() const { + common::v1::GeometriesInFrame proto; + + *proto.mutable_reference_frame() = reference_frame; + for (const auto& geometry : geometries) { + *proto.mutable_geometries()->Add() = geometry.to_proto(); + } + + return proto; +} + +WorldState WorldState::from_proto(const common::v1::WorldState& ws) { + const auto& proto_obstacles = ws.obstacles(); + std::vector obstacles; + for (const auto& po : proto_obstacles) { + obstacles.push_back(geometries_in_frame::from_proto(po)); + } + + const auto& proto_transforms = ws.transforms(); + std::vector transforms; + for (const auto& pt : proto_transforms) { + transforms.push_back(WorldState::transform::from_proto(pt)); + } + + return {obstacles, transforms}; +} + +common::v1::WorldState WorldState::to_proto() const { + common::v1::WorldState proto_ws; + for (const auto& obstacle : obstacles_) { + *proto_ws.mutable_obstacles()->Add() = obstacle.to_proto(); + } + for (const auto& transform : transforms_) { + *proto_ws.mutable_transforms()->Add() = transform.to_proto(); + } + + return proto_ws; +} + +WorldState::transform WorldState::transform::from_proto(const common::v1::Transform& proto) { + WorldState::transform transform; + transform.reference_frame = proto.reference_frame(); + transform.pose_in_observer_frame = pose_in_frame::from_proto(proto.pose_in_observer_frame()); + if (proto.has_physical_object()) { + transform.physical_object = + std::make_shared(GeometryConfig::from_proto(proto.physical_object())); + } + + return transform; +} + +common::v1::Transform WorldState::transform::to_proto() const { + common::v1::Transform proto; + *proto.mutable_reference_frame() = reference_frame; + *proto.mutable_pose_in_observer_frame() = pose_in_observer_frame.to_proto(); + if (physical_object) { + *proto.mutable_physical_object() = physical_object->to_proto(); + } + + return proto; +} +bool operator==(const WorldState::geometries_in_frame& lhs, + const WorldState::geometries_in_frame& rhs) { + return lhs.reference_frame == rhs.reference_frame && lhs.geometries == rhs.geometries; +} +bool operator==(const WorldState::transform& lhs, const WorldState::transform& rhs) { + return lhs.reference_frame == rhs.reference_frame && + (lhs.physical_object == rhs.physical_object || + *lhs.physical_object == *rhs.physical_object) && + lhs.pose_in_observer_frame == rhs.pose_in_observer_frame; +} + +bool operator==(const WorldState& lhs, const WorldState& rhs) { + return lhs.obstacles_ == rhs.obstacles_ && lhs.transforms_ == rhs.transforms_; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/common/world_state.hpp b/src/viam/sdk/common/world_state.hpp new file mode 100644 index 000000000..72f229537 --- /dev/null +++ b/src/viam/sdk/common/world_state.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include +#include + +namespace viam { +namespace sdk { + +// TODO(RSDK-4553) Add documentation for these types +class WorldState { + public: + struct geometries_in_frame { + std::vector geometries; + std::string reference_frame; + common::v1::GeometriesInFrame to_proto() const; + static geometries_in_frame from_proto(const common::v1::GeometriesInFrame& proto); + }; + + struct transform { + std::string reference_frame; + pose_in_frame pose_in_observer_frame; + std::shared_ptr physical_object; + + common::v1::Transform to_proto() const; + static transform from_proto(const common::v1::Transform& proto); + transform() {} + }; + + common::v1::WorldState to_proto() const; + static WorldState from_proto(const common::v1::WorldState& ws); + + WorldState() {} + WorldState(std::vector obstacles, std::vector transforms) + : obstacles_(obstacles), transforms_(transforms) {} + + friend bool operator==(const WorldState& lhs, const WorldState& rhs); + friend bool operator==(const geometries_in_frame& lhs, const geometries_in_frame& rhs); + friend bool operator==(const transform& lhs, const transform& rhs); + + private: + std::vector obstacles_; + std::vector transforms_; +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/resource/resource_api.cpp b/src/viam/sdk/resource/resource_api.cpp index a348ea94b..908ec2a25 100644 --- a/src/viam/sdk/resource/resource_api.cpp +++ b/src/viam/sdk/resource/resource_api.cpp @@ -1,8 +1,9 @@ -#include #include +#include #include #include +#include #include #include @@ -40,11 +41,11 @@ const std::string& APIType::type_namespace() const { return namespace_; } -void APIType::set_namespace(const std::string type_namespace) { +void APIType::set_namespace(const std::string& type_namespace) { this->namespace_ = type_namespace; } -void APIType::set_resource_type(const std::string resource_type) { +void APIType::set_resource_type(const std::string& resource_type) { this->resource_type_ = resource_type; } @@ -56,7 +57,7 @@ const std::string& API::resource_subtype() const { return resource_subtype_; } -void API::set_resource_subtype(const std::string subtype) { +void API::set_resource_subtype(const std::string& subtype) { this->resource_subtype_ = subtype; } @@ -118,6 +119,17 @@ viam::common::v1::ResourceName Name::to_proto() const { return rn; } +Name Name::from_proto(const viam::common::v1::ResourceName& proto) { + const API api(proto.namespace_(), proto.type(), proto.subtype()); + std::vector name_parts; + boost::split(name_parts, proto.name(), boost::is_any_of(":")); + auto name = name_parts.back(); + name_parts.pop_back(); + auto remote_name = std::accumulate(name_parts.begin(), name_parts.end(), std::string(":")); + + return Name({proto.namespace_(), proto.type(), proto.subtype()}, remote_name, name); +}; + Name Name::from_string(std::string name) { if (!std::regex_match(name, NAME_REGEX)) { throw "Received invalid Name string: " + name; @@ -154,6 +166,11 @@ bool operator==(const Name& lhs, const Name& rhs) { return lhs.to_string() == rhs.to_string(); } +std::ostream& operator<<(std::ostream& os, const Name& v) { + os << v.to_string(); + return os; +} + bool operator==(const RPCSubtype& lhs, const RPCSubtype& rhs) { return lhs.api_.to_string() == rhs.api_.to_string() && lhs.proto_service_name_ == rhs.proto_service_name_ && diff --git a/src/viam/sdk/resource/resource_api.hpp b/src/viam/sdk/resource/resource_api.hpp index 06d9322c4..d0802832c 100644 --- a/src/viam/sdk/resource/resource_api.hpp +++ b/src/viam/sdk/resource/resource_api.hpp @@ -20,8 +20,8 @@ class APIType { const std::string& type_namespace() const; const std::string& resource_type() const; - void set_namespace(const std::string type_namespace); - void set_resource_type(const std::string resource_type); + void set_namespace(const std::string& type_namespace); + void set_resource_type(const std::string& resource_type); private: std::string namespace_; @@ -39,7 +39,7 @@ class API : public APIType { static API from_string(std::string api); const std::string& resource_subtype() const; - void set_resource_subtype(const std::string subtype); + void set_resource_subtype(const std::string& subtype); bool is_component_type(); bool is_service_type(); friend bool operator==(API const& lhs, API const& rhs); @@ -56,13 +56,15 @@ class Name { std::string short_name() const; std::string to_string() const; viam::common::v1::ResourceName to_proto() const; + static Name from_proto(const viam::common::v1::ResourceName& proto); static Name from_string(std::string name); Name(API api, std::string remote_name, std::string name); - Name(); + Name(){}; const API& api() const; const std::string& name() const; const std::string& remote_name() const; friend bool operator==(const Name& lhs, const Name& rhs); + friend std::ostream& operator<<(std::ostream& os, const Name& v); private: API api_; diff --git a/src/viam/sdk/robot/client.cpp b/src/viam/sdk/robot/client.cpp index 7e2800def..b45a8ef5d 100644 --- a/src/viam/sdk/robot/client.cpp +++ b/src/viam/sdk/robot/client.cpp @@ -37,6 +37,8 @@ namespace viam { namespace sdk { +// TODO(RSDK-4573) Having all these proto types exposed here in the APIs is sad. Let's fix that. + using google::protobuf::RepeatedPtrField; using grpc::ClientContext; using viam::common::v1::PoseInFrame; @@ -298,6 +300,7 @@ std::vector RobotClient::get_frame_system_config( return fs_configs; } + PoseInFrame RobotClient::transform_pose(PoseInFrame query, std::string destination, std::vector additional_transforms) { diff --git a/src/viam/sdk/services/motion/client.cpp b/src/viam/sdk/services/motion/client.cpp new file mode 100644 index 000000000..9cf20c7f2 --- /dev/null +++ b/src/viam/sdk/services/motion/client.cpp @@ -0,0 +1,150 @@ +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace viam { +namespace sdk { + +MotionClient::MotionClient(std::string name, std::shared_ptr channel) + : Motion(std::move(name)), + stub_(service::motion::v1::MotionService::NewStub(channel)), + channel_(std::move(channel)){}; + +bool MotionClient::move(const pose_in_frame& destination, + const Name& component_name, + std::shared_ptr world_state, + std::shared_ptr constraints, + const AttributeMap& extra) { + service::motion::v1::MoveRequest request; + service::motion::v1::MoveResponse response; + grpc::ClientContext ctx; + + *request.mutable_name() = this->name(); + *request.mutable_component_name() = component_name.to_proto(); + *request.mutable_destination() = destination.to_proto(); + *request.mutable_extra() = map_to_struct(extra); + if (constraints) { + *request.mutable_constraints() = constraints->to_proto(); + } + if (world_state) { + *request.mutable_world_state() = world_state->to_proto(); + } + + const grpc::Status status = stub_->Move(&ctx, request, &response); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + + return response.success(); +} + +bool MotionClient::move_on_map(const pose& destination, + const Name& component_name, + const Name& slam_name, + const AttributeMap& extra) { + service::motion::v1::MoveOnMapRequest request; + service::motion::v1::MoveOnMapResponse response; + grpc::ClientContext ctx; + + *request.mutable_name() = this->name(); + *request.mutable_destination() = destination.to_proto(); + *request.mutable_component_name() = component_name.to_proto(); + *request.mutable_slam_service_name() = slam_name.to_proto(); + *request.mutable_extra() = map_to_struct(extra); + + const grpc::Status status = stub_->MoveOnMap(&ctx, request, &response); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + + return response.success(); +} + +bool MotionClient::move_on_globe(const geo_point& destination, + const boost::optional& heading, + const Name& component_name, + const Name& movement_sensor_name, + const std::vector& obstacles, + std::shared_ptr motion_configuration, + const AttributeMap& extra) { + service::motion::v1::MoveOnGlobeRequest request; + service::motion::v1::MoveOnGlobeResponse response; + grpc::ClientContext ctx; + + *request.mutable_name() = this->name(); + *request.mutable_destination() = destination.to_proto(); + *request.mutable_component_name() = component_name.to_proto(); + *request.mutable_movement_sensor_name() = movement_sensor_name.to_proto(); + + if (heading && !isnan(*heading)) { + request.set_heading(*heading); + } + + for (const auto& obstacle : obstacles) { + *request.mutable_obstacles()->Add() = obstacle.to_proto(); + } + + if (motion_configuration) { + *request.mutable_motion_configuration() = motion_configuration->to_proto(); + } + + *request.mutable_extra() = map_to_struct(extra); + + const grpc::Status status = stub_->MoveOnGlobe(&ctx, request, &response); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + + return response.success(); +} + +pose_in_frame MotionClient::get_pose( + const Name& component_name, + const std::string& destination_frame, + const std::vector& supplemental_transforms, + AttributeMap extra) { + service::motion::v1::GetPoseRequest request; + service::motion::v1::GetPoseResponse response; + grpc::ClientContext ctx; + + *request.mutable_name() = this->name(); + *request.mutable_component_name() = component_name.to_proto(); + *request.mutable_destination_frame() = std::move(destination_frame); + for (const auto& transform : supplemental_transforms) { + *request.mutable_supplemental_transforms()->Add() = transform.to_proto(); + } + *request.mutable_extra() = map_to_struct(extra); + + const grpc::Status status = stub_->GetPose(&ctx, request, &response); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + + return pose_in_frame::from_proto(response.pose()); +} + +AttributeMap MotionClient::do_command(const AttributeMap& command) { + viam::common::v1::DoCommandRequest request; + viam::common::v1::DoCommandResponse response; + grpc::ClientContext ctx; + + *request.mutable_command() = map_to_struct(command); + *request.mutable_name() = this->name(); + + const grpc::Status status = stub_->DoCommand(&ctx, request, &response); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + return struct_to_map(response.result()); +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/motion/client.hpp b/src/viam/sdk/services/motion/client.hpp new file mode 100644 index 000000000..ca8fa9d3b --- /dev/null +++ b/src/viam/sdk/services/motion/client.hpp @@ -0,0 +1,51 @@ +/// @file services/motion/client.hpp +/// +/// @brief Implements a gRPC client for the `Motion` service. +#pragma once + +#include + +#include + +namespace viam { +namespace sdk { + +/// @class MotionClient +/// @brief gRPC client implementation of a `Motion` service. +/// @ingroup Motion +class MotionClient : public Motion { + public: + MotionClient(std::string name, std::shared_ptr channel); + bool move(const pose_in_frame& destination, + const Name& component_name, + std::shared_ptr world_state, + std::shared_ptr constraints, + const AttributeMap& extra) override; + + bool move_on_map(const pose& destination, + const Name& component_name, + const Name& slam_name, + const AttributeMap& extra) override; + + bool move_on_globe(const geo_point& destination, + const boost::optional& heading, + const Name& component_name, + const Name& movement_sensor_name, + const std::vector& obstacles, + std::shared_ptr motion_configuration, + const AttributeMap& extra) override; + + pose_in_frame get_pose(const Name& component_name, + const std::string& destination_frame, + const std::vector& supplemental_transforms, + AttributeMap extra) override; + + AttributeMap do_command(const AttributeMap& command) override; + + private: + std::unique_ptr stub_; + std::shared_ptr channel_; +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/motion/motion.cpp b/src/viam/sdk/services/motion/motion.cpp new file mode 100644 index 000000000..2303d1078 --- /dev/null +++ b/src/viam/sdk/services/motion/motion.cpp @@ -0,0 +1,227 @@ +#include + +#include + +#include +#include +#include + +#include +#include +#include + +namespace viam { +namespace sdk { + +MotionRegistration::MotionRegistration( + const google::protobuf::ServiceDescriptor* service_descriptor) + : ResourceRegistration(service_descriptor){}; + +std::shared_ptr MotionRegistration::create_resource_server( + std::shared_ptr manager) { + return std::make_shared(manager); +}; + +std::shared_ptr MotionRegistration::create_rpc_client( + std::string name, std::shared_ptr chan) { + return std::make_shared(std::move(name), std::move(chan)); +}; + +Motion::Motion(std::string name) : Service(std::move(name)){}; + +service::motion::v1::Constraints Motion::constraints::to_proto() const { + service::motion::v1::Constraints proto; + for (const auto& lc : linear_constraints) { + service::motion::v1::LinearConstraint proto_lc; + proto_lc.set_line_tolerance_mm(lc.line_tolerance_mm); + proto_lc.set_orientation_tolerance_degs(lc.orientation_tolerance_degs); + *proto.mutable_linear_constraint()->Add() = std::move(proto_lc); + } + + for (const auto& oc : orientation_constraints) { + service::motion::v1::OrientationConstraint proto_oc; + proto_oc.set_orientation_tolerance_degs(oc.orientation_tolerance_degs); + *proto.mutable_orientation_constraint()->Add() = std::move(proto_oc); + } + + for (const auto& cs : collision_specifications) { + service::motion::v1::CollisionSpecification proto_cs; + for (const auto& allow : cs.allows) { + service::motion::v1::CollisionSpecification::AllowedFrameCollisions proto_allow; + *proto_allow.mutable_frame1() = allow.frame1; + *proto_allow.mutable_frame2() = allow.frame2; + *proto_cs.mutable_allows()->Add() = std::move(proto_allow); + } + *proto.mutable_collision_specification()->Add() = std::move(proto_cs); + } + + return proto; +} + +Motion::constraints Motion::constraints::from_proto(const service::motion::v1::Constraints& proto) { + std::vector lcs; + for (const auto& proto_lc : proto.linear_constraint()) { + Motion::linear_constraint lc; + lc.orientation_tolerance_degs = proto_lc.orientation_tolerance_degs(); + lc.line_tolerance_mm = proto_lc.line_tolerance_mm(); + lcs.push_back(lc); + } + + std::vector ocs; + for (const auto& proto_oc : proto.orientation_constraint()) { + Motion::orientation_constraint oc; + oc.orientation_tolerance_degs = proto_oc.orientation_tolerance_degs(); + ocs.push_back(oc); + } + + std::vector css; + for (const auto& proto_cs : proto.collision_specification()) { + std::vector allows; + for (const auto& proto_allow : proto_cs.allows()) { + Motion::collision_specification::allowed_frame_collisions allow; + allow.frame1 = proto_allow.frame1(); + allow.frame2 = proto_allow.frame2(); + allows.push_back(allow); + } + Motion::collision_specification cs; + cs.allows = allows; + css.push_back(cs); + } + + Motion::constraints constraints; + constraints.linear_constraints = lcs; + constraints.orientation_constraints = ocs; + constraints.collision_specifications = css; + + return constraints; +} + +API Motion::static_api() { + return {kRDK, kService, "motion"}; +} + +API Motion::dynamic_api() const { + return static_api(); +} + +std::shared_ptr Motion::resource_registration() { + const google::protobuf::DescriptorPool* p = google::protobuf::DescriptorPool::generated_pool(); + const google::protobuf::ServiceDescriptor* sd = + p->FindServiceByName(service::motion::v1::MotionService::service_full_name()); + if (!sd) { + throw std::runtime_error("Unable to get service descriptor for the motion service"); + } + return std::make_shared(sd); +} + +service::motion::v1::MotionConfiguration motion_configuration::to_proto() const { + service::motion::v1::MotionConfiguration proto; + + for (const auto& name : vision_services) { + *proto.mutable_vision_services()->Add() = name.to_proto(); + } + + if (position_polling_frequency_hz && !isnan(*position_polling_frequency_hz)) { + proto.set_position_polling_frequency_hz(*position_polling_frequency_hz); + } + + if (obstacle_polling_frequency_hz && !isnan(*obstacle_polling_frequency_hz)) { + proto.set_obstacle_polling_frequency_hz(*obstacle_polling_frequency_hz); + } + + if (plan_deviation_m && !isnan(*plan_deviation_m)) { + proto.set_plan_deviation_m(*plan_deviation_m); + } + + if (linear_m_per_sec && !isnan(*linear_m_per_sec)) { + proto.set_linear_m_per_sec(*linear_m_per_sec); + } + + if (angular_degs_per_sec && !isnan(*angular_degs_per_sec)) { + proto.set_angular_degs_per_sec(*angular_degs_per_sec); + } + + return proto; +} + +motion_configuration motion_configuration::from_proto( + const service::motion::v1::MotionConfiguration& proto) { + motion_configuration mc; + + for (const auto& proto_name : proto.vision_services()) { + mc.vision_services.push_back(Name::from_proto(proto_name)); + } + + if (proto.has_position_polling_frequency_hz()) { + *mc.position_polling_frequency_hz = proto.position_polling_frequency_hz(); + } + + if (proto.has_obstacle_polling_frequency_hz()) { + *mc.obstacle_polling_frequency_hz = proto.obstacle_polling_frequency_hz(); + } + + if (proto.has_plan_deviation_m()) { + *mc.plan_deviation_m = proto.plan_deviation_m(); + } + + if (proto.has_linear_m_per_sec()) { + *mc.linear_m_per_sec = proto.linear_m_per_sec(); + } + + if (proto.has_angular_degs_per_sec()) { + *mc.angular_degs_per_sec = proto.angular_degs_per_sec(); + } + + return mc; +} + +bool operator==(const motion_configuration& lhs, const motion_configuration& rhs) { + return lhs.angular_degs_per_sec == rhs.angular_degs_per_sec && + lhs.vision_services == rhs.vision_services && + lhs.linear_m_per_sec == rhs.linear_m_per_sec && + lhs.plan_deviation_m == rhs.plan_deviation_m && + lhs.obstacle_polling_frequency_hz == rhs.obstacle_polling_frequency_hz && + lhs.position_polling_frequency_hz == rhs.position_polling_frequency_hz; +} + +std::ostream& operator<<(std::ostream& os, const motion_configuration& v) { + os << "{ "; + if (!v.vision_services.empty()) { + os << "\tvision_services: [\n"; + for (const auto& name : v.vision_services) { + os << "\t\t" << name.to_string() << ",\n"; + } + os << "\t],\n"; + } + if (v.angular_degs_per_sec.has_value()) { + os << "\tangular_degs_per_sec: " << v.angular_degs_per_sec.get() << ",\n"; + } + if (v.linear_m_per_sec.has_value()) { + os << "\tlinear_m_per_sec: " << v.linear_m_per_sec.get() << ",\n"; + } + if (v.obstacle_polling_frequency_hz.has_value()) { + os << "\tobstacle_polling_frequency_hz: " << v.obstacle_polling_frequency_hz.get() << ",\n"; + } + if (v.plan_deviation_m.has_value()) { + os << "\tplan_deviation_m: " << v.plan_deviation_m.get() << ",\n"; + } + if (v.position_polling_frequency_hz.has_value()) { + os << "\tposition_polling_frequency_hz: " << v.position_polling_frequency_hz.get() << ",\n"; + } + os << "}"; + + return os; +} + +namespace { +bool init() { + Registry::register_resource(Motion::static_api(), Motion::resource_registration()); + return true; +} + +// NOLINTNEXTLINE +const bool inited = init(); +} // namespace + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/motion/motion.hpp b/src/viam/sdk/services/motion/motion.hpp new file mode 100644 index 000000000..35d2df5bc --- /dev/null +++ b/src/viam/sdk/services/motion/motion.hpp @@ -0,0 +1,182 @@ +/// @file services/motion/motion.hpp +/// +/// @brief Defines a `Motion` service. +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace viam { +namespace sdk { + +/// @defgroup Motion Classes related to the Motion service. + +/// @class MotionRegistration +/// @brief Defines a `ResourceRegistration` for the `Motion` service. +/// @ingroup Motion +class MotionRegistration : public ResourceRegistration { + public: + explicit MotionRegistration(const google::protobuf::ServiceDescriptor* service_descriptor); + std::shared_ptr create_resource_server( + std::shared_ptr manager) override; + std::shared_ptr create_rpc_client(std::string name, + std::shared_ptr chan) override; +}; + +/// @struct motion_configuration +/// @brief Defines configuration options for certain `Motion` APIs. +/// @ingroup Motion +struct motion_configuration { + /// @brief The name of the vision service(s) that will be used for obstacle detection. + std::vector vision_services; + + /// @brief If not null, sets the frequency to poll for the position of the robot. + boost::optional position_polling_frequency_hz; + + /// @brief If not null, sets the frequency to poll the vision service(s) for new obstacles. + boost::optional obstacle_polling_frequency_hz; + + /// @brief Optional distance in meters a robot is allowed to deviate from the motion plan. + boost::optional plan_deviation_m; + + /// @brief Optional linear velocity to target when moving + boost::optional linear_m_per_sec; + + /// @brief Optional angular velocity to target when turning + boost::optional angular_degs_per_sec; + + service::motion::v1::MotionConfiguration to_proto() const; + static motion_configuration from_proto(const service::motion::v1::MotionConfiguration& proto); + friend bool operator==(const motion_configuration& lhs, const motion_configuration& rhs); + friend std::ostream& operator<<(std::ostream& os, const motion_configuration& v); +}; + +/// @class Motion motion.hpp "services/motion/motion.hpp" +/// @brief The `Motion` service coordinates motion planning across all components of a given robot. +/// @ingroup Motion +/// The Viam Motion planning service calculates a valid path that avoids self-collision by default. +/// If additional constraints are supplied in the `world_state` message, the motion planning +/// service will account for those as well. +/// +/// This acts as an abstract base class to be inherited from by any drivers representing +/// specific motion implementations. This class cannot be used on its own. +class Motion : public Service { + public: + /// @struct linear_constraint + /// @brief Specifies that the component being moved should move linearly to its goal. + struct linear_constraint { + float line_tolerance_mm; + float orientation_tolerance_degs; + }; + + /// @struct orientation_constraint + /// @brief Specifies that the component being moved will not deviate its orientation beyond the + /// specified threshold. + struct orientation_constraint { + float orientation_tolerance_degs; + }; + + /// @struct collision_specification + /// @brief used to selectively apply obstacle avoidance to specific parts of the robot. + struct collision_specification { + struct allowed_frame_collisions { + std::string frame1; + std::string frame2; + }; + std::vector allows; + }; + + /// @struct constraints + /// @brief Specifies all constraints to be passed to Viam's motion planning, along + /// with any optional parameters. + struct constraints { + std::vector linear_constraints; + std::vector orientation_constraints; + std::vector collision_specifications; + + static constraints from_proto(const service::motion::v1::Constraints& proto); + service::motion::v1::Constraints to_proto() const; + }; + + static API static_api(); + API dynamic_api() const override; + + /// @brief Creates a `ResourceRegistration` for the `Motion` service. + static std::shared_ptr resource_registration(); + + /// @brief Moves any compononent on the robot to a specified destination. + /// @param destination Where to move the component to. + /// @param name Name of the component to be moved. + /// @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. + /// @return Whether or not the move was successful. + virtual bool move(const pose_in_frame& destination, + const Name& name, + std::shared_ptr world_state, + std::shared_ptr constraints, + const AttributeMap& extra) = 0; + + /// @brief Moves any component on the robot to a specific destination on a SLAM map. + /// @param destination The destination to move to. + /// @param component_name The component to move. + /// @param slam_name The name of the slam service from which the SLAM map is requested. + /// @param extra Any additional arguments to the method. + /// @return Whether or not the move was successful. + virtual bool move_on_map(const pose& destination, + const Name& component_name, + const Name& slam_name, + const AttributeMap& extra) = 0; + + /// @brief Moves any component on the robot to a specific destination on a globe. + /// @param destination The destination to move to. + /// @param heading Optional compass heading to achieve at the destination in degrees [0-360). + /// @param component_name The name of the component to move. + /// @param movement_sensor_name The name of the movement sensor used to check robot location. + /// @param obstacles Obstacles to be considered for motion planning. + /// @param motion_configuration Optional set of motion configuration options. + /// @param extra Any additional arguments to the method. + /// @return Whether or not the move was successful. + virtual bool move_on_globe(const geo_point& destination, + const boost::optional& heading, + const Name& component_name, + const Name& movement_sensor_name, + const std::vector& obstacles, + std::shared_ptr motion_configuration, + const AttributeMap& extra) = 0; + + /// @brief Get the pose of any component on the robot. + /// @param component_name The component whose pose is being requested. + /// @param destination_frame The reference frame in which the component's pose should be + /// provided. + /// @param supplemental_transforms Pose information on any additional reference frames that are + /// needed to compute the component's pose. + /// @param extra Any additional arguments to the method. + /// @return The pose of the component. + virtual pose_in_frame get_pose( + const Name& component_name, + const std::string& destination_frame, + const std::vector& supplemental_transforms, + AttributeMap extra) = 0; + + /// @brief Send/receive arbitrary commands to the resource. + /// @param Command the command to execute. + /// @return The result of the executed command. + virtual AttributeMap do_command(const AttributeMap& command) = 0; + + protected: + explicit Motion(std::string name); +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/motion/server.cpp b/src/viam/sdk/services/motion/server.cpp new file mode 100644 index 000000000..47ad6a0ef --- /dev/null +++ b/src/viam/sdk/services/motion/server.cpp @@ -0,0 +1,199 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace viam { +namespace sdk { + +MotionServer::MotionServer() : ResourceServer(std::make_shared()){}; +MotionServer::MotionServer(std::shared_ptr manager) : ResourceServer(manager){}; + +::grpc::Status MotionServer::Move(::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveRequest* request, + ::viam::service::motion::v1::MoveResponse* response) { + if (!request) { + return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT, + "Called [Move] without a request"); + }; + + const std::shared_ptr rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr motion = std::dynamic_pointer_cast(rb); + + const pose_in_frame destination = pose_in_frame::from_proto(request->destination()); + const Name name = Name::from_proto(request->component_name()); + std::shared_ptr ws; + if (request->has_world_state()) { + ws = std::make_shared(WorldState::from_proto(request->world_state())); + } + + std::shared_ptr constraints; + if (request->has_constraints()) { + constraints = std::make_shared( + Motion::constraints::from_proto(request->constraints())); + } + + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + const bool success = motion->move(std::move(destination), + std::move(name), + std::move(ws), + std::move(constraints), + std::move(extra)); + response->set_success(success); + + return ::grpc::Status(); +}; + +::grpc::Status MotionServer::MoveOnMap(::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveOnMapRequest* request, + ::viam::service::motion::v1::MoveOnMapResponse* response) { + if (!request) { + return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT, + "Called [MoveOnMap] without a request"); + }; + + const std::shared_ptr rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr motion = std::dynamic_pointer_cast(rb); + + const auto& destination = pose::from_proto(request->destination()); + const auto& component_name = Name::from_proto(request->component_name()); + const auto& slam_name = Name::from_proto(request->slam_service_name()); + + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + const bool success = motion->move_on_map(destination, component_name, slam_name, extra); + + response->set_success(success); + + return ::grpc::Status(); +}; + +::grpc::Status MotionServer::MoveOnGlobe( + ::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveOnGlobeRequest* request, + ::viam::service::motion::v1::MoveOnGlobeResponse* response) { + if (!request) { + return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT, + "Called [MoveOnGlobe] without a request"); + }; + + const std::shared_ptr rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr motion = std::dynamic_pointer_cast(rb); + + const auto& destination = geo_point::from_proto(request->destination()); + const auto& component_name = Name::from_proto(request->component_name()); + const auto& movement_sensor_name = Name::from_proto(request->movement_sensor_name()); + std::vector obstacles; + + for (const auto& obstacle : request->obstacles()) { + obstacles.push_back(geo_obstacle::from_proto(obstacle)); + } + + boost::optional heading; + if (request->has_heading()) { + heading = request->heading(); + } + + std::shared_ptr mc; + if (request->has_motion_configuration()) { + mc = std::make_shared( + motion_configuration::from_proto(request->motion_configuration())); + } + + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + + const bool success = motion->move_on_globe( + destination, heading, component_name, movement_sensor_name, obstacles, mc, extra); + + response->set_success(success); + + return ::grpc::Status(); +}; + +::grpc::Status MotionServer::GetPose(::grpc::ServerContext* context, + const ::viam::service::motion::v1::GetPoseRequest* request, + ::viam::service::motion::v1::GetPoseResponse* response) { + if (!request) { + return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT, + "Called [GetPose] without a request"); + }; + + const std::shared_ptr rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr motion = std::dynamic_pointer_cast(rb); + + const auto& component_name = Name::from_proto(request->component_name()); + const std::string& destination_frame = request->destination_frame(); + std::vector supplemental_transforms; + for (const auto& proto_transform : request->supplemental_transforms()) { + supplemental_transforms.push_back(WorldState::transform::from_proto(proto_transform)); + } + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + + const pose_in_frame pose = + motion->get_pose(component_name, destination_frame, supplemental_transforms, extra); + + *response->mutable_pose() = pose.to_proto(); + + return ::grpc::Status(); +}; + +::grpc::Status MotionServer::DoCommand(::grpc::ServerContext* context, + const ::viam::common::v1::DoCommandRequest* request, + ::viam::common::v1::DoCommandResponse* response) { + if (!request) { + return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT, + "Called [DoCommand] without a request"); + }; + + const std::shared_ptr rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr motion = std::dynamic_pointer_cast(rb); + const AttributeMap result = motion->do_command(struct_to_map(request->command())); + + *response->mutable_result() = map_to_struct(result); + + return ::grpc::Status(); +}; + +void MotionServer::register_server(std::shared_ptr server) { + server->register_service(this); +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/services/motion/server.hpp b/src/viam/sdk/services/motion/server.hpp new file mode 100644 index 000000000..fd4a10698 --- /dev/null +++ b/src/viam/sdk/services/motion/server.hpp @@ -0,0 +1,46 @@ +/// @file services/motion/server.hpp +/// +/// @brief Implements a gRPC server for the `Motion` service. +#pragma once + +#include + +#include + +#include +#include +#include + +namespace viam { +namespace sdk { + +/// @class MotionServer +/// @brief gRPC server implementation of a `Motion` service. +/// @ingroup Motion +class MotionServer : public ResourceServer, + public viam::service::motion::v1::MotionService::Service { + public: + MotionServer(); + explicit MotionServer(std::shared_ptr manager); + + ::grpc::Status Move(::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveRequest* request, + ::viam::service::motion::v1::MoveResponse* response) override; + ::grpc::Status MoveOnMap(::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveOnMapRequest* request, + ::viam::service::motion::v1::MoveOnMapResponse* response) override; + ::grpc::Status MoveOnGlobe(::grpc::ServerContext* context, + const ::viam::service::motion::v1::MoveOnGlobeRequest* request, + ::viam::service::motion::v1::MoveOnGlobeResponse* response) override; + ::grpc::Status GetPose(::grpc::ServerContext* context, + const ::viam::service::motion::v1::GetPoseRequest* request, + ::viam::service::motion::v1::GetPoseResponse* response) override; + ::grpc::Status DoCommand(::grpc::ServerContext* context, + const ::viam::common::v1::DoCommandRequest* request, + ::viam::common::v1::DoCommandResponse* response) override; + + void register_server(std::shared_ptr server) override; +}; + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index 243cce2c3..b950a7e62 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -3,6 +3,8 @@ #include #include +#include + #include #include @@ -11,59 +13,109 @@ namespace viam { namespace sdk { viam::common::v1::Sphere GeometryConfig::sphere_proto() const { - viam::common::v1::Sphere sphere; - sphere.set_radius_mm(r_); - return sphere; + try { + viam::common::v1::Sphere sphere; + const auto sphere_specifics = boost::get(geometry_specifics_); + sphere.set_radius_mm(sphere_specifics.radius); + return sphere; + } catch (...) { + throw std::runtime_error( + "Couldn't convert geometry config to sphere proto; sphere specifics not found"); + } } viam::common::v1::RectangularPrism GeometryConfig::box_proto() const { - viam::common::v1::RectangularPrism box; - viam::common::v1::Vector3 vec3; - vec3.set_x(coordinates_.x); - vec3.set_y(coordinates_.y); - vec3.set_z(coordinates_.z); - *box.mutable_dims_mm() = vec3; - return box; + try { + const auto box_specifics = boost::get(geometry_specifics_); + viam::common::v1::RectangularPrism box; + viam::common::v1::Vector3 vec3; + vec3.set_x(box_specifics.x); + vec3.set_y(box_specifics.y); + vec3.set_z(box_specifics.z); + *box.mutable_dims_mm() = vec3; + return box; + } catch (...) { + throw std::runtime_error( + "Couldn't convert geometry config to box proto; box specifics not found"); + } +} + +viam::common::v1::Capsule GeometryConfig::capsule_proto() const { + try { + const auto capsule_specifics = boost::get(geometry_specifics_); + viam::common::v1::Capsule capsule; + capsule.set_radius_mm(capsule_specifics.radius); + capsule.set_length_mm(capsule_specifics.length); + return capsule; + } catch (...) { + throw std::runtime_error( + "Couldn't convert geometry config to capsule proto; capsule specifics not found"); + } } viam::common::v1::Pose GeometryConfig::pose_proto() const { - viam::common::v1::Pose pose; - pose.set_x(coordinates_.x); - pose.set_y(coordinates_.y); - pose.set_z(coordinates_.z); - pose.set_o_x(pose_.o_x); - pose.set_o_y(pose_.o_y); - pose.set_o_z(pose_.o_z); - pose.set_theta(0); + return pose_.to_proto(); +} + +pose pose::from_proto(const viam::common::v1::Pose& proto) { + struct pose pose; + pose.coordinates.x = proto.x(); + pose.coordinates.y = proto.y(); + pose.coordinates.z = proto.z(); + pose.orientation.o_x = proto.o_x(); + pose.orientation.o_y = proto.o_y(); + pose.orientation.o_z = proto.o_z(); + pose.theta = proto.theta(); + return pose; } +viam::common::v1::Pose pose::to_proto() const { + viam::common::v1::Pose proto; + proto.set_x(coordinates.x); + proto.set_y(coordinates.y); + proto.set_z(coordinates.z); + proto.set_o_x(orientation.o_x); + proto.set_o_y(orientation.o_y); + proto.set_o_z(orientation.o_z); + proto.set_theta(theta); + + return proto; +} + GeometryConfig GeometryConfig::from_proto(const viam::common::v1::Geometry& proto) { GeometryConfig cfg; const auto& pose = proto.center(); - cfg.coordinates_.x = pose.x(); - cfg.coordinates_.y = pose.y(); - cfg.coordinates_.z = pose.z(); - cfg.pose_.o_x = pose.o_x(); - cfg.pose_.o_y = pose.o_y(); - cfg.pose_.o_z = pose.o_z(); + cfg.pose_ = pose::from_proto(pose); cfg.label_ = proto.label(); switch (proto.geometry_type_case()) { case viam::common::v1::Geometry::GeometryTypeCase::kBox: { - cfg.geometry_type_ = box; - cfg.coordinates_.x = proto.box().dims_mm().x(); - cfg.coordinates_.y = proto.box().dims_mm().y(); - cfg.coordinates_.z = proto.box().dims_mm().z(); + cfg.geometry_type_ = GeometryType::box; + struct box box; + box.x = proto.box().dims_mm().x(); + box.y = proto.box().dims_mm().y(); + box.z = proto.box().dims_mm().z(); + cfg.set_geometry_specifics(box); return cfg; } case viam::common::v1::Geometry::GeometryTypeCase::kSphere: { - cfg.r_ = proto.sphere().radius_mm(); - if (cfg.r_ == 0) { - cfg.geometry_type_ = point; + auto r = proto.sphere().radius_mm(); + if (r == 0) { + cfg.geometry_type_ = GeometryType::point; } else { - cfg.geometry_type_ = sphere; + cfg.geometry_type_ = GeometryType::sphere; } + struct sphere sphere({r}); + cfg.set_geometry_specifics(sphere); + return cfg; + } + case viam::common::v1::Geometry::GeometryTypeCase::kCapsule: { + cfg.geometry_type_ = GeometryType::capsule; + struct capsule capsule; + capsule.radius = proto.capsule().radius_mm(); + capsule.length = proto.capsule().length_mm(); + cfg.set_geometry_specifics(capsule); return cfg; } case viam::common::v1::Geometry::GeometryTypeCase::GEOMETRY_TYPE_NOT_SET: @@ -86,24 +138,27 @@ viam::common::v1::Geometry GeometryConfig::to_proto() const { *geometry_.mutable_label() = label_; *geometry_.mutable_center() = pose_proto(); switch (geometry_type_) { - case box: { + case GeometryType::box: { *geometry_.mutable_box() = box_proto(); return geometry_; } - case sphere: { + case GeometryType::sphere: { *geometry_.mutable_sphere() = sphere_proto(); return geometry_; } case point: { - *geometry_.mutable_center() = pose_proto(); viam::common::v1::Sphere sphere; sphere.set_radius_mm(0); *geometry_.mutable_sphere() = sphere; return geometry_; } + case capsule: { + *geometry_.mutable_capsule() = capsule_proto(); + return geometry_; + } case unknown: default: { - if (coordinates_.x == 0 && coordinates_.y == 0 && coordinates_.z == 0) { + if (pose_.coordinates.x == 0 && pose_.coordinates.y == 0 && pose_.coordinates.z == 0) { *geometry_.mutable_box() = box_proto(); } else { *geometry_.mutable_sphere() = sphere_proto(); @@ -113,13 +168,19 @@ viam::common::v1::Geometry GeometryConfig::to_proto() const { } } void GeometryConfig::set_coordinates(coordinates coordinates) { - coordinates_ = std::move(coordinates); + pose_.coordinates = std::move(coordinates); } void GeometryConfig::set_pose(pose pose) { pose_ = std::move(pose); } -void GeometryConfig::set_radius(double r) { - r_ = std::move(r); +void GeometryConfig::set_geometry_specifics(geometry_specifics gs) { + geometry_specifics_ = std::move(gs); +} +void GeometryConfig::set_pose_orientation(pose_orientation orientation) { + pose_.orientation = std::move(orientation); +} +void GeometryConfig::set_theta(double theta) { + pose_.theta = std::move(theta); } void GeometryConfig::set_geometry_type(GeometryType type) { geometry_type_ = std::move(type); @@ -130,17 +191,19 @@ void GeometryConfig::set_orientation_config(OrientationConfig config) { void GeometryConfig::set_label(std::string label) { label_ = std::move(label); } -double GeometryConfig::get_radius() const { - return r_; +geometry_specifics GeometryConfig::get_geometry_specifics() const { + return geometry_specifics_; +} +double GeometryConfig::get_theta() const { + return pose_.theta; } - GeometryType GeometryConfig::get_geometry_type() const { return geometry_type_; } -GeometryConfig::coordinates GeometryConfig::get_coordinates() const { - return coordinates_; +coordinates GeometryConfig::get_coordinates() const { + return pose_.coordinates; } -GeometryConfig::pose GeometryConfig::get_pose() const { +pose GeometryConfig::get_pose() const { return pose_; } OrientationConfig GeometryConfig::get_orientation_config() const { @@ -151,15 +214,103 @@ std::string GeometryConfig::get_label() const { return label_; } +std::ostream& operator<<(std::ostream& os, const pose& v) { + os << "{ coordinates: " + << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z + << "},\norientation: " + << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y + << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const geo_point& v) { + os << "{ latitude: " << v.latitude << ", longitude: " << v.longitude << "}\n"; + return os; +} + +bool operator==(const coordinates& lhs, const coordinates& rhs) { + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; +} + +bool operator==(const pose_orientation& lhs, const pose_orientation& rhs) { + return lhs.o_x == rhs.o_x && lhs.o_y == rhs.o_y && lhs.o_z == rhs.o_z; +} + +bool operator==(const pose& lhs, const pose& rhs) { + return lhs.coordinates == rhs.coordinates && lhs.orientation == rhs.orientation && + lhs.theta == rhs.theta; +} + +bool operator==(const struct box& lhs, const struct box& rhs) { + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; +} + +bool operator==(const struct sphere& lhs, const struct sphere& rhs) { + return lhs.radius == rhs.radius; +} + +bool operator==(const struct capsule& lhs, const struct capsule& rhs) { + return lhs.radius == rhs.radius && lhs.length == rhs.length; +} + bool operator==(const GeometryConfig& lhs, const GeometryConfig& rhs) { - // Only spheres contain a radius variable that is passed "round-trip" to and from proto - if (lhs.geometry_type_ == GeometryType::sphere && lhs.r_ != rhs.r_) { - return false; + const auto& lhs_coordinates = lhs.pose_.coordinates; + const auto& rhs_coordinates = rhs.pose_.coordinates; + const auto& lhs_orientation = lhs.pose_.orientation; + const auto& rhs_orientation = rhs.pose_.orientation; + return lhs_coordinates.x == rhs_coordinates.x && lhs_coordinates.y == rhs_coordinates.y && + lhs_coordinates.z == rhs_coordinates.z && lhs_orientation.o_x == rhs_orientation.o_x && + lhs_orientation.o_y == rhs_orientation.o_y && + lhs_orientation.o_z == rhs_orientation.o_z && lhs.label_ == rhs.label_ && + lhs.geometry_type_ == rhs.geometry_type_ && + lhs.geometry_specifics_ == rhs.geometry_specifics_; +} + +bool operator==(const geo_point& lhs, const geo_point& rhs) { + return lhs.latitude == rhs.latitude && lhs.longitude == rhs.longitude; +} + +bool operator==(const geo_obstacle& lhs, const geo_obstacle& rhs) { + return lhs.location == rhs.location && lhs.geometries == rhs.geometries; +} + +common::v1::GeoPoint geo_point::to_proto() const { + common::v1::GeoPoint proto; + proto.set_latitude(latitude); + proto.set_longitude(longitude); + + return proto; +} + +geo_point geo_point::from_proto(const common::v1::GeoPoint& proto) { + struct geo_point geo_point; + geo_point.latitude = proto.latitude(); + geo_point.longitude = proto.longitude(); + + return geo_point; +} + +common::v1::GeoObstacle geo_obstacle::to_proto() const { + common::v1::GeoObstacle proto; + *proto.mutable_location() = location.to_proto(); + + for (const auto& geometry : geometries) { + *proto.mutable_geometries()->Add() = geometry.to_proto(); } - return lhs.coordinates_.x == rhs.coordinates_.x && lhs.coordinates_.y == rhs.coordinates_.y && - lhs.coordinates_.z == rhs.coordinates_.z && lhs.pose_.o_x == rhs.pose_.o_x && - lhs.pose_.o_y == rhs.pose_.o_y && lhs.pose_.o_z == rhs.pose_.o_z && - lhs.label_ == rhs.label_ && lhs.geometry_type_ == rhs.geometry_type_; + + return proto; +} + +geo_obstacle geo_obstacle::from_proto(const common::v1::GeoObstacle& proto) { + struct geo_obstacle geo_obstacle; + + geo_obstacle.location = geo_point::from_proto(proto.location()); + for (const auto& proto_geometry : proto.geometries()) { + auto geometry = GeometryConfig::from_proto(proto_geometry); + geo_obstacle.geometries.push_back(std::move(geometry)); + } + + return geo_obstacle; } } // namespace sdk diff --git a/src/viam/sdk/spatialmath/geometry.hpp b/src/viam/sdk/spatialmath/geometry.hpp index e9f5577ab..876c13a52 100644 --- a/src/viam/sdk/spatialmath/geometry.hpp +++ b/src/viam/sdk/spatialmath/geometry.hpp @@ -1,61 +1,98 @@ #pragma once +#include #include #include #include +#include #include namespace viam { namespace sdk { +// TODO(RSDK-4553): add thorough documentation to this whole file. enum GeometryType { box, sphere, + capsule, point, unknown, }; +struct box { + double x; + double y; + double z; + friend bool operator==(const box& lhs, const box& rhs); +}; +struct sphere { + double radius; + friend bool operator==(const sphere& lhs, const sphere& rhs); +}; +struct capsule { + double radius; + double length; + friend bool operator==(const capsule& lhs, const capsule& rhs); +}; + +typedef boost::variant geometry_specifics; + class GeometryConfig { public: - struct coordinates { - double x, y, z; - }; - struct pose { - double o_x, o_y, o_z; - }; viam::common::v1::Geometry to_proto() const; viam::common::v1::RectangularPrism box_proto() const; viam::common::v1::Sphere sphere_proto() const; + viam::common::v1::Capsule capsule_proto() const; viam::common::v1::Pose pose_proto() const; static GeometryConfig from_proto(const viam::common::v1::Geometry& proto); static std::vector from_proto( const viam::common::v1::GetGeometriesResponse& proto); void set_coordinates(coordinates coordinates); void set_pose(pose pose); - void set_radius(double r); + void set_pose_orientation(pose_orientation orientation); + void set_theta(double theta); + void set_geometry_specifics(geometry_specifics gs); void set_geometry_type(GeometryType type); void set_orientation_config(OrientationConfig config); void set_label(std::string label); - double get_radius() const; + double get_theta() const; coordinates get_coordinates() const; pose get_pose() const; + geometry_specifics get_geometry_specifics() const; GeometryType get_geometry_type() const; OrientationConfig get_orientation_config() const; std::string get_label() const; friend bool operator==(const GeometryConfig& lhs, const GeometryConfig& rhs); private: - coordinates coordinates_; GeometryType geometry_type_; pose pose_; - double r_; + geometry_specifics geometry_specifics_; // TODO: if and when RDK makes more explicit use of ox/oy/oz, we should // do the same here OrientationConfig orientation_config_; std::string label_; }; +struct geo_point { + double longitude, latitude; + + common::v1::GeoPoint to_proto() const; + static geo_point from_proto(const common::v1::GeoPoint& proto); + friend bool operator==(const geo_point& lhs, const geo_point& rhs); + friend std::ostream& operator<<(std::ostream& os, const geo_point& v); +}; + +struct geo_obstacle { + geo_point location; + std::vector geometries; + + common::v1::GeoObstacle to_proto() const; + static geo_obstacle from_proto(const common::v1::GeoObstacle& proto); + friend bool operator==(const geo_obstacle& lhs, const geo_obstacle& rhs); +}; + } // namespace sdk } // namespace viam diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index e7fc89593..ee3def5f5 100644 --- a/src/viam/sdk/tests/CMakeLists.txt +++ b/src/viam/sdk/tests/CMakeLists.txt @@ -26,6 +26,7 @@ target_sources(viamsdk_test mocks/mock_board.cpp mocks/mock_encoder.cpp mocks/mock_motor.cpp + mocks/mock_motion.cpp mocks/mock_robot.cpp test_utils.cpp ) @@ -42,5 +43,6 @@ viamcppsdk_add_boost_test(test_encoder.cpp) viamcppsdk_add_boost_test(test_generic_component.cpp) 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_robot.cpp) viamcppsdk_add_boost_test(test_types.cpp) diff --git a/src/viam/sdk/tests/mocks/mock_motion.cpp b/src/viam/sdk/tests/mocks/mock_motion.cpp new file mode 100644 index 000000000..faf0b9e2b --- /dev/null +++ b/src/viam/sdk/tests/mocks/mock_motion.cpp @@ -0,0 +1,117 @@ +#include + +#include +#include +#include + +namespace viam { +namespace sdktests { +namespace motion { + +using namespace viam::sdk; + +bool MockMotion::move(const pose_in_frame& destination, + const Name& component_name, + std::shared_ptr world_state, + std::shared_ptr constraints, + const AttributeMap& extra) { + this->current_location = destination; + this->peek_component_name = component_name; + this->peek_world_state = world_state; + this->peek_constraints = constraints; + return true; +}; + +bool MockMotion::move_on_map(const pose& destination, + const Name& component_name, + const Name& slam_name, + const AttributeMap& extra) { + this->peek_current_pose = std::move(destination); + this->peek_component_name = std::move(component_name); + this->peek_slam_name = std::move(slam_name); + this->current_location.pose = std::move(destination); + + return true; +}; + +bool MockMotion::move_on_globe(const geo_point& destination, + const boost::optional& heading, + const Name& component_name, + const Name& movement_sensor_name, + const std::vector& obstacles, + std::shared_ptr motion_configuration, + const AttributeMap& extra) { + this->peek_heading = *heading; + this->peek_component_name = std::move(component_name); + this->peek_movement_sensor_name = std::move(movement_sensor_name); + this->peek_destination = std::move(destination); + this->peek_obstacles = std::move(obstacles); + this->peek_motion_configuration = std::move(motion_configuration); + + return true; +} + +pose_in_frame MockMotion::get_pose( + const Name& component_name, + const std::string& destination_frame, + const std::vector& supplemental_transforms, + AttributeMap extra) { + return current_location; +}; + +AttributeMap MockMotion::do_command(const AttributeMap& _command) { + return peek_map; +}; + +std::shared_ptr MockMotion::get_mock_motion() { + auto motion = std::make_shared("mock_motion"); + motion->current_location = init_fake_pose(); + motion->peek_map = fake_map(); + + return motion; +}; + +pose_in_frame fake_pose() { + return pose_in_frame("fake-reference-frame", {{0, 1, 2}, {3, 4, 5}, 6}); +} + +pose_in_frame init_fake_pose() { + return pose_in_frame("", {{0, 0, 0}, {0, 0, 0}, 0}); +} + +Name fake_component_name() { + return {{"acme", "component", "fake"}, "", "fake-component"}; +} + +Name fake_slam_name() { + return {{"acme", "service", "slam"}, "", "fake-slam"}; +} + +Name fake_movement_sensor_name() { + return {{"acme", "component", ""}, "", "fake-movement-sensor"}; +} + +geo_point fake_geo_point() { + return {5, 10}; +} + +std::shared_ptr fake_motion_configuration() { + auto mc = std::make_shared(); + mc->plan_deviation_m = 55; + return mc; +} + +std::vector fake_obstacles() { + GeometryConfig gc; + gc.set_pose({{20, 25, 30}, {35, 40, 45}, 50}); + gc.set_geometry_type(sdk::sphere); + struct sphere sphere({1}); + gc.set_geometry_specifics(sphere); + gc.set_label("label"); + gc.set_orientation_config({AxisAngles, {}, axis_angles{1, 2, 3, 4}}); + return {{fake_geo_point(), {std::move(gc)}}}; +} + +} // namespace motion +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/mocks/mock_motion.hpp b/src/viam/sdk/tests/mocks/mock_motion.hpp new file mode 100644 index 000000000..9e9b42c47 --- /dev/null +++ b/src/viam/sdk/tests/mocks/mock_motion.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include + +namespace viam { +namespace sdktests { +namespace motion { + +sdk::pose_in_frame init_fake_pose(); +sdk::pose_in_frame fake_pose(); + +sdk::Name fake_component_name(); +sdk::Name fake_slam_name(); +sdk::Name fake_movement_sensor_name(); +sdk::geo_point fake_geo_point(); +std::vector fake_obstacles(); +std::shared_ptr fake_motion_configuration(); + +class MockMotion : public sdk::Motion { + public: + bool move(const sdk::pose_in_frame& destination, + const sdk::Name& component_name, + std::shared_ptr world_state, + std::shared_ptr constraints, + const sdk::AttributeMap& extra) override; + + bool move_on_map(const sdk::pose& destination, + const sdk::Name& component_name, + const sdk::Name& slam_name, + const sdk::AttributeMap& extra) override; + + bool move_on_globe(const sdk::geo_point& destination, + const boost::optional& heading, + const sdk::Name& component_name, + const sdk::Name& movement_sensor_name, + const std::vector& obstacles, + std::shared_ptr motion_configuration, + const sdk::AttributeMap& extra) override; + + sdk::pose_in_frame get_pose( + const sdk::Name& component_name, + const std::string& destination_frame, + const std::vector& supplemental_transforms, + sdk::AttributeMap extra) override; + + sdk::AttributeMap do_command(const sdk::AttributeMap& command) override; + static std::shared_ptr get_mock_motion(); + + // These variables allow the testing infra to `peek` into the mock + // and ensure that the correct values were passed + sdk::pose_in_frame current_location; + sdk::pose peek_current_pose; + sdk::Name peek_component_name; + sdk::Name peek_movement_sensor_name; + sdk::Name peek_slam_name; + sdk::geo_point peek_destination; + std::string peek_destination_frame; + double peek_heading; + std::vector peek_obstacles; + std::shared_ptr peek_constraints; + std::shared_ptr peek_motion_configuration; + std::shared_ptr peek_world_state; + sdk::AttributeMap peek_map; + + MockMotion(std::string name) + : sdk::Motion(std::move(name)), current_location(init_fake_pose()){}; +}; + +} // namespace motion +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/test_motion.cpp b/src/viam/sdk/tests/test_motion.cpp new file mode 100644 index 000000000..942336def --- /dev/null +++ b/src/viam/sdk/tests/test_motion.cpp @@ -0,0 +1,207 @@ +#define BOOST_TEST_MODULE test module test_motion + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::WorldState) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) + +namespace viam { +namespace sdktests { +namespace motion { + +using namespace viam::sdk; + +WorldState mock_world_state() { + WorldState::transform transform; + transform.reference_frame = "fake-reference-frame"; + transform.pose_in_observer_frame = pose_in_frame("fake", {{2, 3, 4}, {5, 6, 7}, 8}); + WorldState::geometries_in_frame obstacle; + obstacle.reference_frame = "ref"; + + return WorldState({obstacle}, {transform}); +} + +BOOST_AUTO_TEST_SUITE(test_mock) + +BOOST_AUTO_TEST_CASE(mock_move_and_get_pose) { + std::shared_ptr motion = MockMotion::get_mock_motion(); + + BOOST_CHECK_EQUAL(motion->current_location, std::move(init_fake_pose())); + + Name fake_name({"acme", "service", "motion"}, "fake-remote", "fake-motion"); + auto ws = std::make_shared(mock_world_state()); + + bool success = motion->move(fake_pose(), fake_name, ws, 0, fake_map()); + + BOOST_TEST(success); + BOOST_CHECK_EQUAL(motion->current_location, fake_pose()); + BOOST_CHECK_EQUAL(motion->peek_component_name, std::move(fake_name)); + BOOST_CHECK_EQUAL(*(motion->peek_world_state), *ws); +} + +BOOST_AUTO_TEST_CASE(mock_move_on_map) { + std::shared_ptr motion = MockMotion::get_mock_motion(); + + BOOST_CHECK_EQUAL(motion->current_location, std::move(init_fake_pose())); + + pose new_destination({{3, 4, 5}, {6, 7, 8}, 9}); + + bool success = + motion->move_on_map(new_destination, fake_component_name(), fake_slam_name(), fake_map()); + + BOOST_TEST(success); + BOOST_CHECK_EQUAL(motion->peek_current_pose, std::move(new_destination)); + BOOST_CHECK_EQUAL(motion->peek_component_name, fake_component_name()); + BOOST_CHECK_EQUAL(motion->peek_slam_name, fake_slam_name()); +} + +BOOST_AUTO_TEST_CASE(mock_move_on_globe) { + std::shared_ptr motion = MockMotion::get_mock_motion(); + + bool success = motion->move_on_globe(fake_geo_point(), + 15, + fake_component_name(), + fake_movement_sensor_name(), + fake_obstacles(), + fake_motion_configuration(), + fake_map()); + + BOOST_TEST(success); + + BOOST_CHECK_EQUAL(motion->peek_destination, fake_geo_point()); + BOOST_CHECK_EQUAL(motion->peek_heading, 15); + BOOST_CHECK_EQUAL(motion->peek_component_name, fake_component_name()); + BOOST_CHECK_EQUAL(motion->peek_movement_sensor_name, fake_movement_sensor_name()); + BOOST_CHECK_EQUAL(motion->peek_obstacles, fake_obstacles()); + BOOST_CHECK_EQUAL(*(motion->peek_motion_configuration), *(fake_motion_configuration())); +} + +BOOST_AUTO_TEST_CASE(mock_do_command) { + std::shared_ptr motion = MockMotion::get_mock_motion(); + AttributeMap expected = fake_map(); + + AttributeMap command = fake_map(); + AttributeMap result_map = motion->do_command(command); + + ProtoType expected_pt = *(expected->at(std::string("test"))); + ProtoType result_pt = *(result_map->at(std::string("test"))); + + BOOST_CHECK(result_pt == expected_pt); +} + +BOOST_AUTO_TEST_SUITE_END() + +BOOST_AUTO_TEST_SUITE(test_motion_client_server) + +// This sets up the following architecture +// -- MockService +// /\ +// +// | (function calls) +// +// \/ +// -- ServiceServer (Real) +// /\ +// +// | (grpc InProcessChannel) +// +// \/ +// -- ServiceClient (Real) +// +// This is as close to a real setup as we can get +// without starting another process +// +// The passed in lambda function has access to the ServiceClient +template +void server_to_mock_pipeline(Lambda&& func) { + MotionServer motion_server; + motion_server.resource_manager()->add(std::string("mock_motion"), + MockMotion::get_mock_motion()); + + grpc::ServerBuilder builder; + builder.RegisterService(&motion_server); + + std::unique_ptr server = builder.BuildAndStart(); + + grpc::ChannelArguments args; + auto grpc_channel = server->InProcessChannel(args); + MotionClient client("mock_motion", grpc_channel); + // Run the passed test on the created stack + std::forward(func)(client); + // shutdown afterwards + server->Shutdown(); +} + +BOOST_AUTO_TEST_CASE(test_move_and_get_pose) { + server_to_mock_pipeline([](Motion& client) -> void { + std::string destination_frame("destination"); + std::vector transforms; + AttributeMap extra = fake_map(); + pose_in_frame pose = client.get_pose(fake_component_name(), destination_frame, {}, extra); + BOOST_CHECK_EQUAL(pose, init_fake_pose()); + + auto ws = std::make_shared(mock_world_state()); + bool success = client.move(fake_pose(), fake_component_name(), ws, nullptr, fake_map()); + BOOST_TEST(success); + + pose = client.get_pose(fake_component_name(), destination_frame, transforms, extra); + BOOST_CHECK_EQUAL(pose, fake_pose()); + }); +} + +BOOST_AUTO_TEST_CASE(test_move_on_map) { + server_to_mock_pipeline([](Motion& client) -> void { + pose destination({{1, 2, 3}, {4, 5, 6}, 7}); + bool success = + client.move_on_map(destination, fake_component_name(), fake_slam_name(), fake_map()); + + BOOST_TEST(success); + + pose_in_frame pose = client.get_pose(fake_component_name(), "", {}, fake_map()); + BOOST_CHECK_EQUAL(pose.pose, destination); + }); +} + +BOOST_AUTO_TEST_CASE(test_move_on_globe) { + BOOST_TEST(true); + server_to_mock_pipeline([](Motion& client) -> void { + bool success = client.move_on_globe(fake_geo_point(), + 15, + fake_component_name(), + fake_movement_sensor_name(), + fake_obstacles(), + fake_motion_configuration(), + fake_map()); + + BOOST_TEST(success); + }); +} + +BOOST_AUTO_TEST_CASE(test_do_command) { + server_to_mock_pipeline([](Motion& client) -> void { + AttributeMap expected = fake_map(); + + AttributeMap command = fake_map(); + AttributeMap result_map = client.do_command(command); + + ProtoType expected_pt = *(expected->at(std::string("test"))); + ProtoType result_pt = *(result_map->at(std::string("test"))); + + BOOST_CHECK(result_pt == expected_pt); + }); +} + +BOOST_AUTO_TEST_SUITE_END() + +} // namespace motion +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/test_utils.cpp b/src/viam/sdk/tests/test_utils.cpp index 50a8459ee..cb2693a95 100644 --- a/src/viam/sdk/tests/test_utils.cpp +++ b/src/viam/sdk/tests/test_utils.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -25,26 +26,38 @@ std::vector fake_geometries() { sphere_config.set_geometry_type(GeometryType::sphere); sphere_config.set_coordinates({1, 2, 3}); sphere_config.set_pose({1, 2, 3}); - sphere_config.set_radius(1); + struct sphere sphere({1}); + sphere_config.set_geometry_specifics(std::move(sphere)); sphere_config.set_label("sphere"); GeometryConfig box_config; box_config.set_geometry_type(GeometryType::box); box_config.set_coordinates({1, 2, 3}); box_config.set_pose({1, 2, 3}); - box_config.set_radius(1); + struct box box({1, 2, 3}); + box_config.set_geometry_specifics(std::move(box)); box_config.set_label("box"); GeometryConfig point_config; point_config.set_geometry_type(GeometryType::point); point_config.set_coordinates({1, 2, 3}); point_config.set_pose({1, 2, 3}); - point_config.set_radius(0); + struct sphere point({0}); + point_config.set_geometry_specifics(std::move(point)); point_config.set_label("point"); - std::vector geometries = { - std::move(sphere_config), std::move(box_config), std::move(point_config)}; - return geometries; + GeometryConfig capsule_config; + capsule_config.set_geometry_type(GeometryType::capsule); + capsule_config.set_coordinates({1, 2, 3}); + capsule_config.set_pose({1, 2, 3}); + struct capsule capsule({2, 4}); + capsule_config.set_geometry_specifics(std::move(capsule)); + capsule_config.set_label("capsule"); + + return {std::move(sphere_config), + std::move(box_config), + std::move(point_config), + std::move(capsule_config)}; } } // namespace sdktests