Feat implement get 3d meshes (#504)

Co-authored-by: Lia Stratopoulos <[email protected]>

Author: mattmacf98

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

src/viam/sdk/common/mesh.cpp

@@ -0,0 +1,22 @@
+#include <viam/sdk/common/mesh.hpp>
+
+#include <viam/sdk/common/utils.hpp>
+
+#include <common/v1/common.pb.h>
+
+namespace viam {
+namespace sdk {
+
+namespace proto_convert_details {
+void to_proto_impl<mesh>::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<common::v1::Mesh>::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

src/viam/sdk/common/mesh.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <viam/sdk/common/proto_convert.hpp>
+
+#include <string>
+#include <vector>
+
+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<unsigned char> data;
+};
+
+namespace proto_convert_details {
+
+template <>
+struct to_proto_impl<mesh> {
+    void operator()(const mesh&, common::v1::Mesh*) const;
+};
+
+template <>
+struct from_proto_impl<common::v1::Mesh> {
+    mesh operator()(const common::v1::Mesh*) const;
+};
+
+}  // namespace proto_convert_details
+
+}  // namespace sdk
+}  // namespace viam

src/viam/sdk/components/arm.hpp

@@ -8,6 +8,7 @@
 #include <boost/optional/optional.hpp>
 #include <boost/variant/variant.hpp>
 
+#include <viam/sdk/common/mesh.hpp>
 #include <viam/sdk/common/pose.hpp>
 #include <viam/sdk/resource/stoppable.hpp>
 #include <viam/sdk/spatialmath/geometry.hpp>
@@ -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<std::string, mesh> 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<std::string, mesh> get_3d_models() {
+        return get_3d_models({});
+    }
+
     /// @brief Returns `GeometryConfig`s associated with the calling arm
     inline std::vector<GeometryConfig> get_geometries() {
         return get_geometries({});

src/viam/sdk/components/private/arm_client.cpp

@@ -106,6 +106,18 @@ Arm::KinematicsData ArmClient::get_kinematics(const ProtoStruct& extra) {
         });
 }
 
+std::map<std::string, mesh> ArmClient::get_3d_models(const ProtoStruct& extra) {
+    return make_client_helper(this, *stub_, &StubType::Get3DModels)
+        .with(extra)
+        .invoke([](auto& response) {
+            std::map<std::string, mesh> models;
+            for (const auto& entry : response.models()) {
+                models.emplace(entry.first, from_proto(entry.second));
+            }
+            return models;
+        });
+}
+
 std::vector<GeometryConfig> ArmClient::get_geometries(const ProtoStruct& extra) {
     return make_client_helper(this, *stub_, &StubType::GetGeometries)
         .with(extra)

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<std::string, mesh> get_3d_models(const ProtoStruct& extra) override;
     std::vector<GeometryConfig> 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;

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<Arm>(
+        "ArmServer::Get3DModels", this, request)([&](auto& helper, auto& arm) {
+        const std::map<std::string, mesh> 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,

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,

src/viam/sdk/tests/mocks/mock_arm.cpp

@@ -1,3 +1,4 @@
+#include "viam/sdk/common/mesh.hpp"
 #include <viam/sdk/tests/mocks/mock_arm.hpp>
 
 #include "mock_arm.hpp"
@@ -7,6 +8,10 @@ namespace viam {
 namespace sdktests {
 namespace arm {
 
+std::map<std::string, sdk::mesh> fake_3d_models() {
+    return {{"model1", sdk::mesh{"model1", {1, 2, 3, 4}}}};
+}
+
 sdk::Arm::KinematicsData fake_kinematics() {
     return sdk::Arm::KinematicsDataSVA{{std::vector<unsigned char>{1, 2, 3, 4}}};
 }
@@ -59,6 +64,10 @@ std::vector<sdk::GeometryConfig> MockArm::get_geometries(const sdk::ProtoStruct&
     return fake_geometries();
 }
 
+std::map<std::string, sdk::mesh> MockArm::get_3d_models(const sdk::ProtoStruct&) {
+    return fake_3d_models();
+}
+
 }  // namespace arm
 }  // namespace sdktests
 }  // namespace viam

src/viam/sdk/tests/mocks/mock_arm.hpp

@@ -7,6 +7,7 @@ namespace sdktests {
 namespace arm {
 
 sdk::Arm::KinematicsData fake_kinematics();
+std::map<std::string, sdk::mesh> 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<sdk::GeometryConfig> get_geometries(const sdk::ProtoStruct&) override;
-
+    std::map<std::string, sdk::mesh> get_3d_models(const sdk::ProtoStruct&) override;
     sdk::pose current_location;
     std::vector<double> joint_positions;
     std::vector<std::vector<double>> move_thru_positions;