Merge branch 'main' of ssh://github.com/viamrobotics/viam-cpp-sdk into make-proto-conversions-private

Author: stuqdog

CMakeLists.txt

@@ -34,7 +34,7 @@
 # constrained by the version of CMake available on target systems.
 cmake_minimum_required(VERSION 3.25 FATAL_ERROR)
 
-set(CMAKE_PROJECT_VERSION 0.0.15)
+set(CMAKE_PROJECT_VERSION 0.0.16)
 
 # Identify the project.
 project(viam-cpp-sdk

src/viam/api/CMakeLists.txt

@@ -225,6 +225,10 @@ if (VIAMCPPSDK_USE_DYNAMIC_PROTOS)
     ${PROTO_GEN_DIR}/service/motion/v1/motion.grpc.pb.h
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.h
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.h
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.h
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.h
     ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc
@@ -328,6 +332,8 @@ target_sources(viamapi
     ${PROTO_GEN_DIR}/service/mlmodel/v1/mlmodel.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.grpc.pb.cc
     ${PROTO_GEN_DIR}/service/motion/v1/motion.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.grpc.pb.cc
+    ${PROTO_GEN_DIR}/service/navigation/v1/navigation.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.grpc.pb.cc
     ${PROTO_GEN_DIR}/tagger/v1/tagger.pb.cc
   PUBLIC FILE_SET viamapi_includes TYPE HEADERS
@@ -385,6 +391,8 @@ target_sources(viamapi
       ${PROTO_GEN_DIR}/../../viam/api/service/mlmodel/v1/mlmodel.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/service/motion/v1/motion.grpc.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/service/motion/v1/motion.pb.h
+      ${PROTO_GEN_DIR}/../../viam/api/service/navigation/v1/navigation.grpc.pb.h
+      ${PROTO_GEN_DIR}/../../viam/api/service/navigation/v1/navigation.pb.h
       ${PROTO_GEN_DIR}/../../viam/api/tagger/v1/tagger.pb.h
 )
 

src/viam/sdk/CMakeLists.txt

@@ -121,13 +121,16 @@ target_sources(viamsdk
     services/generic.cpp
     services/mlmodel.cpp
     services/motion.cpp
+    services/navigation.cpp
     services/private/generic_client.cpp
     services/private/generic_server.cpp
     services/private/mlmodel.cpp
     services/private/mlmodel_client.cpp
     services/private/mlmodel_server.cpp
     services/private/motion_client.cpp
     services/private/motion_server.cpp
+    services/private/navigation_client.cpp
+    services/private/navigation_server.cpp
     services/service.cpp
     spatialmath/geometry.cpp
     spatialmath/orientation.cpp
@@ -181,6 +184,7 @@ target_sources(viamsdk
       ../../viam/sdk/services/generic.hpp
       ../../viam/sdk/services/mlmodel.hpp
       ../../viam/sdk/services/motion.hpp
+      ../../viam/sdk/services/navigation.hpp
       ../../viam/sdk/services/service.hpp
       ../../viam/sdk/spatialmath/geometry.hpp
       ../../viam/sdk/spatialmath/orientation.hpp

src/viam/sdk/common/private/proto_utils.hpp

@@ -0,0 +1,63 @@
+/// @file common/proto_utils.hpp
+///
+/// @brief Utils that require generated proto includes. These should be #included
+/// in cpp implementation files, but not in wrapper headers consumed by third party code.
+#pragma once
+
+#include <viam/api/common/v1/common.pb.h>
+
+namespace viam {
+namespace sdk {
+namespace impl {
+
+/// @brief Copies elements from a protobuf repeated pointer array into a std::vector. Src type
+/// must have a `to_proto` method.
+template <typename Src, typename Dst>
+void vecToRepeatedPtr(const std::vector<Src>& vec, google::protobuf::RepeatedPtrField<Dst>& dest) {
+    dest.Clear();
+    dest.Reserve(vec.size());
+    for (auto& x : vec) {
+        *dest.Add() = x.to_proto();
+    }
+}
+
+/// @brief Non-member to_proto() version. (necessary for moving generated types out of wrapper
+/// headers). Takes explicit `to_proto`.
+template <typename Src, typename Dst>
+void vecToRepeatedPtr(const std::vector<Src>& vec,
+                      google::protobuf::RepeatedPtrField<Dst>& dest,
+                      Dst to_proto(const Src&)) {
+    dest.Clear();
+    dest.Reserve(vec.size());
+    for (auto& x : vec) {
+        *dest.Add() = to_proto(x);
+    }
+}
+
+/// @brief Copies elements from a std::vector into a protobuf repeated pointer array. Dst type
+/// must have a `from_proto` static method.
+template <typename Src, typename Dst>
+void repeatedPtrToVec(const google::protobuf::RepeatedPtrField<Src>& src, std::vector<Dst>& vec) {
+    vec.clear();
+    vec.reserve(src.size());
+    for (auto& x : src) {
+        vec.push_back(Dst::from_proto(x));
+    }
+}
+
+/// @brief Non-member from_proto() version. (necessary for moving generated types out of wrapper
+/// headers). Takes explicit `from_proto`.
+template <typename Src, typename Dst>
+void repeatedPtrToVec(const google::protobuf::RepeatedPtrField<Src>& src,
+                      std::vector<Dst>& vec,
+                      Dst from_proto(const Src&)) {
+    vec.clear();
+    vec.reserve(src.size());
+    for (auto& x : src) {
+        vec.push_back(from_proto(x));
+    }
+}
+
+}  // namespace impl
+}  // namespace sdk
+}  // namespace viam

src/viam/sdk/components/arm.hpp

@@ -5,6 +5,7 @@
 
 #include <string>
 
+#include <boost/optional/optional.hpp>
 #include <boost/variant/variant.hpp>
 
 #include <viam/sdk/common/pose.hpp>
@@ -58,6 +59,12 @@ class Arm : public Component, public Stoppable {
     using KinematicsData =
         boost::variant<KinematicsDataUnspecified, KinematicsDataSVA, KinematicsDataURDF>;
 
+    /// @brief Movement specifications for move_through_join_positions.
+    struct MoveOptions {
+        boost::optional<double> max_vel_degs_per_sec;
+        boost::optional<double> max_acc_degs_per_sec2;
+    };
+
     /// @brief Get the current position of the end of the arm.
     /// @return The `pose` representing the end position of the arm.
     inline pose get_end_position() {
@@ -98,6 +105,22 @@ class Arm : public Component, public Stoppable {
     virtual void move_to_joint_positions(const std::vector<double>& positions,
                                          const ProtoStruct& extra) = 0;
 
+    /// @brief Move each joint on the arm through the positions specified in @param positions
+    /// @param options optional specifications to be obeyed during the motion.
+    /// TODO consider replacing vector vector with xtensor array, and also if it may be
+    /// possible to specify or constrain dimensionality of the array in advance.
+    inline void move_through_joint_positions(const std::vector<std::vector<double>>& positions,
+                                             const MoveOptions& options) {
+        return move_through_joint_positions(positions, options, {});
+    }
+
+    /// @brief Move each joint on the arm through the positions specified in @param positions
+    /// @param options optional specifications to be obeyed during the motion.
+    /// @param extra Any additional arguments to the method.
+    virtual void move_through_joint_positions(const std::vector<std::vector<double>>& positions,
+                                              const MoveOptions& options,
+                                              const ProtoStruct& extra) = 0;
+
     /// @brief Reports if the arm is in motion.
     virtual bool is_moving() = 0;
 

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

@@ -47,6 +47,31 @@ void ArmClient::move_to_joint_positions(const std::vector<double>& positions,
         .invoke();
 }
 
+void ArmClient::move_through_joint_positions(const std::vector<std::vector<double>>& positions,
+                                             const Arm::MoveOptions& options,
+                                             const ProtoStruct& extra) {
+    return make_client_helper(this, *stub_, &StubType::MoveThroughJointPositions)
+        .with(extra,
+              [&](viam::component::arm::v1::MoveThroughJointPositionsRequest& request) {
+                  if (options.max_vel_degs_per_sec) {
+                      request.mutable_options()->set_max_vel_degs_per_sec(
+                          *options.max_vel_degs_per_sec);
+                  }
+
+                  if (options.max_acc_degs_per_sec2) {
+                      request.mutable_options()->set_max_acc_degs_per_sec2(
+                          *options.max_acc_degs_per_sec2);
+                  }
+
+                  for (const auto& pos : positions) {
+                      viam::component::arm::v1::JointPositions jpos;
+                      jpos.mutable_values()->Add(pos.begin(), pos.end());
+                      request.mutable_positions()->Add(std::move(jpos));
+                  }
+              })
+        .invoke();
+}
+
 bool ArmClient::is_moving() {
     return make_client_helper(this, *stub_, &StubType::IsMoving).invoke([](auto& response) {
         return response.is_moving();

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

@@ -26,6 +26,9 @@ class ArmClient : public Arm {
     std::vector<double> get_joint_positions(const ProtoStruct& extra) override;
     void move_to_joint_positions(const std::vector<double>& positions,
                                  const ProtoStruct& extra) override;
+    void move_through_joint_positions(const std::vector<std::vector<double>>& positions,
+                                      const Arm::MoveOptions& options,
+                                      const ProtoStruct& extra) override;
     bool is_moving() override;
     void stop(const ProtoStruct& extra) override;
     ProtoStruct do_command(const ProtoStruct& command) override;
@@ -38,6 +41,7 @@ class ArmClient : public Arm {
     using Arm::get_geometries;
     using Arm::get_joint_positions;
     using Arm::get_kinematics;
+    using Arm::move_through_joint_positions;
     using Arm::move_to_joint_positions;
     using Arm::move_to_position;
     using Arm::stop;

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

@@ -54,6 +54,32 @@ ::grpc::Status ArmServer::MoveToJointPositions(
     });
 }
 
+::grpc::Status ArmServer::MoveThroughJointPositions(
+    ::grpc::ServerContext*,
+    const ::viam::component::arm::v1::MoveThroughJointPositionsRequest* request,
+    ::viam::component::arm::v1::MoveThroughJointPositionsResponse*) noexcept {
+    return make_service_helper<Arm>(
+        "ArmServer::MoveThroughJointPositions", this, request)([&](auto& helper, auto& arm) {
+        std::vector<std::vector<double>> positions;
+
+        positions.reserve(request->positions_size());
+        for (const auto& values : request->positions()) {
+            positions.emplace_back(values.values().begin(), values.values().end());
+        }
+
+        Arm::MoveOptions opts;
+        if (request->options().has_max_vel_degs_per_sec()) {
+            opts.max_vel_degs_per_sec = request->options().max_vel_degs_per_sec();
+        }
+
+        if (request->options().has_max_acc_degs_per_sec2()) {
+            opts.max_acc_degs_per_sec2 = request->options().max_acc_degs_per_sec2();
+        }
+
+        arm->move_through_joint_positions(positions, opts, helper.getExtra());
+    });
+}
+
 ::grpc::Status ArmServer::Stop(::grpc::ServerContext*,
                                const ::viam::component::arm::v1::StopRequest* request,
                                ::viam::component::arm::v1::StopResponse*) noexcept {

src/viam/sdk/components/private/arm_server.hpp

@@ -44,6 +44,11 @@ class ArmServer : public ResourceServer, public viam::component::arm::v1::ArmSer
         const ::viam::component::arm::v1::MoveToJointPositionsRequest* request,
         ::viam::component::arm::v1::MoveToJointPositionsResponse* response) noexcept override;
 
+    ::grpc::Status MoveThroughJointPositions(
+        ::grpc::ServerContext* context,
+        const ::viam::component::arm::v1::MoveThroughJointPositionsRequest* request,
+        ::viam::component::arm::v1::MoveThroughJointPositionsResponse* response) noexcept override;
+
     ::grpc::Status Stop(::grpc::ServerContext* context,
                         const ::viam::component::arm::v1::StopRequest* request,
                         ::viam::component::arm::v1::StopResponse* response) noexcept override;

src/viam/sdk/registry/registry.cpp

@@ -47,6 +47,8 @@
 #include <viam/sdk/services/private/mlmodel_server.hpp>
 #include <viam/sdk/services/private/motion_client.hpp>
 #include <viam/sdk/services/private/motion_server.hpp>
+#include <viam/sdk/services/private/navigation_client.hpp>
+#include <viam/sdk/services/private/navigation_server.hpp>
 #include <viam/sdk/services/service.hpp>
 
 namespace viam {
@@ -179,6 +181,7 @@ void register_resources() {
     Registry::register_resource<impl::GenericServiceClient, impl::GenericServiceServer>();
     Registry::register_resource<impl::MLModelServiceClient, impl::MLModelServiceServer>();
     Registry::register_resource<impl::MotionClient, impl::MotionServer>();
+    Registry::register_resource<impl::NavigationClient, impl::NavigationServer>();
 }
 
 void Registry::initialize() {