diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt index 2be40b68..16532f9f 100644 --- a/cmvr-es/config/devices/arm/arm_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -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 # 位置和速度连续满足条件的采样次数。 diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt index 038da4b6..6d3e83c8 100644 --- a/cmvr-es/config/devices/camera/camera.pb.txt +++ b/cmvr-es/config/devices/camera/camera.pb.txt @@ -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 } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 48486297..c387cc85 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -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 { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 2785290f..21293504 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -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" diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt index 5670dce6..2fdafe8b 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -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 } } diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index 5e1ffee2..2c60e3a8 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -95,7 +95,9 @@ touch_screen_task { rz: 0.08726646259971647 } stable_frames: 5 + # 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。 timeout_s: 20.0 + # 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。 pause_when_reached: false } diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 3cebe17f..16757c8c 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -1043,9 +1044,16 @@ Result MotorRobotArm::waitForJointTarget_(const std::vector& target) con "moveJ target size mismatch while settling"); } + struct JointFeedback { + double position{0.0}; + double velocity{0.0}; + }; + std::vector last_feedback(joint_names_.size()); const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration(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& 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"); } diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 6d870740..ae3fffd1 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -5,9 +5,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -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& 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 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 arm_{nullptr}; std::shared_ptr 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}; }; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 772215ca..433e236f 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -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& arm, const std::shared_ptr& camera, const std::shared_ptr& external_camera) { std::lock_guard lock(mutex_); + stopCoordinateOverlayWorker(); arm_ = arm; dexhand_ = dexhand; camera_ = camera; @@ -380,7 +385,6 @@ bool TouchScreenTask::init(const std::shared_ptr& 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& 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& 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(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(external_camera_); + overlay_perception->setTagSize(config_.perception().tags().screen().size_m()); + { + std::lock_guard 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(now - last_coordinate_overlay_update_time_).count() < - (1.0 / 30.0)) { - return; +} + +void TouchScreenTask::stopCoordinateOverlayWorker() +{ + { + std::lock_guard 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 lock(coordinate_overlay_mutex_); + coordinate_overlay_enabled_ = enabled; + } + coordinate_overlay_cv_.notify_all(); +} + +void TouchScreenTask::coordinateOverlayLoop( + std::shared_ptr 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(std::chrono::duration(1.0 / 30.0)); + + std::unique_lock 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 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 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 diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto index 5ea1823e..588d603e 100644 --- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -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;