bool REIXSSpectrometer::stop()
{
if(!canStop())
return false;
if(moveInProgress()) {
moveAction_->cancel();
/// \todo Actually, have to flag that a stop has started, and also catch when the stop is finished... Motors will take a while to actually receive and decelerate.
moveAction_->deleteLater();
moveAction_ = 0;
emit moveFailed(AMControl::WasStoppedFailure);
AMErrorMon::report(AMErrorReport(this, AMErrorReport::Alert, AMControl::WasStoppedFailure, "Spectrometer Move Stopped."));
}
// just in case anything was moving from outside our instructions (ie: commanded from somewhere else in the building)
spectrometerRotationDrive_->stop();
detectorTranslation_->stop();
detectorTiltDrive_->stop();
endstationTranslation_->stop();
// hexapod: cannot stop without wrecking init. Don't worry for now... just let it stop over time. Not necessary for it to be not-moving before we re-send it somewhere new.
/// \todo Actually, have to flag that a stop has started, and also catch when the stop is finished... Motors will take a while to actually receive and decelerate.
return true;
}
bool BioXASSSRLMonochromatorEnergyControl::stop()
{
bool result = false;
if (canStop())
result = bragg_->stop();
return result;
}
// Tell the motor to stop. (Note: For safety, this will send the stop instruction whether we think we're moving or not.)
bool AMPVwStatusControl::stop() {
if(!canStop())
return false;
stopPV_->setValue(stopValue_);
stopInProgress_ = true; // flag that a stop is "in progress" -- we've issued the stop command.
moveInProgress_ = false; // one of "our" moves is no longer in progress.
startInProgress_ = false;
settlingInProgress_ = false;
return true;
}
bool CLSMAXvMotor::stop(){
if(!canStop())
return false;
if(usingKill_)
killPV_->setValue(stopValue_);
else
stopPV_->setValue(stopValue_);
stopInProgress_ = true; // flag that a stop is "in progress" -- we've issued the stop command.
moveInProgress_ = false; // one of "our" moves is no longer in progress.
return true;
}
void SvdTransformOptimization::optimizeTransform(
CalibrationState& calibrationState) {
tf::Transform currentCameraToHead = getInitialCameraToHead();
this->lastError = INFINITY;
/* std::cout << "initial origin " << currentCameraToHead.getOrigin().getX() << ","
<< currentCameraToHead.getOrigin().getY() << ","
<< currentCameraToHead.getOrigin().getZ() << ";";
std::cout << "initial rotation " << currentCameraToHead.getRotation().getX() << ","
<< currentCameraToHead.getRotation().getY() << ","
<< currentCameraToHead.getRotation().getZ() << ";" << endl;*/
int numOfPoints = measurePoints.size();
// Initialize pointcloud in Frame A (e.g. first camera frame)
std::vector<tf::Vector3> pointcloudX;
for (int i = 0; i < numOfPoints; i++) {
MeasurePoint currentMeasure = measurePoints[i];
tf::Vector3 currentPointCamera = currentMeasure.getOpticalToCamera()
* currentMeasure.measuredPosition;
pointcloudX.push_back(currentPointCamera);
}
// Optimization loop:
while (!canStop()) {
// 1a) Use current transformations to transform the measured ball positions into the fixed (r_sole) frame.
// 1b) Calculate the center of the transformed points.
float centerX = 0, centerY = 0, centerZ = 0;
for (int i = 0; i < numOfPoints; i++) {
MeasurePoint currentMeasure = measurePoints[i];
tf::Vector3 currentPointMeasure = currentMeasure.measuredPosition;
tf::Transform opticalToFixed = currentMeasure.opticalToFixed(
CalibrationState(currentCameraToHead, 0, 0));
tf::Vector3 currentPointFixed = opticalToFixed
* currentPointMeasure;
centerX += currentPointFixed.getX();
centerY += currentPointFixed.getY();
centerZ += currentPointFixed.getZ();
}
tf::Vector3 centerPointFixed(centerX / numOfPoints,
centerY / numOfPoints, centerZ / numOfPoints);
// 2) Transform point from 1) to HeadPitch using transformations from measurements.
std::vector<tf::Vector3> pointcloudP;
for (int i = 0; i < numOfPoints; i++) {
MeasurePoint currentMeasure = measurePoints[i];
tf::Transform headPitchToFixed = currentMeasure.headToFixed(
CalibrationState(currentCameraToHead, 0, 0));
tf::Vector3 currentPointHead = (headPitchToFixed.inverse())
* centerPointFixed;
pointcloudP.push_back(currentPointHead);
}
// 3) Use pointcloud from 2) and measured points and apply SVD to calculate the new transformation Camera to HeadPitch
tf::Transform newTransform = svdPCL(pointcloudX, pointcloudP);
// 4) Update, outputs, statistical...
currentCameraToHead = newTransform;
this->lastError = error;
calculateSqrtDistCameraHead(currentCameraToHead, error);
/*
std::cout << "[optimization]";
std::cout << "iteration " << numOfIterations << ";";
std::cout << "position " << centerPointFixed.getX() << ","
<< centerPointFixed.getY() << "," << centerPointFixed.getZ()
<< ";";
std::cout << "origin " << currentCameraToHead.getOrigin().getX() << ","
<< currentCameraToHead.getOrigin().getY() << ","
<< currentCameraToHead.getOrigin().getZ() << ";";
double rr, rp, ry;
tf::Matrix3x3(currentCameraToHead.getRotation()).getRPY(rr, rp, ry);
std::cout << "rotation " << rr << "," << rp << "," << ry << ";";
std::cout << "error " << error << ";";
std::cout << std::endl;*/
}
calibrationState.setCameraToHead(currentCameraToHead);
calibrationState.setHeadPitchOffset(0.0);
calibrationState.setHeadYawOffset(0.0);
}
void BoardDriverShutdown(void)
{
sdStop(&SD2);
canStop(&CAND1);
}
void CanStopLocal(void){
canStop(&CAND1);
}
