添加管控软件和相机的通讯
This commit is contained in:
@@ -31,6 +31,7 @@
|
||||
#include <QPointer>
|
||||
#include <QTimer>
|
||||
#include <QCoreApplication>
|
||||
#include <QSignalBlocker>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
@@ -345,8 +346,8 @@ void MeasurementOptionsPanel::start() {
|
||||
s.finalPose=selected.pose;s.finalSide=CAM_LEFT;s.valid=selected.pose.valid;record["timings_ms"]=timings;
|
||||
if(self)QMetaObject::invokeMethod(self.data(),[self,s,record,selected,generation,clock,deadlineMs]()mutable{if(!self||self->m_generation!=generation)return;auto times=record["timings_ms"].toObject();if(clock->elapsed()>=deadlineMs){s.valid=false;s.finalPose.valid=false;selected.pose.valid=false;selected.source="NONE";selected.error=QString::fromUtf8("位姿发布前已超时。");record["timed_out"]=true;times["pose_ready_ms"]=QJsonValue();}else if(s.valid)times["pose_ready_ms"]=static_cast<double>(clock->elapsed());record["timings_ms"]=times;self->complete(s,record,selected);},Qt::QueuedConnection);
|
||||
});
|
||||
m_worker=worker;connect(worker,&QThread::finished,this,[this,worker]{if(m_worker==worker)m_worker=nullptr;m_legacy->endExternalMeasurement();setBusy(false);worker->deleteLater();});worker->start();
|
||||
QTimer::singleShot(std::max(0,deadlineMs-int(clock->elapsed())),Qt::PreciseTimer,this,[this,generation,deadlineMs]{if(!m_busy||m_generation!=generation||m_record["generation"].toDouble()==static_cast<double>(generation))return;m_timedOut=true;m_status->setText(QString::fromUtf8("达到%1秒截止,返回失败;等待视觉引导算子退出…").arg(deadlineMs/1000));m_snapshot.valid=false;m_draw->setEnabled(false);m_grasp->setEnabled(false);emit logMessage(QString::fromUtf8("[测量][超时] 迟到结果将丢弃;算子退出前保持资源锁。"));});
|
||||
m_worker=worker;connect(worker,&QThread::finished,this,[this,worker]{if(m_worker==worker)m_worker=nullptr;m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};worker->deleteLater();});worker->start();
|
||||
QTimer::singleShot(std::max(0,deadlineMs-int(clock->elapsed())),Qt::PreciseTimer,this,[this,generation,deadlineMs]{if(!m_busy||m_generation!=generation||m_record["generation"].toDouble()==static_cast<double>(generation))return;m_timedOut=true;m_status->setText(QString::fromUtf8("达到%1秒截止,返回失败;等待视觉引导算子退出…").arg(deadlineMs/1000));m_snapshot.valid=false;m_draw->setEnabled(false);m_grasp->setEnabled(false);remoteFailure("DEADLINE","Measurement deadline reached; waiting for the operator to exit");emit logMessage(QString::fromUtf8("[测量][超时] 迟到结果将丢弃;算子退出前保持资源锁。"));});
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::complete(MeasurementCaptureSnapshot snapshot,QJsonObject record,const MeasurementSelection& selected){
|
||||
@@ -358,10 +359,118 @@ void MeasurementOptionsPanel::complete(MeasurementCaptureSnapshot snapshot,QJson
|
||||
auto metrics=m_record["fusion"].toObject();metrics["position_limit_mm"]=1.0;metrics["reprojection_checked"]=snapshot.fusion.reprojectionChecked;metrics["reprojection_passed"]=snapshot.fusion.reprojectionPassed;metrics["left_reprojection_error_px"]=snapshot.fusion.leftReprojectionErrorPx;metrics["right_reprojection_error_px"]=snapshot.fusion.rightReprojectionErrorPx;metrics["reprojection_limit_px"]=snapshot.fusion.reprojectionLimitPx;m_record["fusion"]=metrics;
|
||||
if(!snapshot.rightImage.isNull())emit logMessage(QString::fromUtf8("[测量][双目] %1").arg(snapshot.fusion.message));
|
||||
if(!snapshot.valid)emit logMessage(QString::fromUtf8("[测量][失败原因] %1;左路:%2;右路:%3").arg(record["error"].toString(),snapshot.leftPose.errorMsg,snapshot.rightPose.errorMsg));
|
||||
// Publish structured values before drawing/archiving; neither may trigger another capture.
|
||||
if(!m_remoteRequest.isEmpty())completeRemote(snapshot,m_record);
|
||||
if(snapshot.leftPose.valid||snapshot.rightPose.valid)renderResult();
|
||||
writeArchive(m_record,m_snapshot,QImage(),false);
|
||||
}
|
||||
|
||||
bool MeasurementOptionsPanel::remoteReady(QString* error) const
|
||||
{
|
||||
try {
|
||||
if(!m_camera||!m_camera->leftConnected()||!m_camera->rightConnected())throw std::runtime_error("Both cameras must be connected");
|
||||
if(!m_matcher||!m_matcher->isReady(CAM_LEFT)||!m_yolo||!m_yolo->isLoaded())throw std::runtime_error("LEFT calibration/model or YOLO is not loaded");
|
||||
const auto cal=effectiveCalibration();
|
||||
if(cal["robot_rotation_rule"].toString()!="Rz(A) Ry(B) Rx(C)"||cal["robot_tool"].toString()!="TOOL1"||cal["robot_reference_frame_human_confirmed"].toString()!="world")throw std::runtime_error("Verified world/TOOL1 Rz(A)Ry(B)Rx(C) calibration required");
|
||||
jsonMatrix(cal["T_TOOL1_camera_mm"]);jsonMatrix(cal["T_grasp_TOOL1_from_one_teach_mm"]);
|
||||
robotTransform(cal["teach_reference_XYZABC_mm_deg"].toArray());
|
||||
const auto normal=cal["tray_normal_world"].toArray();
|
||||
if(normal.size()!=3)throw std::runtime_error("Tray normal missing");
|
||||
double norm=0;for(auto v:normal){if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("Invalid tray normal");norm+=v.toDouble()*v.toDouble();}
|
||||
if(norm<.81)throw std::runtime_error("Invalid tray normal");
|
||||
if(usesTeachPlane()){robotTransform(m_activeTeach);}
|
||||
return true;
|
||||
} catch(const std::exception& e){if(error)*error=QString::fromUtf8(e.what());return false;}
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::remoteFailure(const QString& code,const QString& message)
|
||||
{
|
||||
if(m_remoteRequest.isEmpty()||m_remoteTerminal)return;
|
||||
m_remoteTerminal=true;
|
||||
QJsonObject result{{"type","measure"},{"request_id",m_remoteRequest["request_id"]},
|
||||
{"context",m_remoteRequest["context"]},{"status",m_remoteCancelled?"cancelled":(code=="DEADLINE"?"timeout":"error")},
|
||||
{"camera_valid",false},{"robot_valid",false},{"error_code",code},{"message",message}};
|
||||
if(!m_remoteCancelled&&code=="GRASP")for(auto it=m_remoteCameraResult.begin();it!=m_remoteCameraResult.end();++it)result[it.key()]=it.value();
|
||||
emit remoteFinished(result);
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::startRemote(const QJsonObject& request)
|
||||
{
|
||||
if(!m_legacy||m_busy||m_legacy->isBusy()){
|
||||
emit remoteFinished(QJsonObject{{"type","measure"},{"request_id",request["request_id"]},{"context",request["context"]},{"status","error"},{"error_code","BUSY"},{"message","A measurement is already running"}});return;
|
||||
}
|
||||
m_remoteRequest=request;m_remoteTerminal=false;m_remoteCancelled=false;m_remoteCameraResult={};
|
||||
try {
|
||||
const auto parameters=request["parameters"].toObject();
|
||||
if(parameters["camera_mode"].toString()!="left_only"||parameters["refinement"].toString()!="least_squares_high"||parameters["deadline_ms"].toInt()!=5000||
|
||||
parameters["min_score"].toDouble()!=.70||parameters["greediness"].toDouble()!=.80||parameters["yolo_selection"].toString()!="center"||
|
||||
parameters["pose_display"].toString()!="model_origin"||parameters["grasp_offset_mm"].toDouble()!=4.40)throw std::runtime_error("Unsupported measurement parameters");
|
||||
const auto convention=request["convention"].toObject();
|
||||
if(convention["frame"].toString()!="world"||convention["tool"].toString()!="TOOL1"||convention["rotation"].toString()!="Rz(A)Ry(B)Rx(C)"||convention["linear_unit"].toString()!="mm"||convention["angular_unit"].toString()!="deg")throw std::runtime_error("Robot pose convention mismatch");
|
||||
if(!request["require_robot_pose"].toBool()||(request["pose_source"].toString()!="plc_r40"&&request["pose_source"].toString()!="diagnostic"))throw std::runtime_error("Robot pose source required");
|
||||
m_remoteRobot=request["robot_pose"].toArray();robotTransform(m_remoteRobot);
|
||||
const auto context=request["context"].toObject();
|
||||
if(context["workpiece_id"].toString().isEmpty()||context["task_id"].toString().isEmpty()||context["generation"].toString().isEmpty())throw std::runtime_error("Complete measurement context required");
|
||||
const int tray=context["tray_no"].toInt();
|
||||
if(tray<1||tray>99||context["tray_no"].toDouble()!=tray)throw std::runtime_error("Invalid tray number");
|
||||
if(tray!=m_currentTrayId){
|
||||
const auto profile=m_trayProfiles[QString::number(tray)];
|
||||
if(!profile.isObject())throw std::runtime_error("Requested tray has no saved setup");
|
||||
m_trayProfiles[QString::number(m_currentTrayId)]=traySetupState();
|
||||
restoreTraySetup(profile.toObject());m_currentTrayId=tray;
|
||||
QSignalBlocker blocker(m_traySelector);m_traySelector->setCurrentIndex(m_traySelector->findData(tray));
|
||||
}
|
||||
QString error;if(!remoteReady(&error))throw std::runtime_error(error.toUtf8().constData());
|
||||
// Freeze the exact robot pose and effective tray setup before capture. Display rounding is irrelevant.
|
||||
m_remoteCalibration=effectiveCalibration();m_remoteTeach=m_activeTeach;m_remoteHeightMethod=selectedHeightMethod();
|
||||
m_mode->setCurrentIndex(0);m_refinement->setCurrentIndex(m_refinement->findData("least_squares_high"));m_deadline->setValue(5);
|
||||
m_minScore->setValue(.70);m_greediness->setValue(.80);m_yoloSelection->setCurrentIndex(0);m_posePoint->setCurrentIndex(0);m_offset->setValue(4.4);
|
||||
m_remoteFingerprint=QString::fromLatin1(QCryptographicHash::hash(QJsonDocument(QJsonObject{{"calibration",m_remoteCalibration},{"tray",traySetupState()},{"camera_sha256",m_matcher->cameraParamSha256(CAM_LEFT)},{"model_sha256",shaFile(m_matcher->shapeModelFile(CAM_LEFT))},{"offset_mm",4.4}}).toJson(QJsonDocument::Compact),QCryptographicHash::Sha256).toHex());
|
||||
for(int i=0;i<6;++i)m_robot[i]->setText(QString::number(m_remoteRobot[i].toDouble(),'g',17));
|
||||
start();
|
||||
if(!m_busy&&!m_remoteTerminal)remoteFailure("START_FAILED","Measurement did not start");
|
||||
if(!m_busy)m_remoteRequest={};
|
||||
}catch(const VisionGuideApi::HException& e){remoteFailure("MEASURE",QString::fromLocal8Bit(e.ErrorMessage().TextA()));if(!m_worker){m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};}}
|
||||
catch(const std::exception& e){remoteFailure("VALIDATION",QString::fromUtf8(e.what()));if(!m_worker){m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};}}
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::cancelRemote()
|
||||
{
|
||||
if(m_remoteRequest.isEmpty())return;
|
||||
m_remoteCancelled=true;remoteFailure("CANCELLED","Measurement result invalidated");
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::completeRemote(const MeasurementCaptureSnapshot& snapshot,const QJsonObject& record)
|
||||
{
|
||||
if(m_remoteTerminal)return;
|
||||
if(m_remoteCancelled){remoteFailure("CANCELLED","Measurement result invalidated");return;}
|
||||
if(!snapshot.valid||!snapshot.leftPose.valid||record["output_source"].toString()!="LEFT"){
|
||||
remoteFailure(record["timed_out"].toBool()?"DEADLINE":"MEASURE",record["error"].toString("LEFT measurement failed"));return;
|
||||
}
|
||||
QElapsedTimer clock;clock.start();
|
||||
try {
|
||||
const auto camera=array6(snapshot.leftPose);for(auto v:camera)if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("Non-finite camera pose");
|
||||
if(!std::isfinite(snapshot.leftPose.score)||snapshot.leftPose.score<.70)throw std::runtime_error("LEFT score is below MinScore");
|
||||
m_remoteCameraResult={{"camera_valid",true},{"camera_pose",camera},{"source","LEFT"},{"pose_type","model_origin"},{"camera_rotation_type",snapshot.leftPose.poseType},{"score",snapshot.leftPose.score}};
|
||||
QJsonObject grasp;
|
||||
const QJsonArray reference=m_remoteTeach.size()==6?QJsonArray{m_remoteTeach[0],m_remoteTeach[1],m_remoteTeach[2]}:QJsonArray();
|
||||
if(m_remoteHeightMethod=="keep_visual_xy_solve_z")grasp=graspTargetKeepingXY(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,reference,4.4);
|
||||
else if(m_remoteHeightMethod=="camera_ray_plane_intersection")grasp=graspTargetOnPlane(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,reference,4.4);
|
||||
else if(m_remoteHeightMethod=="visual_3d")grasp=graspTarget(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,4.4);
|
||||
else throw std::runtime_error("Unsupported tray height method");
|
||||
const auto robot=grasp["XYZABC_mm_deg"].toArray();robotTransform(robot);
|
||||
m_record["photo_robot_XYZABC_mm_deg"]=m_remoteRobot;m_record["photo_robot_pair_confirmed"]=true;m_record["photo_robot_pairing_method"]="ipc_request_snapshot";
|
||||
m_record["grasp_targets"]=QJsonObject{{"left",grasp}};
|
||||
for(int i=0;i<6;++i)m_values->setItem(3,i,new QTableWidgetItem(QString::number(robot[i].toDouble(),'f',3)));
|
||||
m_remoteTerminal=true;
|
||||
emit remoteFinished(QJsonObject{{"type","measure"},{"request_id",m_remoteRequest["request_id"]},{"context",m_remoteRequest["context"]},{"status","ok"},
|
||||
{"camera_pose",camera},{"robot_pose",robot},{"camera_valid",true},{"robot_valid",true},{"source","LEFT"},{"pose_type","model_origin"},{"camera_rotation_type",snapshot.leftPose.poseType},
|
||||
{"score",snapshot.leftPose.score},{"parameters",m_remoteRequest["parameters"]},{"convention",m_remoteRequest["convention"]},{"calibration_fingerprint",m_remoteFingerprint},{"run_id",record["run_id"]},
|
||||
{"capture_begin",record["capture_begin_utc"]},{"capture_end",record["capture_end_utc"]},{"measure_elapsed_ms",record["timings_ms"].toObject()["pose_ready_ms"]},{"grasp_elapsed_ms",double(clock.elapsed())}});
|
||||
}catch(const VisionGuideApi::HException& e){remoteFailure("GRASP",QString::fromLocal8Bit(e.ErrorMessage().TextA()));}
|
||||
catch(const std::exception& e){remoteFailure("GRASP",QString::fromUtf8(e.what()));}
|
||||
}
|
||||
|
||||
void MeasurementOptionsPanel::renderResult(){
|
||||
if(!m_matcher)return;
|
||||
auto draw=[&](VisionPoseResult pose,const QImage& input,CameraSide side,const QString& label){
|
||||
|
||||
Reference in New Issue
Block a user