fix(touch): separate camera display thread and improve touch workflow
This commit is contained in:
parent
dbac432565
commit
96e3bcf624
@ -62,7 +62,7 @@ arm {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
settle_position_tolerance_rad: 0.01
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
|
||||
@ -35,8 +35,8 @@ camera {
|
||||
stream_mode: STREAM_MODE_RGB
|
||||
}
|
||||
encoder {
|
||||
width: 640
|
||||
height: 360
|
||||
width: 480
|
||||
height: 320
|
||||
fps: 30
|
||||
codec: "H264"
|
||||
enable_stream_timestamp: true
|
||||
@ -48,21 +48,21 @@ camera {
|
||||
}
|
||||
|
||||
cameras {
|
||||
id: "cam3"
|
||||
id: "left_hand_cam"
|
||||
realsense {
|
||||
serialNumber: "243122075614"
|
||||
camera_mode: CAMERA_MODE_VIDEO
|
||||
capture {
|
||||
width: 640
|
||||
height: 480
|
||||
width: 1280
|
||||
height: 720
|
||||
fps: 30
|
||||
stream_mode: STREAM_MODE_RGBD
|
||||
stream_mode: STREAM_MODE_RGB
|
||||
}
|
||||
encoder {
|
||||
width: 640
|
||||
height: 480
|
||||
width: 480
|
||||
height: 320
|
||||
fps: 30
|
||||
codec: "H265"
|
||||
codec: "H264"
|
||||
enable_stream_timestamp: true
|
||||
buffer_size: 30
|
||||
}
|
||||
|
||||
@ -6,56 +6,56 @@ device_manager {
|
||||
id: "mujoco_world"
|
||||
type: DEVICE_TYPE_MUJOCO_WORLD
|
||||
config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "right_arm_mujoco_motors"
|
||||
type: DEVICE_TYPE_MOTOR_SYSTEM
|
||||
config_file: "devices/motor/mujoco_motors.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_right_arm"
|
||||
type: DEVICE_TYPE_ROBOT_ARM
|
||||
config_file: "devices/arm/arm_mujoco_qp.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_viewer"
|
||||
type: DEVICE_TYPE_MUJOCO_VIEWER
|
||||
config_file: "devices/mujoco/mujoco_viewer.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_hand_cam"
|
||||
type: DEVICE_TYPE_CAMERA
|
||||
config_file: "devices/camera/camera.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_external_touch_cam"
|
||||
type: DEVICE_TYPE_CAMERA
|
||||
config_file: "devices/camera/camera.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "right_hand_cam"
|
||||
type: DEVICE_TYPE_CAMERA
|
||||
config_file: "devices/camera/camera.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "cam5"
|
||||
id: "left_hand_cam"
|
||||
type: DEVICE_TYPE_CAMERA
|
||||
config_file: "devices/camera/camera.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
|
||||
@ -63,21 +63,21 @@ device_manager {
|
||||
id: "hand2"
|
||||
type: DEVICE_TYPE_DEXHAND
|
||||
config_file: "devices/dexhand/dexhand.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "paxini_tip_1"
|
||||
type: DEVICE_TYPE_DEXHAND
|
||||
config_file: "devices/dexhand/dexhand.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_zero_touch_dexhand"
|
||||
type: DEVICE_TYPE_DEXHAND
|
||||
config_file: "devices/dexhand/dexhand.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
@ -91,7 +91,7 @@ device_manager {
|
||||
id: "right_arm_can_motors"
|
||||
type: DEVICE_TYPE_MOTOR_SYSTEM
|
||||
config_file: "devices/motor/ti5_motors.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
@ -119,7 +119,7 @@ device_manager {
|
||||
id: "right_arm"
|
||||
type: DEVICE_TYPE_ROBOT_ARM
|
||||
config_file: "devices/arm/arm_qp.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
|
||||
@ -4,8 +4,8 @@ task_manager {
|
||||
type: TASK_TYPE_TOUCH_SCREEN
|
||||
run_mode: TASK_RUN_MODE_PERIODIC_STEP
|
||||
control_period_s: 0.001
|
||||
config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
|
||||
enable: false
|
||||
config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
|
||||
enable: true
|
||||
}
|
||||
tasks {
|
||||
id: "grpc_server"
|
||||
|
||||
@ -10,37 +10,37 @@ touch_screen_task {
|
||||
dexhand_id: "paxini_tip_1"
|
||||
# 手部相机和外部相机的 DeviceManager ID。
|
||||
camera_id: "right_hand_cam"
|
||||
external_camera_id: "cam5"
|
||||
external_camera_id: "left_hand_cam"
|
||||
}
|
||||
|
||||
initialization {
|
||||
before_start: true
|
||||
before_start: false
|
||||
after_finish: true
|
||||
joint_positions { joint_name: "R_SHOULDER_P" rad: -0.3678 }
|
||||
joint_positions { joint_name: "R_SHOULDER_R" rad: 1.1127 }
|
||||
joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.6084 }
|
||||
joint_positions { joint_name: "R_ELBOW_R" rad: 1.61 }
|
||||
joint_positions { joint_name: "R_WRIST_P" rad: -2.5718 }
|
||||
joint_positions { joint_name: "R_WRIST_Y" rad: 0.1276 }
|
||||
joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
|
||||
joint_positions { joint_name: "R_SHOULDER_P" rad: -0.338732 }
|
||||
joint_positions { joint_name: "R_SHOULDER_R" rad: 1.265943 }
|
||||
joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.572396 }
|
||||
joint_positions { joint_name: "R_ELBOW_R" rad: 1.5464 }
|
||||
joint_positions { joint_name: "R_WRIST_P" rad: -2.8179 }
|
||||
joint_positions { joint_name: "R_WRIST_Y" rad: 0.1105 }
|
||||
joint_positions { joint_name: "R_WRIST_R" rad: 0.1347 }
|
||||
velocity: 1.0
|
||||
acceleration: 2.0
|
||||
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
|
||||
skip_position_tolerance_rad: 0.001
|
||||
skip_position_tolerance_rad: 0.01
|
||||
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
|
||||
skip_velocity_tolerance_rad_s: 0.01
|
||||
skip_velocity_tolerance_rad_s: 0.1
|
||||
}
|
||||
|
||||
perception {
|
||||
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
|
||||
tags {
|
||||
screen {
|
||||
id: 1
|
||||
size_m: 0.03
|
||||
id: 14
|
||||
size_m: 0.016
|
||||
}
|
||||
hand {
|
||||
id: 0
|
||||
size_m: 0.03
|
||||
id: 16
|
||||
size_m: 0.016
|
||||
}
|
||||
}
|
||||
# 手部相机：用于点击目标点和手部目标跟踪。
|
||||
@ -52,18 +52,18 @@ touch_screen_task {
|
||||
|
||||
alignment {
|
||||
calibration {
|
||||
# TCP P 相对于屏幕 Hand Tag H 的目标姿态
|
||||
# TCP P 相对于 Hand Tag H 的目标姿态
|
||||
hand_tag_to_tcp {
|
||||
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
|
||||
m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
|
||||
m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
|
||||
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.035
|
||||
m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.09
|
||||
m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.18
|
||||
m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
|
||||
}
|
||||
}
|
||||
pbvs {
|
||||
position_gain { x: 2.0 y: 2.0 z: 1.5 }
|
||||
rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
|
||||
vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
|
||||
vmax6 { x: 0.02 y: 0.02 z: 0.02 rx: 0.050 ry: 0.050 rz: 0.050 }
|
||||
amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
|
||||
twist_filter_alpha: 1.0
|
||||
}
|
||||
@ -72,29 +72,31 @@ touch_screen_task {
|
||||
# rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
|
||||
# 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
|
||||
# PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
|
||||
hand_orientation_G { rx: 0.0 ry: 0.0 rz: 3.141592653589793 }
|
||||
hand_orientation_G { rx: 0.0 ry: 0.0 rz: -1.57 }
|
||||
# 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
|
||||
position_offset_G { x: 0.0 y: 0.0 z: 0.05 }
|
||||
position_offset_G { x: 0.0 y: 0.0 z: 0.00 }
|
||||
mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
|
||||
}
|
||||
error_threshold {
|
||||
x: 0.005
|
||||
y: 0.005
|
||||
z: 0.01
|
||||
rx: 0.1026646259971647
|
||||
ry: 0.1026646259971647
|
||||
z: 0.005
|
||||
rx: 0.0126646259971647
|
||||
ry: 0.0126646259971647
|
||||
rz: 0.1026646259971647
|
||||
}
|
||||
stable_frames: 2
|
||||
# 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。
|
||||
timeout_s: 20.0
|
||||
# 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。
|
||||
pause_when_reached: false
|
||||
}
|
||||
|
||||
touch {
|
||||
speed_l {
|
||||
twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 6.0
|
||||
max_distance_m: 0.035
|
||||
twist_tool { x: 0.0 y: -0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 5.0
|
||||
max_distance_m: 0.03
|
||||
}
|
||||
tactile {
|
||||
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
@ -107,8 +109,8 @@ touch_screen_task {
|
||||
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 8.0
|
||||
acceleration: 3.0
|
||||
# TCP 后退目标距离，单位为米。
|
||||
distance_m: 0.02
|
||||
distance_m: 0.03
|
||||
}
|
||||
}
|
||||
|
||||
@ -95,7 +95,9 @@ touch_screen_task {
|
||||
rz: 0.08726646259971647
|
||||
}
|
||||
stable_frames: 5
|
||||
# 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。
|
||||
timeout_s: 20.0
|
||||
# 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。
|
||||
pause_when_reached: false
|
||||
}
|
||||
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <Eigen/Dense>
|
||||
#include <iomanip>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
@ -1043,9 +1044,16 @@ Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) con
|
||||
"moveJ target size mismatch while settling");
|
||||
}
|
||||
|
||||
struct JointFeedback {
|
||||
double position{0.0};
|
||||
double velocity{0.0};
|
||||
};
|
||||
std::vector<JointFeedback> last_feedback(joint_names_.size());
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::duration<double>(move_j_settle_timeout_s_);
|
||||
std::size_t sample_count = 0;
|
||||
int stable_samples = 0;
|
||||
int max_stable_samples = 0;
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
if (const auto stopped = safetyStopResult_("moveJ", true)) {
|
||||
return *stopped;
|
||||
@ -1061,22 +1069,54 @@ Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) con
|
||||
}
|
||||
const double position = motor->getQ();
|
||||
const double velocity = motor->getQd();
|
||||
last_feedback[i] = {position, velocity};
|
||||
if (!std::isfinite(position) || !std::isfinite(velocity) ||
|
||||
std::abs(position - target[i]) > move_j_position_tolerance_rad_ ||
|
||||
std::abs(velocity) > move_j_velocity_tolerance_rad_s_) {
|
||||
settled = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
++sample_count;
|
||||
stable_samples = settled ? stable_samples + 1 : 0;
|
||||
max_stable_samples = std::max(max_stable_samples, stable_samples);
|
||||
if (stable_samples >= move_j_stable_sample_count_) {
|
||||
return Result::success();
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_;
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_
|
||||
<< ", timeout_s=" << move_j_settle_timeout_s_
|
||||
<< ", sample_count=" << sample_count
|
||||
<< ", stable_samples=" << stable_samples
|
||||
<< ", required_stable_samples=" << move_j_stable_sample_count_
|
||||
<< ", max_stable_samples=" << max_stable_samples
|
||||
<< ", reason=" << (sample_count == 0 ? "no_feedback_samples"
|
||||
: stable_samples > 0 ? "insufficient_stable_samples"
|
||||
: "joint_feedback_not_settled");
|
||||
// Report the feedback from the final check, rather than reading newer
|
||||
// values that may no longer explain the timeout. Include every joint so
|
||||
// valid axes and non-finite feedback are distinguishable in the same sample.
|
||||
for (std::size_t i = 0; sample_count > 0 && i < joint_names_.size(); ++i) {
|
||||
const auto& feedback = last_feedback[i];
|
||||
const double position_error = std::abs(feedback.position - target[i]);
|
||||
const bool position_ok = std::isfinite(feedback.position) &&
|
||||
position_error <= move_j_position_tolerance_rad_;
|
||||
const bool velocity_ok = std::isfinite(feedback.velocity) &&
|
||||
std::abs(feedback.velocity) <= move_j_velocity_tolerance_rad_s_;
|
||||
CMVR_LOG(ERROR) << std::setprecision(12)
|
||||
<< "[MotorRobotArm][MOVEJ_SETTLE_CHECK] arm=" << id_
|
||||
<< ", joint=" << joint_names_[i]
|
||||
<< ", target_rad=" << target[i]
|
||||
<< ", actual_rad=" << feedback.position
|
||||
<< ", abs_position_error_rad=" << position_error
|
||||
<< ", position_tolerance_rad=" << move_j_position_tolerance_rad_
|
||||
<< ", position_ok=" << (position_ok ? "true" : "false")
|
||||
<< ", velocity_rad_s=" << feedback.velocity
|
||||
<< ", velocity_tolerance_rad_s=" << move_j_velocity_tolerance_rad_s_
|
||||
<< ", velocity_ok=" << (velocity_ok ? "true" : "false");
|
||||
}
|
||||
return Result::failure(ArmErrorCode::Timeout,
|
||||
"timed out waiting for moveJ target to settle");
|
||||
}
|
||||
|
||||
@ -5,9 +5,11 @@
|
||||
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
@ -45,7 +47,7 @@ public:
|
||||
ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。
|
||||
ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。
|
||||
ALIGN_COMPUTE_FAILED, // 对准阶段 PBVS 计算失败。
|
||||
ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。
|
||||
ALIGN_TIMEOUT, // 对准阶段或对准完成后的暂停超时。
|
||||
ALIGNING, // 正在执行视觉对准。
|
||||
ALIGN_REACHED, // 视觉对准完成。
|
||||
TOUCHING, // 正在向前触控。
|
||||
@ -61,7 +63,7 @@ public:
|
||||
};
|
||||
|
||||
explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg);
|
||||
~TouchScreenTask() = default;
|
||||
~TouchScreenTask() override;
|
||||
|
||||
bool init() override;
|
||||
bool init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
@ -133,11 +135,23 @@ private:
|
||||
void enterFailed(Status status);
|
||||
|
||||
bool updateTouchPressure();
|
||||
void logTouchPressure(bool force = false);
|
||||
device::CameraStreamOverlay buildCoordinateOverlay(
|
||||
const perception::AprilTagPerception* overlay_perception) const;
|
||||
void publishCoordinateOverlay();
|
||||
void refreshCoordinateOverlay();
|
||||
void startCoordinateOverlayWorker();
|
||||
void stopCoordinateOverlayWorker();
|
||||
void setCoordinateOverlayEnabled(bool enabled);
|
||||
void coordinateOverlayLoop(std::shared_ptr<perception::AprilTagPerception> overlay_perception);
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
// The display worker never takes mutex_ or uses the PBVS perception state.
|
||||
std::mutex coordinate_overlay_mutex_;
|
||||
std::condition_variable coordinate_overlay_cv_;
|
||||
std::thread coordinate_overlay_thread_;
|
||||
bool coordinate_overlay_stop_{false};
|
||||
bool coordinate_overlay_enabled_{true};
|
||||
std::string id_;
|
||||
std::shared_ptr<device::RobotArm> arm_{nullptr};
|
||||
std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
|
||||
@ -170,6 +184,7 @@ private:
|
||||
int last_active_tag_id_{-1};
|
||||
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_resultant_fz_{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()};
|
||||
@ -190,7 +205,7 @@ private:
|
||||
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_touch_pressure_log_time_{};
|
||||
Clock::time_point last_retract_log_time_{};
|
||||
Status final_status_after_retract_{Status::DONE};
|
||||
};
|
||||
|
||||
@ -283,6 +283,10 @@ TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg)
|
||||
}
|
||||
}
|
||||
|
||||
TouchScreenTask::~TouchScreenTask() {
|
||||
stopCoordinateOverlayWorker();
|
||||
}
|
||||
|
||||
bool TouchScreenTask::init() {
|
||||
if (!config_valid_) {
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
@ -339,6 +343,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
const std::shared_ptr<device::AbstractCamera>& external_camera) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
stopCoordinateOverlayWorker();
|
||||
arm_ = arm;
|
||||
dexhand_ = dexhand;
|
||||
camera_ = camera;
|
||||
@ -380,7 +385,6 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
|
||||
initialized_ = applyConfig();
|
||||
if (initialized_) {
|
||||
refreshCoordinateOverlay();
|
||||
tracker_.clear();
|
||||
tracker_.resetActiveTagTracking();
|
||||
tcp_pose_tracker_.clear();
|
||||
@ -395,6 +399,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
align_stable_count_ = 0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_resultant_fz_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_align_error_screen_tag_.setZero();
|
||||
touch_start_position_valid_ = false;
|
||||
@ -405,8 +410,9 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
last_T_B_G_.setIdentity();
|
||||
max_T_B_G_translation_delta_m_ = 0.0;
|
||||
max_T_B_G_rotation_delta_rad_ = 0.0;
|
||||
last_coordinate_overlay_update_time_ = Clock::time_point{};
|
||||
last_touch_pressure_log_time_ = Clock::time_point{};
|
||||
last_status_ = Status::IDLE;
|
||||
startCoordinateOverlayWorker();
|
||||
} else {
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
}
|
||||
@ -457,6 +463,7 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
|
||||
align_debug_count_ = 0;
|
||||
pbvs_debug_count_ = 0;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_resultant_fz_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_align_error_screen_tag_.setZero();
|
||||
@ -471,6 +478,8 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
|
||||
max_T_B_G_translation_delta_m_ = 0.0;
|
||||
max_T_B_G_rotation_delta_rad_ = 0.0;
|
||||
phase_ = Phase::ALIGNING;
|
||||
setCoordinateOverlayEnabled(false);
|
||||
startCoordinateOverlayWorker();
|
||||
phase_after_retract_ = Phase::DONE;
|
||||
final_status_after_retract_ = Status::DONE;
|
||||
phase_start_time_ = Clock::now();
|
||||
@ -489,22 +498,9 @@ bool TouchScreenTask::step(const double dt) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (dexhand_) {
|
||||
const bool tactile_ok = updateTouchPressure();
|
||||
// std::cout << "[TouchScreenTask][TACTILE] finger="
|
||||
// << fingerTypeToString(toFingerType(config_.touch().tactile().finger()))
|
||||
// << ", region=" << tactileRegionToString(toTactileRegion(config_.touch().tactile().region()))
|
||||
// << ", ok=" << (tactile_ok ? 1 : 0)
|
||||
// << ", nonzero_count=" << last_touch_nonzero_count_
|
||||
// << ", pressure_sum=" << last_touch_pressure_sum_
|
||||
// << std::endl;
|
||||
}
|
||||
|
||||
// Keep the presentation snapshot alive outside ALIGNING as well. This
|
||||
// path only updates the debug video overlay; it is not consumed by PBVS or
|
||||
// any motion computation.
|
||||
if (phase_ != Phase::ALIGNING) {
|
||||
refreshCoordinateOverlay();
|
||||
// TOUCHING reads and checks the same tactile sample together in stepTouching().
|
||||
if (dexhand_ && phase_ != Phase::TOUCHING) {
|
||||
updateTouchPressure();
|
||||
}
|
||||
|
||||
switch (phase_) {
|
||||
@ -516,6 +512,18 @@ bool TouchScreenTask::step(const double dt) {
|
||||
case Phase::ALIGN_REACHED:
|
||||
last_status_ = Status::ALIGN_REACHED;
|
||||
if (config_.alignment().pause_when_reached()) {
|
||||
// Entering ALIGN_REACHED resets phase_start_time_, so the
|
||||
// pause has its own timeout measured from successful alignment.
|
||||
const double paused_s =
|
||||
std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
|
||||
if (paused_s > config_.alignment().timeout_s()) {
|
||||
CMVR_LOG(WARNING) << "[TouchScreenTask][ALIGN_PAUSE_TIMEOUT]"
|
||||
<< " elapsed_s=" << paused_s
|
||||
<< ", timeout_s=" << config_.alignment().timeout_s()
|
||||
<< ", move_to_init=" << config_.initialization().after_finish();
|
||||
enterFailed(Status::ALIGN_TIMEOUT);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
if (!startTouchPhase()) {
|
||||
@ -543,12 +551,9 @@ bool TouchScreenTask::step(const double dt) {
|
||||
return false;
|
||||
}
|
||||
|
||||
void TouchScreenTask::publishCoordinateOverlay()
|
||||
device::CameraStreamOverlay TouchScreenTask::buildCoordinateOverlay(
|
||||
const perception::AprilTagPerception* overlay_perception) const
|
||||
{
|
||||
if (!external_camera_) {
|
||||
return;
|
||||
}
|
||||
|
||||
device::CameraStreamOverlay overlay;
|
||||
overlay.draw_coordinate_frames =
|
||||
!config_.has_debug_draw_coordinate_frames() ||
|
||||
@ -560,11 +565,11 @@ void TouchScreenTask::publishCoordinateOverlay()
|
||||
overlay.coordinate_axis_length_m = config_.debug_coordinate_axis_length_m();
|
||||
}
|
||||
|
||||
if (overlay.draw_coordinate_frames && external_perception_) {
|
||||
if (overlay.draw_coordinate_frames && overlay_perception) {
|
||||
const auto& tags = config_.perception().tags();
|
||||
const int screen_tag_id = tags.screen().id();
|
||||
const int hand_tag_id = tags.hand().id();
|
||||
const auto* screen_tag = external_perception_->findTag(screen_tag_id);
|
||||
const auto* screen_tag = overlay_perception->findTag(screen_tag_id);
|
||||
|
||||
if (screen_tag) {
|
||||
device::CoordinateFrameOverlay frame;
|
||||
@ -575,7 +580,7 @@ void TouchScreenTask::publishCoordinateOverlay()
|
||||
overlay.coordinate_frames.push_back(std::move(frame));
|
||||
}
|
||||
|
||||
if (const auto* hand_tag = external_perception_->findTag(hand_tag_id)) {
|
||||
if (const auto* hand_tag = overlay_perception->findTag(hand_tag_id)) {
|
||||
device::CoordinateFrameOverlay frame;
|
||||
frame.T_C_Frame = hand_tag->T_C_Tag();
|
||||
frame.label = "H";
|
||||
@ -585,36 +590,114 @@ void TouchScreenTask::publishCoordinateOverlay()
|
||||
}
|
||||
}
|
||||
|
||||
external_camera_->setStreamOverlay(overlay);
|
||||
return overlay;
|
||||
}
|
||||
|
||||
void TouchScreenTask::refreshCoordinateOverlay()
|
||||
void TouchScreenTask::publishCoordinateOverlay()
|
||||
{
|
||||
if (!external_perception_ || !external_camera_) {
|
||||
if (external_camera_) {
|
||||
external_camera_->setStreamOverlay(buildCoordinateOverlay(external_perception_.get()));
|
||||
}
|
||||
}
|
||||
|
||||
void TouchScreenTask::startCoordinateOverlayWorker()
|
||||
{
|
||||
if (coordinate_overlay_thread_.joinable() || !external_camera_ ||
|
||||
(config_.has_debug_draw_coordinate_frames() &&
|
||||
!config_.debug_draw_coordinate_frames())) {
|
||||
return;
|
||||
}
|
||||
if (config_.has_debug_draw_coordinate_frames() &&
|
||||
!config_.debug_draw_coordinate_frames()) {
|
||||
return;
|
||||
|
||||
try {
|
||||
// AprilTagPerception owns mutable frame/detector state. Never share
|
||||
// the PBVS instance with the display worker.
|
||||
auto overlay_perception =
|
||||
std::make_shared<perception::AprilTagPerception>(external_camera_);
|
||||
overlay_perception->setTagSize(config_.perception().tags().screen().size_m());
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(coordinate_overlay_mutex_);
|
||||
coordinate_overlay_stop_ = false;
|
||||
coordinate_overlay_enabled_ = phase_ != Phase::ALIGNING;
|
||||
}
|
||||
coordinate_overlay_thread_ = std::thread(
|
||||
&TouchScreenTask::coordinateOverlayLoop, this, std::move(overlay_perception));
|
||||
} catch (const std::exception& e) {
|
||||
CMVR_LOG(WARNING) << "[TouchScreenTask] Failed to start coordinate overlay worker: "
|
||||
<< e.what();
|
||||
}
|
||||
const auto now = Clock::now();
|
||||
if (last_coordinate_overlay_update_time_.time_since_epoch().count() != 0 &&
|
||||
std::chrono::duration<double>(now - last_coordinate_overlay_update_time_).count() <
|
||||
(1.0 / 30.0)) {
|
||||
return;
|
||||
}
|
||||
|
||||
void TouchScreenTask::stopCoordinateOverlayWorker()
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(coordinate_overlay_mutex_);
|
||||
coordinate_overlay_stop_ = true;
|
||||
}
|
||||
last_coordinate_overlay_update_time_ = now;
|
||||
coordinate_overlay_cv_.notify_all();
|
||||
if (coordinate_overlay_thread_.joinable()) {
|
||||
coordinate_overlay_thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
void TouchScreenTask::setCoordinateOverlayEnabled(const bool enabled)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(coordinate_overlay_mutex_);
|
||||
coordinate_overlay_enabled_ = enabled;
|
||||
}
|
||||
coordinate_overlay_cv_.notify_all();
|
||||
}
|
||||
|
||||
void TouchScreenTask::coordinateOverlayLoop(
|
||||
std::shared_ptr<perception::AprilTagPerception> overlay_perception)
|
||||
{
|
||||
perception::AprilTagPerception::Options options;
|
||||
options.depth_policy = perception::AprilTagPerception::DepthPolicy::NONE;
|
||||
options.detect_tags = true;
|
||||
options.fetch_encoded = false;
|
||||
external_perception_->update(options);
|
||||
publishCoordinateOverlay();
|
||||
const auto refresh_period =
|
||||
std::chrono::duration_cast<Clock::duration>(std::chrono::duration<double>(1.0 / 30.0));
|
||||
|
||||
std::unique_lock<std::mutex> lock(coordinate_overlay_mutex_);
|
||||
while (true) {
|
||||
coordinate_overlay_cv_.wait(lock, [this] {
|
||||
return coordinate_overlay_stop_ || coordinate_overlay_enabled_;
|
||||
});
|
||||
if (coordinate_overlay_stop_) {
|
||||
break;
|
||||
}
|
||||
const auto next_refresh = Clock::now() + refresh_period;
|
||||
lock.unlock();
|
||||
|
||||
// No task/worker lock is held during camera acquisition or tag detection.
|
||||
try {
|
||||
overlay_perception->update(options);
|
||||
const auto overlay = buildCoordinateOverlay(overlay_perception.get());
|
||||
std::lock_guard<std::mutex> publish_lock(coordinate_overlay_mutex_);
|
||||
// ALIGNING publishes its own perception snapshot. Discard an
|
||||
// in-flight display frame if alignment or shutdown has started.
|
||||
if (!coordinate_overlay_stop_ && coordinate_overlay_enabled_) {
|
||||
external_camera_->setStreamOverlay(overlay);
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
CMVR_LOG_EVERY_N(WARNING, 30)
|
||||
<< "[TouchScreenTask] Coordinate overlay refresh failed: " << e.what();
|
||||
} catch (...) {
|
||||
CMVR_LOG_EVERY_N(WARNING, 30)
|
||||
<< "[TouchScreenTask] Coordinate overlay refresh failed: unknown exception";
|
||||
}
|
||||
|
||||
lock.lock();
|
||||
coordinate_overlay_cv_.wait_until(lock, next_refresh, [this] {
|
||||
return coordinate_overlay_stop_ || !coordinate_overlay_enabled_;
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void TouchScreenTask::stop() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
stopUnlocked();
|
||||
stopCoordinateOverlayWorker();
|
||||
}
|
||||
|
||||
void TouchScreenTask::stopUnlocked() {
|
||||
@ -628,6 +711,7 @@ void TouchScreenTask::stopUnlocked() {
|
||||
pbvs_.resetTwistCommandState();
|
||||
|
||||
phase_ = Phase::IDLE;
|
||||
setCoordinateOverlayEnabled(true);
|
||||
phase_after_retract_ = Phase::DONE;
|
||||
final_status_after_retract_ = Status::DONE;
|
||||
target_locked_ = false;
|
||||
@ -637,6 +721,7 @@ void TouchScreenTask::stopUnlocked() {
|
||||
align_stable_count_ = 0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_resultant_fz_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_align_error_screen_tag_.setZero();
|
||||
locked_target_rotation_valid_ = false;
|
||||
@ -1428,6 +1513,7 @@ bool TouchScreenTask::stepAligning(const double dt) {
|
||||
<< output.rotation_error_G.z() << "]";
|
||||
stopPbvsMotion();
|
||||
phase_ = Phase::ALIGN_REACHED;
|
||||
setCoordinateOverlayEnabled(true);
|
||||
phase_start_time_ = Clock::now();
|
||||
touch_command_started_ = false;
|
||||
last_status_ = Status::ALIGN_REACHED;
|
||||
@ -1438,6 +1524,10 @@ bool TouchScreenTask::stepAligning(const double dt) {
|
||||
pbvs_command_acceleration_,
|
||||
0.0,
|
||||
device::FrameType::Base);
|
||||
// const auto speed_result = arm_->speedL({0,0,0,0,0,0},
|
||||
// pbvs_command_acceleration_,
|
||||
// 0.0,
|
||||
// device::FrameType::Base);
|
||||
if (!speed_result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTask][ALIGNING] speedL failed: "
|
||||
<< speed_result.message;
|
||||
@ -1461,15 +1551,13 @@ bool TouchScreenTask::stepTouching() {
|
||||
}
|
||||
}
|
||||
|
||||
if (!dexhand_) {
|
||||
if (!updateTouchPressure()) {
|
||||
enterFailed(Status::TACTILE_UNAVAILABLE);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Decide from the freshly read sample before logging, FK, or display work.
|
||||
if (isTouchTriggered(config_, last_touch_pressure_sum_)) {
|
||||
if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) {
|
||||
logTouchingSpeedLState();
|
||||
}
|
||||
if (!handleTouchTriggered(true)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
@ -1478,6 +1566,7 @@ bool TouchScreenTask::stepTouching() {
|
||||
}
|
||||
|
||||
if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kMoveL) {
|
||||
logTouchPressure();
|
||||
last_status_ = Status::TOUCHING;
|
||||
return true;
|
||||
}
|
||||
@ -1498,7 +1587,6 @@ bool TouchScreenTask::stepTouching() {
|
||||
const double traveled_distance =
|
||||
(current_position_base - touch_start_position_base_).norm();
|
||||
if (traveled_distance >= speed_l_max_distance_m) {
|
||||
logTouchingSpeedLState();
|
||||
if (!updateTouchPressure()) {
|
||||
enterFailed(Status::TACTILE_UNAVAILABLE);
|
||||
return false;
|
||||
@ -1510,6 +1598,8 @@ bool TouchScreenTask::stepTouching() {
|
||||
}
|
||||
return true;
|
||||
}
|
||||
logTouchPressure();
|
||||
logTouchingSpeedLState();
|
||||
if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
@ -1518,6 +1608,7 @@ bool TouchScreenTask::stepTouching() {
|
||||
}
|
||||
}
|
||||
|
||||
logTouchPressure();
|
||||
last_status_ = Status::TOUCHING;
|
||||
return true;
|
||||
}
|
||||
@ -1774,8 +1865,12 @@ bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
|
||||
}
|
||||
}
|
||||
|
||||
// Stop first: synchronous logging/FK must not delay the contact response.
|
||||
const auto triggered_time = Clock::now();
|
||||
logTouchPressure(true);
|
||||
logTouchingSpeedLState();
|
||||
phase_ = Phase::DWELLING;
|
||||
phase_start_time_ = Clock::now();
|
||||
phase_start_time_ = triggered_time;
|
||||
last_status_ = Status::TOUCH_TRIGGERED;
|
||||
return true;
|
||||
}
|
||||
@ -1788,6 +1883,7 @@ bool TouchScreenTask::startTouchPhase() {
|
||||
|
||||
phase_ = Phase::TOUCHING;
|
||||
phase_start_time_ = Clock::now();
|
||||
last_touch_pressure_log_time_ = Clock::time_point{};
|
||||
touch_command_started_ = true;
|
||||
retract_command_started_ = false;
|
||||
retract_start_position_valid_ = false;
|
||||
@ -1900,6 +1996,7 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
|
||||
void TouchScreenTask::enterFailed(const Status status) {
|
||||
stopPbvsMotion();
|
||||
phase_ = Phase::FAILED;
|
||||
setCoordinateOverlayEnabled(true);
|
||||
touch_command_started_ = false;
|
||||
retract_command_started_ = false;
|
||||
last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED;
|
||||
@ -1907,6 +2004,7 @@ void TouchScreenTask::enterFailed(const Status status) {
|
||||
|
||||
bool TouchScreenTask::updateTouchPressure() {
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_resultant_fz_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
if (!dexhand_) {
|
||||
return false;
|
||||
@ -1934,13 +2032,21 @@ bool TouchScreenTask::updateTouchPressure() {
|
||||
|
||||
last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0;
|
||||
last_touch_pressure_sum_ = resultant_value;
|
||||
if (phase_ == Phase::TOUCHING) {
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << resultant_fz
|
||||
<< ", criterion_value=" << resultant_value
|
||||
<< ", threshold=" << tactile.force_threshold()
|
||||
;
|
||||
}
|
||||
last_touch_resultant_fz_ = resultant_fz;
|
||||
return true;
|
||||
}
|
||||
|
||||
void TouchScreenTask::logTouchPressure(const bool force) {
|
||||
const auto now = Clock::now();
|
||||
if (!force && last_touch_pressure_log_time_.time_since_epoch().count() != 0 &&
|
||||
now - last_touch_pressure_log_time_ < std::chrono::milliseconds(100)) {
|
||||
return;
|
||||
}
|
||||
last_touch_pressure_log_time_ = now;
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << last_touch_resultant_fz_
|
||||
<< ", criterion_value=" << last_touch_pressure_sum_
|
||||
<< ", threshold=" << config_.touch().tactile().force_threshold()
|
||||
<< ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_);
|
||||
}
|
||||
|
||||
} // namespace cmvr::task
|
||||
|
||||
@ -83,7 +83,11 @@ message TouchScreenTaskAlignmentConfig {
|
||||
TouchScreenAlignmentTargetConfig target = 3;
|
||||
.cmvr.common.Vec6 error_threshold = 4;
|
||||
optional int32 stable_frames = 5;
|
||||
// Separate time limit for alignment and, when enabled, the pause after reaching.
|
||||
// The pause timer starts on entering ALIGN_REACHED. Either timeout fails the
|
||||
// task and attempts to return to initialization when after_finish is enabled.
|
||||
optional double timeout_s = 6;
|
||||
// Pause after alignment instead of starting touch, until timeout_s expires.
|
||||
optional bool pause_when_reached = 7;
|
||||
TouchScreenTaskPbvsConfig pbvs = 8;
|
||||
TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user