99 lines
3.1 KiB
C++
99 lines
3.1 KiB
C++
#ifndef CMVR_ES_SELF_COLLISION_TASK_H
|
|
#define CMVR_ES_SELF_COLLISION_TASK_H
|
|
|
|
#include <condition_variable>
|
|
#include <cstdint>
|
|
#include <deque>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
|
|
#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
|
|
#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h"
|
|
#include "devices/arm/robot_arm.h"
|
|
#include "task/task.h"
|
|
|
|
namespace cmvr::task {
|
|
|
|
enum class CollisionSafetyLevel {
|
|
UNKNOWN = 0,
|
|
SAFE,
|
|
WARNING,
|
|
STOP,
|
|
};
|
|
|
|
enum class ProtectiveRecoveryState {
|
|
IDLE = 0,
|
|
AVAILABLE,
|
|
RECOVERING,
|
|
SUCCEEDED,
|
|
FAILED,
|
|
};
|
|
|
|
struct SelfCollisionTaskStatus {
|
|
CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN};
|
|
SelfCollisionResult result;
|
|
bool stop_latched{false};
|
|
std::uint64_t event_id{0};
|
|
ProtectiveRecoveryState recovery_state{ProtectiveRecoveryState::IDLE};
|
|
std::size_t recovery_sample_count{0};
|
|
std::string recovery_error;
|
|
};
|
|
|
|
class SelfCollisionTask final : public Task {
|
|
public:
|
|
explicit SelfCollisionTask(const config::SelfCollisionTaskConfig& config);
|
|
|
|
const std::string& id() const override { return id_; }
|
|
TaskRunMode runMode() const override { return TaskRunMode::PERIODIC_STEP; }
|
|
|
|
bool init() override;
|
|
bool start() override;
|
|
bool step(double dt) override;
|
|
void stop() override;
|
|
|
|
TaskState state() const override;
|
|
bool isBusy() const override;
|
|
bool isFinished() const override;
|
|
bool isFailed() const override;
|
|
std::string stateString() const override;
|
|
std::string detailStatusString() const override;
|
|
|
|
SelfCollisionTaskStatus latestStatus() const;
|
|
device::Result requestRecovery(std::uint64_t event_id);
|
|
|
|
private:
|
|
static bool validateConfig(const config::SelfCollisionTaskConfig& config,
|
|
std::string* error);
|
|
static const char* safetyLevelToString(CollisionSafetyLevel level);
|
|
static const char* recoveryStateToString(ProtectiveRecoveryState state);
|
|
void recordJointSample_(const device::JointGroupState& joint_state,
|
|
DistanceSamplingPolicy::Clock::time_point now);
|
|
|
|
config::SelfCollisionTaskConfig config_;
|
|
std::string id_;
|
|
std::shared_ptr<device::RobotArm> arm_;
|
|
SelfCollisionChecker checker_;
|
|
DistanceSamplingPolicy sampling_;
|
|
|
|
mutable std::mutex mutex_;
|
|
std::condition_variable recovery_cv_;
|
|
TaskState state_{TaskState::UNINITIALIZED};
|
|
SelfCollisionTaskStatus latest_status_{};
|
|
std::deque<device::JointTrajectoryPoint> joint_history_;
|
|
device::JointTrajectory recovery_path_;
|
|
DistanceSamplingPolicy::Clock::time_point history_epoch_{};
|
|
std::optional<DistanceSamplingPolicy::Clock::time_point> recovery_clear_since_;
|
|
double recovery_best_distance_m_{0.0};
|
|
bool recovery_clear_confirmed_{false};
|
|
std::uint64_t next_event_id_{1};
|
|
std::string last_error_;
|
|
};
|
|
|
|
} // namespace cmvr::task
|
|
|
|
#endif // CMVR_ES_SELF_COLLISION_TASK_H
|