lgv
0257ac85ca
Add Pinocchio and Coal based self-collision checking with collision-pair filtering and displacement-based sampling. Integrate a periodic safety task with warning and stop thresholds, plus the simplified collision URDF and runtime configuration.
70 lines
2.0 KiB
C++
70 lines
2.0 KiB
C++
#ifndef CMVR_ES_SELF_COLLISION_TASK_H
|
|
#define CMVR_ES_SELF_COLLISION_TASK_H
|
|
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
|
|
#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,
|
|
};
|
|
|
|
struct SelfCollisionTaskStatus {
|
|
CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN};
|
|
SelfCollisionResult result;
|
|
bool stop_latched{false};
|
|
};
|
|
|
|
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;
|
|
|
|
private:
|
|
static bool validateConfig(const config::SelfCollisionTaskConfig& config,
|
|
std::string* error);
|
|
static const char* safetyLevelToString(CollisionSafetyLevel level);
|
|
|
|
config::SelfCollisionTaskConfig config_;
|
|
std::string id_;
|
|
std::shared_ptr<device::RobotArm> arm_;
|
|
SelfCollisionChecker checker_;
|
|
DistanceSamplingPolicy sampling_;
|
|
|
|
mutable std::mutex mutex_;
|
|
TaskState state_{TaskState::UNINITIALIZED};
|
|
SelfCollisionTaskStatus latest_status_{};
|
|
std::string last_error_;
|
|
};
|
|
|
|
} // namespace cmvr::task
|
|
|
|
#endif // CMVR_ES_SELF_COLLISION_TASK_H
|