diff --git a/src/viam/sdk/common/utils.cpp b/src/viam/sdk/common/utils.cpp index b394e6ac5..8c96663d5 100644 --- a/src/viam/sdk/common/utils.cpp +++ b/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; std::vector resource_names_for_resource(const std::shared_ptr& resource) { std::string resource_type; @@ -60,16 +61,14 @@ std::string bytes_to_string(const std::vector& b) { return img_string; }; -std::chrono::time_point 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( - std::chrono::duration_cast(seconds) + nanos); + return time_point(std::chrono::duration_cast(seconds) + + nanos); } -google::protobuf::Timestamp time_pt_to_timestamp( - const std::chrono::time_point& time_pt) { +google::protobuf::Timestamp time_pt_to_timestamp(const time_point& time_pt) { const std::chrono::seconds duration_s = std::chrono::duration_cast(time_pt.time_since_epoch()); const std::chrono::nanoseconds duration_ns = time_pt.time_since_epoch() - duration_s; diff --git a/src/viam/sdk/common/utils.hpp b/src/viam/sdk/common/utils.hpp index 1a2d6cbaa..a1a9991fa 100644 --- a/src/viam/sdk/common/utils.hpp +++ b/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. +/// @brief convert a google::protobuf::Timestamp to +/// std::chrono::time_point std::chrono::time_point timestamp_to_time_pt( const google::protobuf::Timestamp& timestamp); -/// @brief convert a std::chrono::time_point to a -/// google::protobuf::Timestamp. +/// @brief convert a std::chrono::time_point to +/// a google::protobuf::Timestamp. google::protobuf::Timestamp time_pt_to_timestamp( const std::chrono::time_point& time_pt); diff --git a/src/viam/sdk/components/board/board.hpp b/src/viam/sdk/components/board/board.hpp index 01c59658e..da97625a2 100644 --- a/src/viam/sdk/components/board/board.hpp +++ b/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, {}); diff --git a/src/viam/sdk/components/board/client.cpp b/src/viam/sdk/components/board/client.cpp index e6d16cc1a..6168d2e45 100644 --- a/src/viam/sdk/components/board/client.cpp +++ b/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; diff --git a/src/viam/sdk/components/board/client.hpp b/src/viam/sdk/components/board/client.hpp index 91526bc3b..569c2326d 100644 --- a/src/viam/sdk/components/board/client.hpp +++ b/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 stub_; diff --git a/src/viam/sdk/components/board/server.cpp b/src/viam/sdk/components/board/server.cpp index 2d6c6c978..86ce21fa8 100644 --- a/src/viam/sdk/components/board/server.cpp +++ b/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 rb = resource_manager()->resource(request->name()); + if (!rb) { + return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name()); + } + + const std::shared_ptr board = std::dynamic_pointer_cast(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, diff --git a/src/viam/sdk/components/board/server.hpp b/src/viam/sdk/components/board/server.hpp index d8ca74e2f..93ca00d07 100644 --- a/src/viam/sdk/components/board/server.hpp +++ b/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, diff --git a/src/viam/sdk/resource/resource_api.cpp b/src/viam/sdk/resource/resource_api.cpp index 908ec2a25..28433e5b8 100644 --- a/src/viam/sdk/resource/resource_api.cpp +++ b/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_; diff --git a/src/viam/sdk/services/motion/client.cpp b/src/viam/sdk/services/motion/client.cpp index 3d183cc9f..eb29146ed 100644 --- a/src/viam/sdk/services/motion/client.cpp +++ b/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> MotionClient::get_plan_( + const Name& name, + boost::optional 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> +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> +MotionClient::get_latest_plan_with_replan_history(const Name& name, const AttributeMap& extra) { + return get_plan_(name, {}, false, extra); +} + +std::vector 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 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 MotionClient::list_plan_statuses( + const AttributeMap& extra) { + return list_plan_statuses_(false, extra); +} + +std::vector 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; diff --git a/src/viam/sdk/services/motion/client.hpp b/src/viam/sdk/services/motion/client.hpp index 949c18cee..cb6354b66 100644 --- a/src/viam/sdk/services/motion/client.hpp +++ b/src/viam/sdk/services/motion/client.hpp @@ -40,6 +40,30 @@ class MotionClient : public Motion { const std::vector& 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> + + 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> + get_plan_with_replan_history(const Name& component_name, + const std::string& execution_id, + const AttributeMap& extra) override; + + std::vector list_active_plan_statuses( + const AttributeMap& extra) override; + + std::vector 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 list_plan_statuses_(bool only_active_plans, + const AttributeMap& extra); + std::pair> get_plan_( + const Name& component_name, + boost::optional execution_id, + bool last_plan_only, + const AttributeMap& extra); std::unique_ptr stub_; std::shared_ptr channel_; }; diff --git a/src/viam/sdk/services/motion/motion.cpp b/src/viam/sdk/services/motion/motion.cpp index a4bbf22e2..d6d08590e 100644 --- a/src/viam/sdk/services/motion/motion.cpp +++ b/src/viam/sdk/services/motion/motion.cpp @@ -133,6 +133,30 @@ bool operator==(const obstacle_detector& lhs, const obstacle_detector& rhs) { return lhs.vision_service == rhs.vision_service && lhs.camera == rhs.camera; } +bool operator==(const Motion::steps& lhs, const Motion::steps& rhs) { + return lhs.steps == rhs.steps; +} + +bool operator==(const Motion::plan_status& lhs, const Motion::plan_status& rhs) { + return std::tie(lhs.reason, lhs.state, lhs.timestamp) == + std::tie(rhs.reason, rhs.state, rhs.timestamp); +} + +bool operator==(const Motion::plan_status_with_id& lhs, const Motion::plan_status_with_id& rhs) { + return std::tie(lhs.execution_id, lhs.component_name, lhs.status, lhs.plan_id) == + std::tie(rhs.execution_id, rhs.component_name, rhs.status, rhs.plan_id); +} + +bool operator==(const Motion::plan& lhs, const Motion::plan& rhs) { + return std::tie(lhs.component_name, lhs.execution_id, lhs.steps, lhs.id) == + std::tie(rhs.component_name, rhs.execution_id, rhs.steps, rhs.id); +} + +bool operator==(const Motion::plan_with_status& lhs, const Motion::plan_with_status& rhs) { + return std::tie(lhs.plan, lhs.status, lhs.status_history) == + std::tie(rhs.plan, rhs.status, rhs.status_history); +} + std::ostream& operator<<(std::ostream& os, const obstacle_detector& v) { os << "{ "; os << "\tvision_service: " << v.vision_service << std::endl; @@ -240,6 +264,175 @@ std::ostream& operator<<(std::ostream& os, const motion_configuration& v) { return os; } +Motion::plan_state Motion::from_proto(const service::motion::v1::PlanState& proto) { + switch (proto) { + case service::motion::v1::PLAN_STATE_FAILED: { + return Motion::plan_state::k_failed; + } + case service::motion::v1::PLAN_STATE_SUCCEEDED: { + return Motion::plan_state::k_succeeded; + } + case service::motion::v1::PLAN_STATE_IN_PROGRESS: { + return Motion::plan_state::k_in_progress; + } + case service::motion::v1::PLAN_STATE_STOPPED: { + return Motion::plan_state::k_stopped; + } + default: { + throw std::runtime_error("Invalid proto PlanState to encode"); + } + } +} + +service::motion::v1::PlanState Motion::to_proto(const Motion::plan_state& state) { + switch (state) { + case Motion::plan_state::k_failed: { + return service::motion::v1::PLAN_STATE_FAILED; + } + case Motion::plan_state::k_succeeded: { + return service::motion::v1::PLAN_STATE_SUCCEEDED; + } + case Motion::plan_state::k_in_progress: { + return service::motion::v1::PLAN_STATE_IN_PROGRESS; + } + case Motion::plan_state::k_stopped: { + return service::motion::v1::PLAN_STATE_STOPPED; + } + default: { + throw std::runtime_error("Invalid plan_state to encode to proto"); + } + } +} + +Motion::plan_status Motion::plan_status::from_proto(const service::motion::v1::PlanStatus& proto) { + plan_status mps; + mps.state = Motion::from_proto(proto.state()); + if (proto.has_reason()) { + mps.reason = proto.reason(); + } + mps.timestamp = timestamp_to_time_pt(proto.timestamp()); + + return mps; +} + +std::vector Motion::plan_status::from_proto( + const google::protobuf::RepeatedPtrField& proto) { + std::vector pss; + for (const auto& ps : proto) { + pss.push_back(Motion::plan_status::from_proto(ps)); + } + + return pss; +} +service::motion::v1::PlanStatus Motion::plan_status::to_proto() const { + service::motion::v1::PlanStatus proto; + *proto.mutable_timestamp() = time_pt_to_timestamp(timestamp); + if (reason) { + *proto.mutable_reason() = *reason; + } + proto.set_state(Motion::to_proto(state)); + + return proto; +} + +Motion::steps Motion::steps::from_proto( + const google::protobuf::RepeatedPtrField& proto) { + std::vector steps; + for (const auto& ps : proto) { + step step; + for (const auto& component : ps.step()) { + step.emplace(component.first, pose::from_proto(component.second.pose())); + } + steps.push_back(std::move(step)); + } + + return {steps}; +} + +service::motion::v1::PlanStep Motion::steps::to_proto(const Motion::steps::step& step) { + service::motion::v1::PlanStep proto; + for (const auto& kv : step) { + service::motion::v1::ComponentState cs; + *cs.mutable_pose() = kv.second.to_proto(); + proto.mutable_step()->insert({kv.first, cs}); + } + + return proto; +} + +Motion::plan Motion::plan::from_proto(const service::motion::v1::Plan& proto) { + Motion::plan plan; + plan.id = proto.id(); + plan.execution_id = proto.execution_id(); + plan.component_name = Name::from_proto(proto.component_name()); + plan.steps = Motion::steps::from_proto(proto.steps()); + return plan; +} + +service::motion::v1::Plan Motion::plan::to_proto() const { + service::motion::v1::Plan proto; + *proto.mutable_id() = id; + *proto.mutable_component_name() = component_name.to_proto(); + *proto.mutable_execution_id() = execution_id; + for (const auto& step : steps.steps) { + *proto.mutable_steps()->Add() = Motion::steps::to_proto(step); + } + + return proto; +} + +Motion::plan_with_status Motion::plan_with_status::from_proto( + const service::motion::v1::PlanWithStatus& proto) { + Motion::plan_with_status pws; + pws.plan = Motion::plan::from_proto(proto.plan()); + pws.status = Motion::plan_status::from_proto(proto.status()); + pws.status_history = Motion::plan_status::from_proto(proto.status_history()); + + return pws; +} + +std::vector Motion::plan_with_status::from_proto( + const google::protobuf::RepeatedPtrField& proto) { + std::vector plans; + for (const auto& plan : proto) { + plans.push_back(Motion::plan_with_status::from_proto(plan)); + } + return plans; +} + +service::motion::v1::PlanWithStatus Motion::plan_with_status::to_proto() const { + service::motion::v1::PlanWithStatus proto; + *proto.mutable_plan() = plan.to_proto(); + *proto.mutable_status() = status.to_proto(); + for (const auto& sh : status_history) { + *proto.mutable_status_history()->Add() = sh.to_proto(); + } + + return proto; +} + +Motion::plan_status_with_id Motion::plan_status_with_id::from_proto( + const service::motion::v1::PlanStatusWithID& proto) { + Motion::plan_status_with_id pswi; + pswi.execution_id = proto.execution_id(); + pswi.component_name = Name::from_proto(proto.component_name()); + pswi.plan_id = proto.plan_id(); + pswi.status = Motion::plan_status::from_proto(proto.status()); + + return pswi; +} + +service::motion::v1::PlanStatusWithID Motion::plan_status_with_id::to_proto() const { + service::motion::v1::PlanStatusWithID proto; + + *proto.mutable_execution_id() = execution_id; + *proto.mutable_component_name() = component_name.to_proto(); + *proto.mutable_plan_id() = plan_id; + *proto.mutable_status() = status.to_proto(); + + return proto; +} + namespace { bool init() { Registry::register_resource(Motion::static_api(), Motion::resource_registration()); diff --git a/src/viam/sdk/services/motion/motion.hpp b/src/viam/sdk/services/motion/motion.hpp index ee0f2e456..149dd8c89 100644 --- a/src/viam/sdk/services/motion/motion.hpp +++ b/src/viam/sdk/services/motion/motion.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -87,6 +88,122 @@ struct motion_configuration { /// specific motion implementations. This class cannot be used on its own. class Motion : public Service { public: + /// @enum plan_state + /// @brief Describes the possible states a plan can be in. + /// @ingroup Motion + enum class plan_state : uint8_t { + k_in_progress, + k_stopped, + k_succeeded, + k_failed, + }; + + static plan_state from_proto(const service::motion::v1::PlanState& proto); + static service::motion::v1::PlanState to_proto(const plan_state& state); + + /// @struct plan_status + /// @brief Describes the state of a given plan at a point in time. + /// @ingroup Motion + struct plan_status { + /// @brief The state of the plan execution + plan_state state; + + /// @brief The time the executing plan transitioned to the state. + std::chrono::time_point timestamp; + + /// @brief The reason for the state change. The error message if the plan failed, or the + /// re-plan reason if re-planning was necessary. + boost::optional reason; + + static plan_status from_proto(const service::motion::v1::PlanStatus& proto); + static std::vector from_proto( + const google::protobuf::RepeatedPtrField& proto); + service::motion::v1::PlanStatus to_proto() const; + friend bool operator==(const plan_status& lhs, const plan_status& rhs); + }; + + /// @struct plan_status_with_id + /// @brief The motion plan status, plus plan ID, component name, and execution ID. + /// @ingroup Motion + struct plan_status_with_id { + /// @brief The unique ID of the plan + std::string plan_id; + + /// @brief The component to be moved. Used for tracking and stopping. + Name component_name; + + /// @brief The unique ID which identifies the plan execution. + std::string execution_id; + + /// @brief The plan status. + plan_status status; + + static plan_status_with_id from_proto(const service::motion::v1::PlanStatusWithID& proto); + service::motion::v1::PlanStatusWithID to_proto() const; + friend bool operator==(const plan_status_with_id& lhs, const plan_status_with_id& rhs); + }; + + /// @struct steps + /// @brief An ordered list of plan steps. + /// @ingroup Motion + struct steps { + /// @brief An individual "step", representing the state each component (keyed as a fully + /// qualified component name) should reach while executing that step of the plan. + typedef std::unordered_map step; + + /// @brief The ordered list of steps. + std::vector steps; + + static struct steps from_proto( + const google::protobuf::RepeatedPtrField& proto); + + static service::motion::v1::PlanStep to_proto(const step& step); + friend bool operator==(const struct steps& lhs, const struct steps& rhs); + }; + + /// @struct plan + /// @brief Describes a motion plan. + /// @ingroup Motion + struct plan { + /// @brief The plan's unique ID. + std::string id; + + /// @brief The component requested to be moved. Used for tracking and stopping. + Name component_name; + + /// @brief The unique ID which identifies the execution. + /// Multiple plans can share the same execution_id if they were generated due to replanning. + std::string execution_id; + + /// @brief An ordered list of plan steps. + struct steps steps; + + static plan from_proto(const service::motion::v1::Plan& proto); + service::motion::v1::Plan to_proto() const; + friend bool operator==(const plan& lhs, const plan& rhs); + }; + + /// @struct plan_with_status + /// @brief Describes a plan, its current status, and all status changes that have occurred + /// previously on that plan. + /// @ingroup Motion + struct plan_with_status { + /// @brief The plan. + struct plan plan; + + /// @brief The current status of the plan. + plan_status status; + + /// @brief The prior status changes that have happened during plan execution. + std::vector status_history; + + static plan_with_status from_proto(const service::motion::v1::PlanWithStatus& proto); + static std::vector from_proto( + const google::protobuf::RepeatedPtrField& proto); + service::motion::v1::PlanWithStatus to_proto() const; + friend bool operator==(const plan_with_status& lhs, const plan_with_status& rhs); + }; + /// @struct linear_constraint /// @brief Specifies that the component being moved should move linearly to its goal. struct linear_constraint { @@ -247,6 +364,131 @@ class Motion : public Service { const std::vector& supplemental_transforms, const AttributeMap& extra) = 0; + /// @brief Stop a currently executing motion plan. + /// @param component_name the component of the currently executing plan to stop. + inline void stop_plan(const Name& component_name) { + return stop_plan(component_name, {}); + } + + /// @brief Stop a currently executing motion plan. + /// @param component_name the component of the currently executing plan to stop. + /// @param extra Any additional arguments to the method. + virtual void stop_plan(const Name& component_name, const AttributeMap& extra) = 0; + + /// @brief Returns the plan and state history of the most recent execution to move a component. + /// Returns a result if the last execution is still executing, or changed state within the last + /// 24 hours without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @return the plan and status of the most recent execution to move the requested component + inline plan_with_status get_latest_plan(const Name& component_name) { + return get_latest_plan(component_name, {}); + } + + /// @brief Returns the plan and state history of the most recent execution to move a component. + /// Returns a result if the last execution is still executing, or changed state within the last + /// 24 hours without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param extra Any additional arguments to the method. + /// @return the plan and status of the most recent execution to move the requested component + virtual plan_with_status get_latest_plan(const Name& component_name, + const AttributeMap& extra) = 0; + + /// @brief Returns the plan, state history, and replan history of the most recent execution to + /// move a component. Returns a result if the last execution is still executing, or changed + /// state within the last 24 hours without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @return a pair of (1) the plan and status and (2) the replan history of the most recent + /// execution to move the requested component + inline std::pair> + get_latest_plan_with_replan_history(Name component_name) { + return get_latest_plan_with_replan_history(component_name, {}); + } + + /// @brief Returns the plan, state history, and replan history of the most recent execution to + /// move a component. Returns a result if the last execution is still executing, or changed + /// state within the last 24 hours without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param extra Any additional arguments to the method. + /// @return a pair of (1) the plan and status and (2) the replan history of the most recent + /// execution to move the requested component + virtual std::pair> + get_latest_plan_with_replan_history(const Name& component_name, const AttributeMap& extra) = 0; + + /// @brief Returns the plan and state history of the requested plan. Returns a result + /// if the last execution is still executing, or changed state within the last 24 hours + /// without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param execution_id The execution id of the requested plan. + /// @return the plan and status of the requested execution's move the requested component + inline plan_with_status get_plan(Name component_name, const std::string& execution_id) { + return get_plan(component_name, execution_id, {}); + } + + /// @brief Returns the plan and state history of the requested plan. Returns a result + /// if the last execution is still executing, or changed state within the last 24 hours + /// without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param execution_id The execution id of the requested plan. + /// @param extra Any additional arguments to the method. + /// @return the plan and status of the requested execution's move the requested component + virtual plan_with_status get_plan(const Name& component_name, + const std::string& execution_id, + const AttributeMap& extra) = 0; + + /// @brief Returns the plan, state history, and replan history of the requested plan. Returns a + /// result if the last execution is still executing, or changed state within the last 24 hours + /// without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param execution_id The execution id of the requested plan. + /// @return a pair of (1) the plan and status and (2) the replan history of the most recent + /// execution to move the requested component + inline std::pair> get_plan_with_replan_history( + const Name& component_name, const std::string& execution_id) { + return get_plan_with_replan_history(component_name, execution_id, {}); + } + + /// @brief Returns the plan, state history, and replan history of the requested plan. Returns a + /// result if the last execution is still executing, or changed state within the last 24 hours + /// without an intervening robot reinitialization. + /// @param component_name The name of the component which the MoveOnGlobe request asked to move. + /// @param execution_id The execution id of the requested plan. + /// @param extra Any additional arguments to the method. + /// @return a pair of (1) the plan and status and (2) the replan history of the most recent + /// execution to move the requested component + virtual std::pair> get_plan_with_replan_history( + const Name& component_name, const std::string& execution_id, const AttributeMap& extra) = 0; + + /// @brief Returns the status of plans created by MoveOnGlobe requests. + /// Includes statuses of plans that are executing, or are part of an executing that changed + /// its state within the last 24 hours. + /// @return a vector of plan statuses. + inline std::vector list_plan_statuses() { + return list_plan_statuses({}); + } + + /// @brief Returns the status of plans created by MoveOnGlobe requests. + /// Includes statuses of plans that are executing, or are part of an execution that changed + /// its state within the last 24 hours. + /// @param extra Any additional arguments to the method. + /// @return a vector of plan statuses. + virtual std::vector list_plan_statuses(const AttributeMap& extra) = 0; + + /// @brief Returns the status of currently active plans created by MoveOnGlobe requests. + /// Includes statuses of plans that are executing, or are part of an execution that changed + /// its state within the last 24 hours. + /// @return a vector of plan statuses. + inline std::vector list_active_plan_statuses() { + return list_plan_statuses({}); + } + + /// @brief Returns the status of currently active plans created by MoveOnGlobe requests. + /// Includes statuses of plans that are executing, or are part of an executing that changed + /// its state within the last 24 hours. + /// @param extra Any additional arguments to the method. + /// @return a vector of plan statuses. + virtual std::vector list_active_plan_statuses( + const AttributeMap& extra) = 0; + /// @brief Send/receive arbitrary commands to the resource. /// @param Command the command to execute. /// @return The result of the executed command. diff --git a/src/viam/sdk/services/motion/server.cpp b/src/viam/sdk/services/motion/server.cpp index 998c2ef3a..7076ad658 100644 --- a/src/viam/sdk/services/motion/server.cpp +++ b/src/viam/sdk/services/motion/server.cpp @@ -178,6 +178,120 @@ ::grpc::Status MotionServer::GetPose(::grpc::ServerContext* context, return ::grpc::Status(); }; +::grpc::Status MotionServer::GetPlan(::grpc::ServerContext* context, + const ::viam::service::motion::v1::GetPlanRequest* request, + ::viam::service::motion::v1::GetPlanResponse* 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()); + boost::optional execution_id; + if (request->has_execution_id()) { + execution_id = request->execution_id(); + } + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + + Motion::plan_with_status plan; + std::vector replan_history; + bool last_plan_only(request->last_plan_only()); + bool has_execution_id(request->has_execution_id()); + + if (last_plan_only && has_execution_id) { + plan = motion->get_plan(component_name, request->execution_id()); + } else if (last_plan_only) { + plan = motion->get_latest_plan(component_name, extra); + } else if (has_execution_id) { + const auto& res = + motion->get_plan_with_replan_history(component_name, request->execution_id(), extra); + plan = res.first; + replan_history = res.second; + } else { + const auto& res = motion->get_latest_plan_with_replan_history(component_name, extra); + plan = res.first; + replan_history = res.second; + } + + *response->mutable_current_plan_with_status() = plan.to_proto(); + for (const auto& p : replan_history) { + *response->mutable_replan_history()->Add() = p.to_proto(); + } + + return ::grpc::Status(); +} + +::grpc::Status MotionServer::ListPlanStatuses( + ::grpc::ServerContext* context, + const service::motion::v1::ListPlanStatusesRequest* request, + service::motion::v1::ListPlanStatusesResponse* 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); + + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + + std::vector statuses; + if (request->only_active_plans()) { + statuses = motion->list_active_plan_statuses(extra); + } else { + statuses = motion->list_plan_statuses(extra); + } + + for (const auto& status : statuses) { + *response->mutable_plan_statuses_with_ids()->Add() = status.to_proto(); + } + + return ::grpc::Status(); +} + +::grpc::Status MotionServer::StopPlan(::grpc::ServerContext* context, + const ::viam::service::motion::v1::StopPlanRequest* request, + ::viam::service::motion::v1::StopPlanResponse* 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()); + AttributeMap extra; + if (request->has_extra()) { + extra = struct_to_map(request->extra()); + } + + motion->stop_plan(component_name, extra); + + return ::grpc::Status(); +} + ::grpc::Status MotionServer::DoCommand(::grpc::ServerContext* context, const ::viam::common::v1::DoCommandRequest* request, ::viam::common::v1::DoCommandResponse* response) { diff --git a/src/viam/sdk/services/motion/server.hpp b/src/viam/sdk/services/motion/server.hpp index fd4a10698..2176583c5 100644 --- a/src/viam/sdk/services/motion/server.hpp +++ b/src/viam/sdk/services/motion/server.hpp @@ -35,6 +35,16 @@ class MotionServer : public ResourceServer, ::grpc::Status GetPose(::grpc::ServerContext* context, const ::viam::service::motion::v1::GetPoseRequest* request, ::viam::service::motion::v1::GetPoseResponse* response) override; + ::grpc::Status GetPlan(::grpc::ServerContext* context, + const ::viam::service::motion::v1::GetPlanRequest* request, + ::viam::service::motion::v1::GetPlanResponse* response) override; + ::grpc::Status ListPlanStatuses( + ::grpc::ServerContext* context, + const ::viam::service::motion::v1::ListPlanStatusesRequest* request, + ::viam::service::motion::v1::ListPlanStatusesResponse* response) override; + ::grpc::Status StopPlan(::grpc::ServerContext* context, + const ::viam::service::motion::v1::StopPlanRequest* request, + ::viam::service::motion::v1::StopPlanResponse* response) override; ::grpc::Status DoCommand(::grpc::ServerContext* context, const ::viam::common::v1::DoCommandRequest* request, ::viam::common::v1::DoCommandResponse* response) override; diff --git a/src/viam/sdk/tests/mocks/mock_board.cpp b/src/viam/sdk/tests/mocks/mock_board.cpp index a13fbf984..be950c274 100644 --- a/src/viam/sdk/tests/mocks/mock_board.cpp +++ b/src/viam/sdk/tests/mocks/mock_board.cpp @@ -65,6 +65,11 @@ Board::analog_value MockBoard::read_analog(const std::string& analog_reader_name return this->peek_read_analog_ret; } +void MockBoard::write_analog(const std::string& pin, int value, const AttributeMap& extra) { + this->peek_pin = pin; + this->peek_pin_value = value; +} + Board::digital_value MockBoard::read_digital_interrupt(const std::string& digital_interrupt_name, const AttributeMap& extra) { this->peek_digital_interrupt_name = digital_interrupt_name; diff --git a/src/viam/sdk/tests/mocks/mock_board.hpp b/src/viam/sdk/tests/mocks/mock_board.hpp index b2f3f3483..c70697367 100644 --- a/src/viam/sdk/tests/mocks/mock_board.hpp +++ b/src/viam/sdk/tests/mocks/mock_board.hpp @@ -30,6 +30,7 @@ class MockBoard : public viam::sdk::Board { viam::sdk::AttributeMap do_command(const viam::sdk::AttributeMap& command) override; Board::analog_value read_analog(const std::string& analog_reader_name, const sdk::AttributeMap& extra) override; + void write_analog(const std::string& pin, int value, const sdk::AttributeMap& extra) override; Board::digital_value read_digital_interrupt(const std::string& digital_interrupt_name, const sdk::AttributeMap& extra) override; void set_power_mode(power_mode power_mode, @@ -38,6 +39,7 @@ class MockBoard : public viam::sdk::Board { std::vector get_geometries(const sdk::AttributeMap& extra) override; std::string peek_pin, peek_analog_reader_name, peek_digital_interrupt_name; + int peek_pin_value; Board::status peek_get_status_ret; bool peek_set_gpio_high; bool peek_get_gpio_ret; diff --git a/src/viam/sdk/tests/mocks/mock_motion.cpp b/src/viam/sdk/tests/mocks/mock_motion.cpp index 190dc0e79..b2f9d9737 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.cpp +++ b/src/viam/sdk/tests/mocks/mock_motion.cpp @@ -1,5 +1,8 @@ #include +#include + +#include #include #include #include @@ -59,22 +62,78 @@ pose_in_frame MockMotion::get_pose( return current_location; } -AttributeMap MockMotion::do_command(const AttributeMap& _command) { - return peek_map; +Motion::plan_with_status MockMotion::get_plan(const sdk::Name& component_name, + const std::string& execution_id, + const sdk::AttributeMap& extra) { + return fake_plan_with_status(); +} + +std::pair> +MockMotion::get_plan_with_replan_history(const sdk::Name& component_name, + const std::string& execution_id, + const sdk::AttributeMap& extra) { + return {fake_plan_with_status(), {fake_plan_with_status()}}; +} + +Motion::plan_with_status MockMotion::get_latest_plan(const sdk::Name& component_name, + const sdk::AttributeMap& extra) { + return fake_plan_with_status(); +} + +std::pair> +MockMotion::get_latest_plan_with_replan_history(const sdk::Name& component_name, + const sdk::AttributeMap& extra) { + return {fake_plan_with_status(), {fake_plan_with_status()}}; +} + +std::vector MockMotion::list_active_plan_statuses( + const sdk::AttributeMap& extra) { + return {fake_plan_status_with_id()}; +} + +std::vector MockMotion::list_plan_statuses( + const sdk::AttributeMap& extra) { + return {fake_plan_status_with_id()}; +} + +void MockMotion::stop_plan(const sdk::Name& name, const sdk::AttributeMap& extra) { + this->peek_stop_plan_called = true; +} + +AttributeMap MockMotion::do_command(const AttributeMap& command) { + return command; }; 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; }; +Motion::plan_with_status MockMotion::fake_plan_with_status() { + plan_with_status pws; + pws.plan = {"id", fake_component_name(), "exec-id", {}}; + pws.status = fake_plan_status(); + pws.status_history = {fake_plan_status()}; + + return pws; +} + pose_in_frame fake_pose() { return pose_in_frame("fake-reference-frame", {{0, 1, 2}, {3, 4, 5}, 6}); } +Motion::plan_status MockMotion::fake_plan_status() { + return {Motion::plan_state::k_succeeded, + std::chrono::time_point::max(), + boost::optional("reason")}; +} + +Motion::plan_status_with_id MockMotion::fake_plan_status_with_id() { + return {"plan_id", fake_component_name(), "execution_id", fake_plan_status()}; +} + pose_in_frame init_fake_pose() { return pose_in_frame("", {{0, 0, 0}, {0, 0, 0}, 0}); } diff --git a/src/viam/sdk/tests/mocks/mock_motion.hpp b/src/viam/sdk/tests/mocks/mock_motion.hpp index 3519560be..c57ab341c 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.hpp +++ b/src/viam/sdk/tests/mocks/mock_motion.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -46,8 +47,33 @@ class MockMotion : public sdk::Motion { const std::vector& supplemental_transforms, const sdk::AttributeMap& extra) override; + plan_with_status get_plan(const sdk::Name& component_name, + const std::string& execution_id, + const sdk::AttributeMap& extra) override; + + std::pair> get_plan_with_replan_history( + const sdk::Name& component_name, + const std::string& execution_id, + const sdk::AttributeMap& extra) override; + + plan_with_status get_latest_plan(const sdk::Name& component_name, + const sdk::AttributeMap& extra) override; + + std::pair> get_latest_plan_with_replan_history( + const sdk::Name& component_name, const sdk::AttributeMap& extra) override; + + std::vector list_plan_statuses(const sdk::AttributeMap& extra) override; + + std::vector list_active_plan_statuses( + const sdk::AttributeMap& extra) override; + + void stop_plan(const sdk::Name& name, const sdk::AttributeMap& extra) override; + sdk::AttributeMap do_command(const sdk::AttributeMap& command) override; static std::shared_ptr get_mock_motion(); + static plan_status fake_plan_status(); + static plan_with_status fake_plan_with_status(); + static plan_status_with_id fake_plan_status_with_id(); // These variables allow the testing infra to `peek` into the mock // and ensure that the correct values were passed @@ -59,11 +85,11 @@ class MockMotion : public sdk::Motion { sdk::geo_point peek_destination; std::string peek_destination_frame; double peek_heading; + bool peek_stop_plan_called = false; 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()){}; diff --git a/src/viam/sdk/tests/test_board.cpp b/src/viam/sdk/tests/test_board.cpp index 9938f5cdf..6b189ccd0 100644 --- a/src/viam/sdk/tests/test_board.cpp +++ b/src/viam/sdk/tests/test_board.cpp @@ -152,6 +152,16 @@ BOOST_AUTO_TEST_CASE(test_read_analog) { }); } +BOOST_AUTO_TEST_CASE(test_write_analog) { + server_to_mock_pipeline([](Board& client, std::shared_ptr mock) -> void { + std::string pin = "pin1"; + int value = 42; + client.write_analog(pin, value); + BOOST_CHECK_EQUAL(pin, mock->peek_pin); + BOOST_CHECK_EQUAL(value, mock->peek_pin_value); + }); +} + BOOST_AUTO_TEST_CASE(test_read_digital_interrupt) { server_to_mock_pipeline([](Board& client, std::shared_ptr mock) -> void { mock->peek_read_digital_interrupt_ret = 515; diff --git a/src/viam/sdk/tests/test_motion.cpp b/src/viam/sdk/tests/test_motion.cpp index 942336def..6373a4fc1 100644 --- a/src/viam/sdk/tests/test_motion.cpp +++ b/src/viam/sdk/tests/test_motion.cpp @@ -13,6 +13,10 @@ BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::WorldState) BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Motion::plan_status_with_id) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Motion::plan_with_status) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) namespace viam { namespace sdktests { @@ -85,6 +89,50 @@ BOOST_AUTO_TEST_CASE(mock_move_on_globe) { BOOST_CHECK_EQUAL(*(motion->peek_motion_configuration), *(fake_motion_configuration())); } +BOOST_AUTO_TEST_CASE(mock_get_plan) { + std::shared_ptr mock = MockMotion::get_mock_motion(); + + auto ret1 = mock->get_plan(fake_component_name(), "execution_id", fake_map()); + BOOST_CHECK_EQUAL(ret1, mock->fake_plan_with_status()); + + auto ret2 = mock->get_latest_plan(fake_component_name(), fake_map()); + BOOST_CHECK_EQUAL(ret1, ret2); +} + +BOOST_AUTO_TEST_CASE(mock_get_plan_with_replan_history) { + std::shared_ptr mock = MockMotion::get_mock_motion(); + auto ret1 = + mock->get_plan_with_replan_history(fake_component_name(), "execution_id", fake_map()); + + BOOST_CHECK_EQUAL(ret1.first, mock->fake_plan_with_status()); + BOOST_CHECK_EQUAL(ret1.second.size(), 1); + BOOST_CHECK_EQUAL(ret1.second[0], mock->fake_plan_with_status()); + + auto ret2 = mock->get_latest_plan_with_replan_history(fake_component_name(), fake_map()); + + BOOST_CHECK_EQUAL(ret1.first, ret2.first); + BOOST_CHECK_EQUAL(ret1.second, ret2.second); +} + +BOOST_AUTO_TEST_CASE(mock_stop_plan) { + std::shared_ptr mock = MockMotion::get_mock_motion(); + BOOST_CHECK(!mock->peek_stop_plan_called); + mock->stop_plan(fake_component_name(), fake_map()); + BOOST_CHECK(mock->peek_stop_plan_called); +} + +BOOST_AUTO_TEST_CASE(mock_list_plan_statuses) { + std::shared_ptr mock = MockMotion::get_mock_motion(); + std::vector statuses1 = mock->list_plan_statuses(fake_map()); + BOOST_CHECK_EQUAL(statuses1.size(), 1); + BOOST_CHECK_EQUAL(statuses1[0], mock->fake_plan_status_with_id()); + + std::vector statuses2 = + mock->list_active_plan_statuses(fake_map()); + + BOOST_CHECK_EQUAL(statuses1, statuses2); +} + BOOST_AUTO_TEST_CASE(mock_do_command) { std::shared_ptr motion = MockMotion::get_mock_motion(); AttributeMap expected = fake_map(); @@ -124,8 +172,8 @@ BOOST_AUTO_TEST_SUITE(test_motion_client_server) template void server_to_mock_pipeline(Lambda&& func) { MotionServer motion_server; - motion_server.resource_manager()->add(std::string("mock_motion"), - MockMotion::get_mock_motion()); + auto mock = std::make_shared("mock_motion"); + motion_server.resource_manager()->add(std::string("mock_motion"), mock); grpc::ServerBuilder builder; builder.RegisterService(&motion_server); @@ -136,13 +184,13 @@ void server_to_mock_pipeline(Lambda&& func) { auto grpc_channel = server->InProcessChannel(args); MotionClient client("mock_motion", grpc_channel); // Run the passed test on the created stack - std::forward(func)(client); + std::forward(func)(client, mock); // shutdown afterwards server->Shutdown(); } BOOST_AUTO_TEST_CASE(test_move_and_get_pose) { - server_to_mock_pipeline([](Motion& client) -> void { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { std::string destination_frame("destination"); std::vector transforms; AttributeMap extra = fake_map(); @@ -159,7 +207,7 @@ BOOST_AUTO_TEST_CASE(test_move_and_get_pose) { } BOOST_AUTO_TEST_CASE(test_move_on_map) { - server_to_mock_pipeline([](Motion& client) -> void { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> 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()); @@ -172,8 +220,7 @@ BOOST_AUTO_TEST_CASE(test_move_on_map) { } BOOST_AUTO_TEST_CASE(test_move_on_globe) { - BOOST_TEST(true); - server_to_mock_pipeline([](Motion& client) -> void { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { bool success = client.move_on_globe(fake_geo_point(), 15, fake_component_name(), @@ -186,8 +233,52 @@ BOOST_AUTO_TEST_CASE(test_move_on_globe) { }); } +BOOST_AUTO_TEST_CASE(test_get_plan) { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { + auto ret1 = client.get_plan(fake_component_name(), "execution_id", fake_map()); + BOOST_CHECK_EQUAL(ret1, MockMotion::fake_plan_with_status()); + + auto ret2 = client.get_latest_plan(fake_component_name(), fake_map()); + BOOST_CHECK_EQUAL(ret1, ret2); + }); +} + +BOOST_AUTO_TEST_CASE(test_get_plan_with_replan_history) { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { + auto ret1 = + client.get_plan_with_replan_history(fake_component_name(), "execution_id", fake_map()); + BOOST_CHECK_EQUAL(ret1.first, MockMotion::fake_plan_with_status()); + BOOST_CHECK_EQUAL(ret1.second.size(), 1); + BOOST_CHECK_EQUAL(ret1.second[0], MockMotion::fake_plan_with_status()); + + auto ret2 = client.get_latest_plan_with_replan_history(fake_component_name(), fake_map()); + BOOST_CHECK_EQUAL(ret1.first, ret2.first); + BOOST_CHECK_EQUAL(ret1.second, ret2.second); + }); +} + +BOOST_AUTO_TEST_CASE(test_list_plan_statuses) { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { + std::vector statuses1 = client.list_plan_statuses(fake_map()); + BOOST_CHECK_EQUAL(statuses1.size(), 1); + BOOST_CHECK_EQUAL(statuses1[0], MockMotion::fake_plan_status_with_id()); + + std::vector statuses2 = + client.list_active_plan_statuses(fake_map()); + + BOOST_CHECK_EQUAL(statuses1, statuses2); + }); +} + +BOOST_AUTO_TEST_CASE(test_stop_plan) { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { + client.stop_plan(fake_component_name()); + BOOST_CHECK(mock->peek_stop_plan_called); + }); +} + BOOST_AUTO_TEST_CASE(test_do_command) { - server_to_mock_pipeline([](Motion& client) -> void { + server_to_mock_pipeline([](Motion& client, std::shared_ptr mock) -> void { AttributeMap expected = fake_map(); AttributeMap command = fake_map();