motion.hpp

 
#pragma once
 
#include <string>
 
#include <viam/sdk/common/pose.hpp>
#include <viam/sdk/common/proto_value.hpp>
#include <viam/sdk/common/utils.hpp>
#include <viam/sdk/common/world_state.hpp>
#include <viam/sdk/resource/resource_api.hpp>
#include <viam/sdk/services/service.hpp>
#include <viam/sdk/spatialmath/geometry.hpp>
 
namespace viam {
namespace sdk {
 
struct obstacle_detector {
    Name vision_service;
    Name camera;
 
    friend bool operator==(const obstacle_detector& lhs, const obstacle_detector& rhs);
    friend std::ostream& operator<<(std::ostream& os, const obstacle_detector& v);
};
 
struct motion_configuration {
    std::vector<obstacle_detector> obstacle_detectors;
 
    boost::optional<double> position_polling_frequency_hz;
 
    boost::optional<double> obstacle_polling_frequency_hz;
 
    boost::optional<double> plan_deviation_m;
 
    boost::optional<double> linear_m_per_sec;
 
    boost::optional<double> angular_degs_per_sec;
 
    friend bool operator==(const motion_configuration& lhs, const motion_configuration& rhs);
    friend std::ostream& operator<<(std::ostream& os, const motion_configuration& v);
};
 
class Motion : public Service {
   public:
    enum class plan_state : uint8_t {
        k_in_progress,
        k_stopped,
        k_succeeded,
        k_failed,
    };
 
    struct plan_status {
        plan_state state;
 
        time_pt timestamp;
 
        boost::optional<std::string> reason;
 
        friend bool operator==(const plan_status& lhs, const plan_status& rhs);
    };
 
    struct plan_status_with_id {
        std::string plan_id;
 
        Name component_name;
 
        std::string execution_id;
 
        plan_status status;
 
        friend bool operator==(const plan_status_with_id& lhs, const plan_status_with_id& rhs);
    };
 
    using step = std::unordered_map<std::string, pose>;
 
    struct plan {
        std::string id;
 
        Name component_name;
 
        std::string execution_id;
 
        std::vector<step> steps;
 
        friend bool operator==(const plan& lhs, const plan& rhs);
    };
 
    struct plan_with_status {
        struct plan plan;
 
        plan_status status;
 
        std::vector<plan_status> status_history;
 
        friend bool operator==(const plan_with_status& lhs, const plan_with_status& rhs);
    };
 
    struct linear_constraint {
        float line_tolerance_mm;
        float orientation_tolerance_degs;
    };
 
    struct orientation_constraint {
        float orientation_tolerance_degs;
    };
 
    struct collision_specification {
        struct allowed_frame_collisions {
            std::string frame1;
            std::string frame2;
        };
        std::vector<allowed_frame_collisions> allows;
    };
 
    struct constraints {
        std::vector<linear_constraint> linear_constraints;
        std::vector<orientation_constraint> orientation_constraints;
        std::vector<collision_specification> collision_specifications;
    };
 
    API api() const override;
 
    inline bool move(const pose_in_frame& destination,
                     const Name& name,
                     const std::shared_ptr<WorldState>& world_state,
                     const std::shared_ptr<constraints>& constraints) {
        return move(destination, name, world_state, constraints, {});
    }
 
    virtual bool move(const pose_in_frame& destination,
                      const Name& name,
                      const std::shared_ptr<WorldState>& world_state,
                      const std::shared_ptr<constraints>& constraints,
                      const ProtoStruct& extra) = 0;
 
    inline std::string move_on_map(
        const pose& destination,
        const Name& component_name,
        const Name& slam_name,
        const std::shared_ptr<motion_configuration>& motion_configuration,
        const std::vector<GeometryConfig>& obstacles) {
        return move_on_map(
            destination, component_name, slam_name, motion_configuration, obstacles, {});
    }
 
