RSDK-5233 - implement missing methods (#181)

src/viam/sdk/common/utils.cpp

@@ -22,6 +22,7 @@ namespace viam {
 namespace sdk {
 
 using viam::common::v1::ResourceName;
+using time_point = std::chrono::time_point<long long, std::chrono::nanoseconds>;
 
 std::vector<ResourceName> resource_names_for_resource(const std::shared_ptr<Resource>& resource) {
     std::string resource_type;
@@ -60,16 +61,14 @@ std::string bytes_to_string(const std::vector<unsigned char>& b) {
     return img_string;
 };
 
-std::chrono::time_point<long long, std::chrono::nanoseconds> timestamp_to_time_pt(
-    const google::protobuf::Timestamp& timestamp) {
+time_point timestamp_to_time_pt(const google::protobuf::Timestamp& timestamp) {
     const std::chrono::seconds seconds(timestamp.seconds());
     const std::chrono::nanoseconds nanos(timestamp.nanos());
-    return std::chrono::time_point<long long, std::chrono::nanoseconds>(
-        std::chrono::duration_cast<std::chrono::system_clock::duration>(seconds) + nanos);
+    return time_point(std::chrono::duration_cast<std::chrono::system_clock::duration>(seconds) +
+                      nanos);
 }
 
-google::protobuf::Timestamp time_pt_to_timestamp(
-    const std::chrono::time_point<long long, std::chrono::nanoseconds>& time_pt) {
+google::protobuf::Timestamp time_pt_to_timestamp(const time_point& time_pt) {
     const std::chrono::seconds duration_s =
         std::chrono::duration_cast<std::chrono::seconds>(time_pt.time_since_epoch());
     const std::chrono::nanoseconds duration_ns = time_pt.time_since_epoch() - duration_s;

src/viam/sdk/common/utils.hpp

@@ -51,13 +51,13 @@ struct response_metadata {
 
 bool operator==(const response_metadata& lhs, const response_metadata& rhs);
 
-/// @brief convert a google::protobuf::Timestamp to std::chrono::time_point<long long,
-/// std::chrono::nanoseconds>.
+/// @brief convert a google::protobuf::Timestamp to
+/// std::chrono::time_point<long long, std::chrono::nanoseconds>
 std::chrono::time_point<long long, std::chrono::nanoseconds> timestamp_to_time_pt(
     const google::protobuf::Timestamp& timestamp);
 
-/// @brief convert a std::chrono::time_point<long long, std::chrono::nanoseconds> to a
-/// google::protobuf::Timestamp.
+/// @brief convert a std::chrono::time_point<long long, std::chrono::nanoseconds> to
+/// a google::protobuf::Timestamp.
 google::protobuf::Timestamp time_pt_to_timestamp(
     const std::chrono::time_point<long long, std::chrono::nanoseconds>& time_pt);
 

src/viam/sdk/components/board/board.hpp

@@ -213,8 +213,21 @@ class Board : public Component {
     virtual analog_value read_analog(const std::string& analog_reader_name,
                                      const AttributeMap& extra) = 0;
 
-    /// @brief Returns the current value of the interrupt which is based on the type of interrupt.
-    /// Consult Viam's `Board` docs for more information.
+    /// @brief Writes the value to the analog writer of the board.
+    /// @param pin the pin to write to
+    /// @param value the value to set the pin to
+    inline void write_analog(const std::string& pin, int value) {
+        return write_analog(pin, value, {});
+    }
+
+    /// @brief Writes the value to the analog writer of the board.
+    /// @param pin the pin to write to
+    /// @param value the value to set the pin to
+    /// @param extra any additional arguments to the method
+    virtual void write_analog(const std::string& pin, int value, const AttributeMap& extra) = 0;
+
+    /// @brief Returns the current value of the interrupt which is based on the type of
+    /// interrupt. Consult Viam's `Board` docs for more information.
     /// @param digital_interrupt_name digital interrupt to check
     inline digital_value read_digital_interrupt(const std::string& digital_interrupt_name) {
         return read_digital_interrupt(digital_interrupt_name, {});

src/viam/sdk/components/board/client.cpp

@@ -187,6 +187,24 @@ Board::analog_value BoardClient::read_analog(const std::string& analog_reader_na
     return response.value();
 }
 
+void BoardClient::write_analog(const std::string& pin, int value, const AttributeMap& extra) {
+    component::board::v1::WriteAnalogRequest request;
+    component::board::v1::WriteAnalogResponse response;
+
+    grpc::ClientContext ctx;
+    set_client_ctx_authority(ctx);
+
+    request.set_name(this->name());
+    request.set_pin(pin);
+    request.set_value(value);
+    *request.mutable_extra() = map_to_struct(extra);
+
+    const grpc::Status status = stub_->WriteAnalog(&ctx, request, &response);
+    if (!status.ok()) {
+        throw std::runtime_error(status.error_message());
+    }
+}
+
 Board::digital_value BoardClient::read_digital_interrupt(const std::string& digital_interrupt_name,
                                                          const AttributeMap& extra) {
     viam::component::board::v1::GetDigitalInterruptValueRequest request;

src/viam/sdk/components/board/client.hpp

@@ -35,6 +35,7 @@ class BoardClient : public Board {
                            const AttributeMap& extra) override;
     analog_value read_analog(const std::string& analog_reader_name,
                              const AttributeMap& extra) override;
+    void write_analog(const std::string& pin, int value, const AttributeMap& extra) override;
     digital_value read_digital_interrupt(const std::string& digital_interrupt_name,
                                          const AttributeMap& extra) override;
     void set_power_mode(power_mode power_mode,
@@ -62,6 +63,7 @@ class BoardClient : public Board {
     using Board::set_power_mode;
     using Board::set_pwm_duty_cycle;
     using Board::set_pwm_frequency;
+    using Board::write_analog;
 
    private:
     std::unique_ptr<viam::component::board::v1::BoardService::StubInterface> stub_;

src/viam/sdk/components/board/server.cpp

@@ -240,6 +240,31 @@ ::grpc::Status BoardServer::ReadAnalogReader(
     return ::grpc::Status();
 }
 
+::grpc::Status BoardServer::WriteAnalog(
+    ::grpc::ServerContext* context,
+    const ::viam::component::board::v1::WriteAnalogRequest* request,
+    ::viam::component::board::v1::WriteAnalogResponse* response) {
+    if (!request) {
+        return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT,
+                              "Called [Board::WriteAnalog] without a request");
+    };
+
+    const std::shared_ptr<Resource> rb = resource_manager()->resource(request->name());
+    if (!rb) {
+        return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name());
+    }
+
+    const std::shared_ptr<Board> board = std::dynamic_pointer_cast<Board>(rb);
+
+    AttributeMap extra;
+    if (request->has_extra()) {
+        extra = struct_to_map(request->extra());
+    }
+
+    board->write_analog(request->pin(), request->value(), extra);
+    return ::grpc::Status();
+}
+
 ::grpc::Status BoardServer::GetDigitalInterruptValue(
     ::grpc::ServerContext* context,
     const ::viam::component::board::v1::GetDigitalInterruptValueRequest* request,

src/viam/sdk/components/board/server.hpp

@@ -60,6 +60,10 @@ class BoardServer : public ResourceServer,
         const ::viam::component::board::v1::ReadAnalogReaderRequest* request,
         ::viam::component::board::v1::ReadAnalogReaderResponse* response) override;
 
+    ::grpc::Status WriteAnalog(::grpc::ServerContext* context,
+                               const component::board::v1::WriteAnalogRequest* request,
+                               component::board::v1::WriteAnalogResponse* response) override;
+
     ::grpc::Status GetDigitalInterruptValue(
         ::grpc::ServerContext* context,
         const ::viam::component::board::v1::GetDigitalInterruptValueRequest* request,

src/viam/sdk/resource/resource_api.cpp

@@ -97,7 +97,7 @@ const std::string& Name::remote_name() const {
 }
 
 std::string Name::to_string() const {
-    if (remote_name_.empty()) {
+    if (remote_name_.empty() || remote_name_ == ":") {
         return api_.to_string() + "/" + name_;
     }
     return api_.to_string() + "/" + remote_name_ + ":" + name_;

src/viam/sdk/services/motion/client.cpp

@@ -135,6 +135,104 @@ pose_in_frame MotionClient::get_pose(
     return pose_in_frame::from_proto(response.pose());
 }
 
+void MotionClient::stop_plan(const Name& name, const AttributeMap& extra) {
+    service::motion::v1::StopPlanRequest request;
+    service::motion::v1::StopPlanResponse response;
+    grpc::ClientContext ctx;
+    set_client_ctx_authority(ctx);
+
+    *request.mutable_name() = this->name();
+    *request.mutable_extra() = map_to_struct(extra);
+
+    const grpc::Status status = stub_->StopPlan(&ctx, request, &response);
+    if (!status.ok()) {
+        throw std::runtime_error(status.error_message());
+    }
+}
+
+std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>> MotionClient::get_plan_(
+    const Name& name,
+    boost::optional<std::string> execution_id,
+    bool last_plan_only,
+    const AttributeMap& extra) {
+    service::motion::v1::GetPlanRequest request;
+    service::motion::v1::GetPlanResponse response;
+    grpc::ClientContext ctx;
+    set_client_ctx_authority(ctx);
+
+    *request.mutable_name() = this->name();
+    *request.mutable_extra() = map_to_struct(extra);
+    request.set_last_plan_only(last_plan_only);
+    if (execution_id) {
+        *request.mutable_execution_id() = *execution_id;
+    }
+
+    const grpc::Status status = stub_->GetPlan(&ctx, request, &response);
+    if (!status.ok()) {
+        throw std::runtime_error(status.error_message());
+    }
+
+    return {Motion::plan_with_status::from_proto(response.current_plan_with_status()),
+            Motion::plan_with_status::from_proto(response.replan_history())};
+}
+
+Motion::plan_with_status MotionClient::get_plan(const Name& name,
+                                                const std::string& execution_id,
+                                                const AttributeMap& extra) {
+    return get_plan_(name, execution_id, true, extra).first;
+}
+
+Motion::plan_with_status MotionClient::get_latest_plan(const Name& name,
+                                                       const AttributeMap& extra) {
+    return get_plan_(name, {}, true, extra).first;
+}
+
+std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>>
+MotionClient::get_plan_with_replan_history(const Name& name,
+                                           const std::string& execution_id,
+                                           const AttributeMap& extra) {
+    return get_plan_(name, execution_id, false, extra);
+}
+
+std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>>
+MotionClient::get_latest_plan_with_replan_history(const Name& name, const AttributeMap& extra) {
+    return get_plan_(name, {}, false, extra);
+}
+
+std::vector<Motion::plan_status_with_id> MotionClient::list_plan_statuses_(
+    bool only_active_plans, const AttributeMap& extra) {
+    service::motion::v1::ListPlanStatusesRequest request;
+    service::motion::v1::ListPlanStatusesResponse response;
+    grpc::ClientContext ctx;
+    set_client_ctx_authority(ctx);
+
+    *request.mutable_name() = this->name();
+    *request.mutable_extra() = map_to_struct(extra);
+    request.set_only_active_plans(only_active_plans);
+
+    const grpc::Status status = stub_->ListPlanStatuses(&ctx, request, &response);
+    if (!status.ok()) {
+        throw std::runtime_error(status.error_message());
+    }
+
+    std::vector<Motion::plan_status_with_id> statuses;
+    for (const auto& proto : response.plan_statuses_with_ids()) {
+        statuses.push_back(Motion::plan_status_with_id::from_proto(proto));
+    }
+
+    return statuses;
+}
+
+std::vector<Motion::plan_status_with_id> MotionClient::list_plan_statuses(
+    const AttributeMap& extra) {
+    return list_plan_statuses_(false, extra);
+}
+
+std::vector<Motion::plan_status_with_id> MotionClient::list_active_plan_statuses(
+    const AttributeMap& extra) {
+    return list_plan_statuses_(true, extra);
+}
+
 AttributeMap MotionClient::do_command(const AttributeMap& command) {
     viam::common::v1::DoCommandRequest request;
     viam::common::v1::DoCommandResponse response;

src/viam/sdk/services/motion/client.hpp

@@ -40,6 +40,30 @@ class MotionClient : public Motion {
                            const std::vector<WorldState::transform>& supplemental_transforms,
                            const AttributeMap& extra) override;
 
+    void stop_plan(const Name& component_name, const AttributeMap& extra) override;
+
+    Motion::plan_with_status get_latest_plan(const Name& component_name,
+                                             const AttributeMap& extra) override;
+
+    std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>>
+
+    get_latest_plan_with_replan_history(const Name& component_name,
+                                        const AttributeMap& extra) override;
+
+    Motion::plan_with_status get_plan(const Name& component_name,
+                                      const std::string& execution_id,
+                                      const AttributeMap& extra) override;
+
+    std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>>
+    get_plan_with_replan_history(const Name& component_name,
+                                 const std::string& execution_id,
+                                 const AttributeMap& extra) override;
+
+    std::vector<Motion::plan_status_with_id> list_active_plan_statuses(
+        const AttributeMap& extra) override;
+
+    std::vector<Motion::plan_status_with_id> list_plan_statuses(const AttributeMap& extra) override;
+
     AttributeMap do_command(const AttributeMap& command) override;
 
     // the `extra` param is frequently unnecessary but needs to be supported. Ideally, we'd
@@ -51,12 +75,26 @@ class MotionClient : public Motion {
     // that calls the virtual method and passes a `nullptr` by default in place of the `extra`
     // param. In order to access these versions of the methods within the client code, however,
     // we need to include these `using` lines.
+    using Motion::get_latest_plan;
+    using Motion::get_latest_plan_with_replan_history;
+    using Motion::get_plan;
+    using Motion::get_plan_with_replan_history;
     using Motion::get_pose;
+    using Motion::list_active_plan_statuses;
+    using Motion::list_plan_statuses;
     using Motion::move;
     using Motion::move_on_globe;
     using Motion::move_on_map;
+    using Motion::stop_plan;
 
    private:
+    std::vector<Motion::plan_status_with_id> list_plan_statuses_(bool only_active_plans,
+                                                                 const AttributeMap& extra);
+    std::pair<Motion::plan_with_status, std::vector<Motion::plan_with_status>> get_plan_(
+        const Name& component_name,
+        boost::optional<std::string> execution_id,
+        bool last_plan_only,
+        const AttributeMap& extra);
     std::unique_ptr<service::motion::v1::MotionService::StubInterface> stub_;
     std::shared_ptr<grpc::Channel> channel_;
 };