diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index 52a6eabdd..f15e25ae4 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -66,6 +66,7 @@ target_sources(viamsdk common/exception.cpp common/instance.cpp common/linear_algebra.cpp + common/mesh.cpp common/pose.cpp common/proto_value.cpp common/utils.cpp @@ -177,6 +178,7 @@ target_sources(viamsdk ../../viam/sdk/common/exception.hpp ../../viam/sdk/common/instance.hpp ../../viam/sdk/common/linear_algebra.hpp + ../../viam/sdk/common/mesh.hpp ../../viam/sdk/common/pose.hpp ../../viam/sdk/common/proto_convert.hpp ../../viam/sdk/common/proto_value.hpp diff --git a/src/viam/sdk/common/mesh.cpp b/src/viam/sdk/common/mesh.cpp new file mode 100644 index 000000000..cbb9b8ec7 --- /dev/null +++ b/src/viam/sdk/common/mesh.cpp @@ -0,0 +1,22 @@ +#include + +#include + +#include + +namespace viam { +namespace sdk { + +namespace proto_convert_details { +void to_proto_impl::operator()(const mesh& self, common::v1::Mesh* proto) const { + proto->set_content_type(self.content_type); + proto->set_mesh(bytes_to_string(self.data)); +} + +mesh from_proto_impl::operator()(const common::v1::Mesh* proto) const { + return mesh{proto->content_type(), string_to_bytes(proto->mesh())}; +} + +} // namespace proto_convert_details +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/common/mesh.hpp b/src/viam/sdk/common/mesh.hpp new file mode 100644 index 000000000..bb239258a --- /dev/null +++ b/src/viam/sdk/common/mesh.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + +#include +#include + +namespace viam { +namespace common { +namespace v1 { + +class Mesh; + +} // namespace v1 +} // namespace common +} // namespace viam + +namespace viam { +namespace sdk { + +struct mesh { + std::string content_type; + std::vector data; +}; + +namespace proto_convert_details { + +template <> +struct to_proto_impl { + void operator()(const mesh&, common::v1::Mesh*) const; +}; + +template <> +struct from_proto_impl { + mesh operator()(const common::v1::Mesh*) const; +}; + +} // namespace proto_convert_details + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/components/arm.hpp b/src/viam/sdk/components/arm.hpp index 79b60bb26..c9d2f75d4 100644 --- a/src/viam/sdk/components/arm.hpp +++ b/src/viam/sdk/components/arm.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -142,6 +143,17 @@ class Arm : public Component, public Stoppable { return get_kinematics({}); } + /// @brief Returns `3DModel`s associated with the calling arm + /// @param extra Any additional arguments to the method + /// @return A map of `3DModel`s associated with the calling arm + virtual std::map get_3d_models(const ProtoStruct& extra) = 0; + + /// @brief Returns `3DModel`s associated with the calling arm + /// @return A map of `3DModel`s associated with the calling arm + inline std::map get_3d_models() { + return get_3d_models({}); + } + /// @brief Returns `GeometryConfig`s associated with the calling arm inline std::vector get_geometries() { return get_geometries({}); diff --git a/src/viam/sdk/components/private/arm_client.cpp b/src/viam/sdk/components/private/arm_client.cpp index 619580ad4..88b775dc7 100644 --- a/src/viam/sdk/components/private/arm_client.cpp +++ b/src/viam/sdk/components/private/arm_client.cpp @@ -106,6 +106,18 @@ Arm::KinematicsData ArmClient::get_kinematics(const ProtoStruct& extra) { }); } +std::map ArmClient::get_3d_models(const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::Get3DModels) + .with(extra) + .invoke([](auto& response) { + std::map models; + for (const auto& entry : response.models()) { + models.emplace(entry.first, from_proto(entry.second)); + } + return models; + }); +} + std::vector ArmClient::get_geometries(const ProtoStruct& extra) { return make_client_helper(this, *stub_, &StubType::GetGeometries) .with(extra) diff --git a/src/viam/sdk/components/private/arm_client.hpp b/src/viam/sdk/components/private/arm_client.hpp index 6608a1340..41d6cc003 100644 --- a/src/viam/sdk/components/private/arm_client.hpp +++ b/src/viam/sdk/components/private/arm_client.hpp @@ -33,10 +33,12 @@ class ArmClient : public Arm { void stop(const ProtoStruct& extra) override; ProtoStruct do_command(const ProtoStruct& command) override; Arm::KinematicsData get_kinematics(const ProtoStruct& extra) override; + std::map get_3d_models(const ProtoStruct& extra) override; std::vector get_geometries(const ProtoStruct& extra) override; // Using declarations to introduce convenience overloads of interface which do not need to be // passed the ProtoStruct parameter. + using Arm::get_3d_models; using Arm::get_end_position; using Arm::get_geometries; using Arm::get_joint_positions; diff --git a/src/viam/sdk/components/private/arm_server.cpp b/src/viam/sdk/components/private/arm_server.cpp index 3c1970412..d8850f669 100644 --- a/src/viam/sdk/components/private/arm_server.cpp +++ b/src/viam/sdk/components/private/arm_server.cpp @@ -135,6 +135,19 @@ ::grpc::Status ArmServer::GetKinematics( }); } +::grpc::Status ArmServer::Get3DModels(::grpc::ServerContext*, + const ::viam::common::v1::Get3DModelsRequest* request, + ::viam::common::v1::Get3DModelsResponse* response) noexcept { + return make_service_helper( + "ArmServer::Get3DModels", this, request)([&](auto& helper, auto& arm) { + const std::map models = arm->get_3d_models(helper.getExtra()); + + for (const auto& entry : models) { + response->mutable_models()->insert({entry.first, to_proto(entry.second)}); + } + }); +} + ::grpc::Status ArmServer::GetGeometries( ::grpc::ServerContext*, const ::viam::common::v1::GetGeometriesRequest* request, diff --git a/src/viam/sdk/components/private/arm_server.hpp b/src/viam/sdk/components/private/arm_server.hpp index f19788b60..dae2a5af9 100644 --- a/src/viam/sdk/components/private/arm_server.hpp +++ b/src/viam/sdk/components/private/arm_server.hpp @@ -67,6 +67,10 @@ class ArmServer : public ResourceServer, public viam::component::arm::v1::ArmSer const ::viam::common::v1::GetKinematicsRequest* request, ::viam::common::v1::GetKinematicsResponse* response) noexcept override; + ::grpc::Status Get3DModels(::grpc::ServerContext* context, + const ::viam::common::v1::Get3DModelsRequest* request, + ::viam::common::v1::Get3DModelsResponse* response) noexcept override; + ::grpc::Status GetGeometries( ::grpc::ServerContext* context, const ::viam::common::v1::GetGeometriesRequest* request, diff --git a/src/viam/sdk/tests/mocks/mock_arm.cpp b/src/viam/sdk/tests/mocks/mock_arm.cpp index 2eb4c8fb0..e8993b8ce 100644 --- a/src/viam/sdk/tests/mocks/mock_arm.cpp +++ b/src/viam/sdk/tests/mocks/mock_arm.cpp @@ -1,3 +1,4 @@ +#include "viam/sdk/common/mesh.hpp" #include #include "mock_arm.hpp" @@ -7,6 +8,10 @@ namespace viam { namespace sdktests { namespace arm { +std::map fake_3d_models() { + return {{"model1", sdk::mesh{"model1", {1, 2, 3, 4}}}}; +} + sdk::Arm::KinematicsData fake_kinematics() { return sdk::Arm::KinematicsDataSVA{{std::vector{1, 2, 3, 4}}}; } @@ -59,6 +64,10 @@ std::vector MockArm::get_geometries(const sdk::ProtoStruct& return fake_geometries(); } +std::map MockArm::get_3d_models(const sdk::ProtoStruct&) { + return fake_3d_models(); +} + } // namespace arm } // namespace sdktests } // namespace viam diff --git a/src/viam/sdk/tests/mocks/mock_arm.hpp b/src/viam/sdk/tests/mocks/mock_arm.hpp index fdd7537eb..7052f4b05 100644 --- a/src/viam/sdk/tests/mocks/mock_arm.hpp +++ b/src/viam/sdk/tests/mocks/mock_arm.hpp @@ -7,6 +7,7 @@ namespace sdktests { namespace arm { sdk::Arm::KinematicsData fake_kinematics(); +std::map fake_3d_models(); class MockArm : public sdk::Arm { public: @@ -29,7 +30,7 @@ class MockArm : public sdk::Arm { sdk::ProtoStruct do_command(const sdk::ProtoStruct& command) override; sdk::Arm::KinematicsData get_kinematics(const sdk::ProtoStruct&) override; std::vector get_geometries(const sdk::ProtoStruct&) override; - + std::map get_3d_models(const sdk::ProtoStruct&) override; sdk::pose current_location; std::vector joint_positions; std::vector> move_thru_positions; diff --git a/src/viam/sdk/tests/test_arm.cpp b/src/viam/sdk/tests/test_arm.cpp index 933fe781b..73e32fc5d 100644 --- a/src/viam/sdk/tests/test_arm.cpp +++ b/src/viam/sdk/tests/test_arm.cpp @@ -104,6 +104,26 @@ BOOST_AUTO_TEST_CASE(test_do_command) { }); } +BOOST_AUTO_TEST_CASE(test_get_3d_models) { + std::shared_ptr mock = MockArm::get_mock_arm(); + client_to_mock_pipeline(mock, [](Arm& client) { + const auto& models = client.get_3d_models(); + const auto& expected = fake_3d_models(); + + // Compare keysets directly + std::set model_keys; + std::set expected_keys; + for (const auto& [key, _] : models) { + model_keys.insert(key); + } + for (const auto& [key, _] : expected) { + expected_keys.insert(key); + } + BOOST_CHECK_EQUAL_COLLECTIONS( + model_keys.begin(), model_keys.end(), expected_keys.begin(), expected_keys.end()); + }); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace sdktests