    virtual std::string move_on_map(
        const pose& destination,
        const Name& component_name,
        const Name& slam_name,
        const std::shared_ptr<motion_configuration>& motion_configuration,
        const std::vector<GeometryConfig>& obstacles,
        const ProtoStruct& extra) = 0;
 
    inline std::string move_on_globe(
        const geo_point& destination,
        const boost::optional<double>& heading,
        const Name& component_name,
        const Name& movement_sensor_name,
        const std::vector<geo_geometry>& obstacles,
        const std::shared_ptr<motion_configuration>& motion_configuration,
        const std::vector<geo_geometry>& bounding_regions) {
        return move_on_globe(destination,
                             heading,
                             component_name,
                             movement_sensor_name,
                             obstacles,
                             motion_configuration,
                             bounding_regions,
                             {});
    }
 
    virtual std::string move_on_globe(
        const geo_point& destination,
        const boost::optional<double>& heading,
        const Name& component_name,
        const Name& movement_sensor_name,
        const std::vector<geo_geometry>& obstacles,
        const std::shared_ptr<motion_configuration>& motion_configuration,
        const std::vector<geo_geometry>& bounding_regions,
        const ProtoStruct& extra) = 0;
 
    inline pose_in_frame get_pose(
        const Name& component_name,
        const std::string& destination_frame,
        const std::vector<WorldState::transform>& supplemental_transforms) {
        return get_pose(component_name, destination_frame, supplemental_transforms, {});
    }
 
    virtual pose_in_frame get_pose(
        const Name& component_name,
        const std::string& destination_frame,
        const std::vector<WorldState::transform>& supplemental_transforms,
        const ProtoStruct& extra) = 0;
 
    inline void stop_plan(const Name& component_name) {
        return stop_plan(component_name, {});
    }
 
    virtual void stop_plan(const Name& component_name, const ProtoStruct& extra) = 0;
 
    inline plan_with_status get_latest_plan(const Name& component_name) {
        return get_latest_plan(component_name, {});
    }
 
    virtual plan_with_status get_latest_plan(const Name& component_name,
                                             const ProtoStruct& extra) = 0;
 
    inline std::pair<plan_with_status, std::vector<plan_with_status>>
    get_latest_plan_with_replan_history(Name component_name) {
        return get_latest_plan_with_replan_history(component_name, {});
    }
 
    virtual std::pair<plan_with_status, std::vector<plan_with_status>>
    get_latest_plan_with_replan_history(const Name& component_name, const ProtoStruct& extra) = 0;
 
    inline plan_with_status get_plan(const Name& component_name, const std::string& execution_id) {
        return get_plan(component_name, execution_id, {});
    }
 
    virtual plan_with_status get_plan(const Name& component_name,
                                      const std::string& execution_id,
                                      const ProtoStruct& extra) = 0;
 
    inline std::pair<plan_with_status, std::vector<plan_with_status>> get_plan_with_replan_history(
        const Name& component_name, const std::string& execution_id) {
        return get_plan_with_replan_history(component_name, execution_id, {});
    }
 
    virtual std::pair<plan_with_status, std::vector<plan_with_status>> get_plan_with_replan_history(
        const Name& component_name, const std::string& execution_id, const ProtoStruct& extra) = 0;
 
    inline std::vector<plan_status_with_id> list_plan_statuses() {
        return list_plan_statuses({});
    }
 
    virtual std::vector<plan_status_with_id> list_plan_statuses(const ProtoStruct& extra) = 0;
 
    inline std::vector<plan_status_with_id> list_active_plan_statuses() {
        return list_plan_statuses({});
    }
 
    virtual std::vector<plan_status_with_id> list_active_plan_statuses(
        const ProtoStruct& extra) = 0;
 
    virtual ProtoStruct do_command(const ProtoStruct& command) = 0;
 
   protected:
    explicit Motion(std::string name);
};
 
template <>
struct API::traits<Motion> {
    static API api();
};
 
}  // namespace sdk
}  // namespace viam