203 lines
8.2 KiB
C++
203 lines
8.2 KiB
C++
#pragma once
|
|
|
|
#ifndef CMVR_ES_TOUCH_SCREEN_TASK_H
|
|
#define CMVR_ES_TOUCH_SCREEN_TASK_H
|
|
|
|
#include <array>
|
|
#include <chrono>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
|
|
#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
|
|
#include "algorithms/perception/apriltag/include/apriltag_perception.h"
|
|
#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
|
|
#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
|
|
#include "devices/camera/abstract_camera.h"
|
|
#include "devices/dexhand/abstract_dexhand.h"
|
|
#include "devices/arm/robot_arm.h"
|
|
#include "task/task.h"
|
|
|
|
namespace cmvr::task {
|
|
|
|
class TouchScreenTask : public Task {
|
|
public:
|
|
enum class Phase {
|
|
IDLE = 0, // 空闲，尚未开始任务。
|
|
ALIGNING, // 视觉对准阶段：持续 PBVS 对齐目标点。
|
|
ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。
|
|
TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。
|
|
DWELLING, // 已检测到接触，保持当前位置短暂停留。
|
|
RETRACTING, // 回退阶段：沿设定回退方向离开屏幕。
|
|
DONE, // 整个流程成功完成。
|
|
FAILED // 流程失败并已停止。
|
|
};
|
|
|
|
enum class Status {
|
|
IDLE = 0, // 空闲状态。
|
|
NOT_INITIALIZED, // 尚未调用 init() 完成初始化。
|
|
INVALID_CONFIG, // 配置非法，无法启动或应用参数。
|
|
ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
|
|
ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。
|
|
ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。
|
|
ALIGN_COMPUTE_FAILED, // 对准阶段 PBVS 计算失败。
|
|
ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。
|
|
ALIGNING, // 正在执行视觉对准。
|
|
ALIGN_REACHED, // 视觉对准完成。
|
|
TOUCHING, // 正在向前触控。
|
|
TACTILE_UNAVAILABLE, // 触觉数据不可用。
|
|
TOUCH_TRIGGERED, // 已检测到接触触发。
|
|
TOUCH_FORWARD_TIMEOUT, // 前进触控时间到，但未触发接触。
|
|
RETRACTING, // 正在回退离开屏幕。
|
|
DONE, // 流程成功完成。
|
|
STOPPED, // 被外部 stop() 主动停止。
|
|
ROBOT_STATE_FAILED, // 读取机器人状态失败。
|
|
ROBOT_COMMAND_FAILED, // 向机器人下发控制命令失败。
|
|
TASK_BUSY // 已有触屏流程正在运行，新的 touch 请求被拒绝。
|
|
};
|
|
|
|
explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg);
|
|
~TouchScreenTask() = default;
|
|
|
|
bool init() override;
|
|
bool init(const std::shared_ptr<device::RobotArm>& arm,
|
|
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
|
const std::shared_ptr<device::AbstractCamera>& camera);
|
|
bool init(const std::shared_ptr<device::RobotArm>& arm,
|
|
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
|
const std::shared_ptr<device::AbstractCamera>& camera,
|
|
const std::shared_ptr<device::AbstractCamera>& external_camera);
|
|
|
|
const std::string& id() const override { return id_; }
|
|
|
|
bool touch(int u, int v);
|
|
bool startFromPixel(int u, int v);
|
|
bool step(double dt) override;
|
|
void stop() override;
|
|
|
|
Phase phase() const;
|
|
Status lastStatus() const;
|
|
static const char* phaseToString(Phase phase);
|
|
static const char* statusToString(Status status);
|
|
|
|
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;
|
|
|
|
int targetU() const;
|
|
int targetV() const;
|
|
double lastTouchPressureSum() const;
|
|
int lastTouchNonzeroCount() const;
|
|
int lastActiveTagId() const;
|
|
Eigen::Vector3d lastAlignErrorScreenTag() const;
|
|
|
|
const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
|
|
const std::shared_ptr<perception::AprilTagPerception>& externalPerception() const {
|
|
return external_perception_;
|
|
}
|
|
const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
|
|
const perception::TagRelativeTcpPose& tcpPoseTracker() const { return tcp_pose_tracker_; }
|
|
const PbvsController& pbvs() const { return pbvs_; }
|
|
|
|
private:
|
|
using Clock = std::chrono::steady_clock;
|
|
|
|
static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config);
|
|
bool isBusyUnlocked() const;
|
|
bool startFromPixelUnlocked(int u, int v);
|
|
void stopUnlocked();
|
|
bool applyConfig();
|
|
bool stepAligning(double dt);
|
|
bool stepTouching();
|
|
bool stepDwelling();
|
|
bool stepRetracting();
|
|
|
|
void stopPbvsMotion();
|
|
bool buildInitJointPositions(std::vector<double>& positions_out) const;
|
|
bool isAtInitPosition(const std::vector<double>& positions) const;
|
|
bool moveToInitPositionBeforeStartIfEnabled();
|
|
bool moveToInitPositionIfEnabled() const;
|
|
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
|
|
void logTouchingSpeedLState() const;
|
|
|
|
bool startTouchPhase();
|
|
bool handleTouchTriggered(bool stop_forward_motion);
|
|
bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
|
|
void enterFailed(Status status);
|
|
|
|
bool updateTouchPressure();
|
|
void publishCoordinateOverlay();
|
|
void refreshCoordinateOverlay();
|
|
|
|
private:
|
|
mutable std::mutex mutex_;
|
|
std::string id_;
|
|
std::shared_ptr<device::RobotArm> arm_{nullptr};
|
|
std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
|
|
std::shared_ptr<device::AbstractCamera> camera_{nullptr};
|
|
std::shared_ptr<device::AbstractCamera> external_camera_{nullptr};
|
|
|
|
std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
|
|
std::shared_ptr<perception::AprilTagPerception> external_perception_{nullptr};
|
|
perception::TagRelativeTarget3D tracker_;
|
|
perception::TagRelativeTcpPose tcp_pose_tracker_;
|
|
PbvsController pbvs_;
|
|
cmvr::config::TouchScreenTaskConfig config_{};
|
|
bool config_valid_{false};
|
|
|
|
Phase phase_{Phase::IDLE};
|
|
Phase phase_after_retract_{Phase::DONE};
|
|
Status last_status_{Status::NOT_INITIALIZED};
|
|
|
|
bool initialized_{false};
|
|
bool target_locked_{false};
|
|
bool pbvs_target_initialized_{false};
|
|
bool touch_command_started_{false};
|
|
bool retract_command_started_{false};
|
|
|
|
int target_u_{-1};
|
|
int target_v_{-1};
|
|
int align_stable_count_{0};
|
|
int align_debug_count_{0};
|
|
int pbvs_debug_count_{0};
|
|
int last_active_tag_id_{-1};
|
|
|
|
double last_touch_pressure_sum_{0.0};
|
|
int last_touch_nonzero_count_{0};
|
|
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
|
|
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
|
|
double pbvs_command_acceleration_{0.25};
|
|
// Initialization MoveJ is skipped only when both position and velocity
|
|
// are within these configured limits.
|
|
double init_skip_position_tolerance_rad_{1e-3};
|
|
double init_skip_velocity_tolerance_rad_s_{1e-2};
|
|
bool locked_target_rotation_valid_{false};
|
|
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
|
|
bool touch_start_position_valid_{false};
|
|
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
|
|
bool retract_start_position_valid_{false};
|
|
Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
|
|
bool have_last_T_B_G_{false};
|
|
Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
|
|
double max_T_B_G_translation_delta_m_{0.0};
|
|
double max_T_B_G_rotation_delta_rad_{0.0};
|
|
|
|
Clock::time_point phase_start_time_{};
|
|
Clock::time_point last_coordinate_overlay_update_time_{};
|
|
Clock::time_point last_retract_log_time_{};
|
|
Status final_status_after_retract_{Status::DONE};
|
|
};
|
|
|
|
using touch_screen_task = TouchScreenTask;
|
|
|
|
} // namespace cmvr::task
|
|
|
|
#endif // CMVR_ES_TOUCH_SCREEN_TASK_H
|