SWCreateAvatarInterface::~SWCreateAvatarInterface()
{
// stop threads
m_oKinectThread.stopListening();
m_TCreateAvatar.quit();
m_TCreateAvatar.wait();
// delete worker and ui
deleteAndNullify(m_WCreateAvatar);
deleteAndNullify(m_uiCreateAvatar);
}
SWViewerInterface::~SWViewerInterface()
{
m_TViewer.quit();
m_TViewer.wait();
deleteAndNullify(m_pWViewer);
}
void CmdInterpreter::visit(ProxyCmdCloseDatabase &cmd)
{
DbHandle *pDbHandle = getDbHandle(cmd.getDbHandle());
pDbHandle->pResourceGovernor.reset();
AutoBacktrace::setTraceTarget();
deleteAndNullify(pDbHandle);
}
void CmdInterpreter::visit(ProxyCmdRollback &cmd)
{
TxnHandle *pTxnHandle = getTxnHandle(cmd.getTxnHandle());
SharedDatabase pDb = pTxnHandle->pDb;
// block checkpoints during this method
bool txnBlocksCheckpoint = !pTxnHandle->readOnly && pDb->shouldForceTxns();
SXMutexSharedGuard actionMutexGuard(
pDb->getCheckpointThread()->getActionMutex());
if (pDb->areSnapshotsEnabled()) {
SnapshotRandomAllocationSegment *pSegment =
SegmentFactory::dynamicCast<SnapshotRandomAllocationSegment *>(
pTxnHandle->pSnapshotSegment);
pSegment->rollbackChanges();
}
if (cmd.getSvptHandle()) {
SavepointId svptId = getSavepointId(cmd.getSvptHandle());
pTxnHandle->pTxn->rollback(&svptId);
} else {
pTxnHandle->pTxn->rollback();
deleteAndNullify(pTxnHandle);
if (txnBlocksCheckpoint && !pDb->areSnapshotsEnabled()) {
// Implement rollback by simulating crash recovery,
// reverting all pages modified by transaction. No need
// to do this when snapshots are in use because no permanent
// pages were modified.
pDb->recoverOnline();
}
}
}
swDetect::SWFaceDetection_thread::~SWFaceDetection_thread()
{
if(m_bListening)
{
stopDetection();
}
deleteAndNullify(m_pFaceDetection);
}
extern "C" JNIEXPORT void JNICALL
Java_net_sf_farrago_fennel_FennelStorage_deleteExecutionHandle(
JNIEnv *pEnvInit, jclass, jlong handle)
{
CmdInterpreter::ExecutionHandle &execHandle =
CmdInterpreter::getExecutionHandleFromLong(handle);
CmdInterpreter::ExecutionHandle *pExecHandle = &execHandle;
JniUtil::decrementHandleCount(EXECHANDLE_TRACE_TYPE_STR, pExecHandle);
deleteAndNullify(pExecHandle);
}
swTeleop::SWIcubArm::~SWIcubArm()
{
if(m_pVelocityController)
{
while(m_pVelocityController->isRunning())
{
yarp::os::Time::delay(0.1);
}
}
deleteAndNullify(m_pVelocityController);
}
void CmdInterpreter::visit(ProxyCmdCommit &cmd)
{
TxnHandle *pTxnHandle = getTxnHandle(cmd.getTxnHandle());
SharedDatabase pDb = pTxnHandle->pDb;
// block checkpoints during this method
bool txnBlocksCheckpoint = !pTxnHandle->readOnly && pDb->shouldForceTxns();
SXMutexSharedGuard actionMutexGuard(
pDb->getCheckpointThread()->getActionMutex());
if (pDb->areSnapshotsEnabled()) {
// Commit the current txn, and start a new one so the versioned
// pages that we're now going to commit will be marked with a txnId
// corresponding to the time of the commit. At present, those pages
// are marked with a txnId corresponding to the start of the txn.
pTxnHandle->pTxn->commit();
pTxnHandle->pTxn = pDb->getTxnLog()->newLogicalTxn(pDb->getCache());
SnapshotRandomAllocationSegment *pSnapshotSegment =
SegmentFactory::dynamicCast<SnapshotRandomAllocationSegment *>(
pTxnHandle->pSnapshotSegment);
TxnId commitTxnId = pTxnHandle->pTxn->getTxnId();
pSnapshotSegment->commitChanges(commitTxnId);
// Flush pages associated with the snapshot segment. Note that we
// don't need to flush the underlying versioned segment first since
// the snapshot pages are all new and therefore, are never logged.
// Pages in the underlying versioned segment will be flushed in the
// requestCheckpoint call further below. Also note that the
// checkpoint is not initiated through the dynamically cast segment
// to ensure that the command is traced if tracing is turned on.
if (txnBlocksCheckpoint) {
pTxnHandle->pSnapshotSegment->checkpoint(CHECKPOINT_FLUSH_ALL);
}
}
if (cmd.getSvptHandle()) {
SavepointId svptId = getSavepointId(cmd.getSvptHandle());
pTxnHandle->pTxn->commitSavepoint(svptId);
} else {
pTxnHandle->pTxn->commit();
deleteAndNullify(pTxnHandle);
if (txnBlocksCheckpoint) {
// release the checkpoint lock acquired above
actionMutexGuard.unlock();
// force a checkpoint now to flush all data modified by transaction
// to disk; wait for it to complete before reporting the
// transaction as committed
pDb->requestCheckpoint(CHECKPOINT_FLUSH_ALL, false);
}
}
}
void IoCompletionPortScheduler::stop()
{
assert(isStarted());
quit = true;
// post dummy wakeup notifications; threads will see these and
// exit
for (uint i = 0; i < threads.size(); ++i) {
if (!PostQueuedCompletionStatus(hCompletionPort, 0, 0, NULL)) {
throw SysCallExcn("PostQueuedCompletionStatus failed");
}
}
for (uint i = 0; i < threads.size(); ++i) {
threads[i]->join();
deleteAndNullify(threads[i]);
}
threads.clear();
}
bool SWTeleoperation_nao::close()
{
resetHeadPosition();
resetTorsoPosition();
resetLeftArmPosition();
resetRightArmPosition();
// close ports
m_oHeadTrackerPort.close();
m_oTorsoTrackerPort.close();
m_oLeftArmTrackerPort.close();
m_oRightArmTrackerPort.close();
m_oFaceTrackerPort.close();
// set stiffnesses
if(m_bHeadActivated)
{
m_oRobotMotionProxy->setStiffnesses("Head", 0.0f);
}
if(m_bTorsoActivated)
{
m_oRobotMotionProxy->setStiffnesses("Torso", 0.0f);
}
if(m_bLeftArmActivated)
{
m_oRobotMotionProxy->setStiffnesses("LArm", 0.0f);
}
if(m_bRightArmActivated)
{
m_oRobotMotionProxy->setStiffnesses("RArm", 0.0f);
}
// set ending posture
ALRobotPostureProxy l_oRobotRobotPosture= ALRobotPostureProxy(m_sRobotAddress);
try
{
if(m_endPosture == 0)
{
l_oRobotRobotPosture.goToPosture("Crouch", 1.0);
}
else if(m_endPosture == 1)
{
l_oRobotRobotPosture.goToPosture("Stand", 1.0);
}
else if(m_endPosture == 2)
{
l_oRobotRobotPosture.goToPosture("Sit", 1.0);
}
}
catch (const AL::ALError& e)
{
std::cerr << "-ERROR: " << e.what() << std::endl;
deleteAndNullify(m_oRobotMotionProxy);
return false;
}
deleteAndNullify(m_oRobotMotionProxy);
return true;
}
void SWCreateAvatarInterface::updateDisplay()
{
if(!m_oKinectThread.isDataAvailable())
{
qWarning() << "Kinect data not available, display disactivated. ";
return;
}
// get the current image from the kinect
cv::Mat l_oBGR, l_oCloud;
l_oBGR = m_oKinectThread.bgrImage().clone();
l_oCloud = m_oKinectThread.cloudMap().clone();
// check size mat
if(l_oBGR.rows != l_oCloud.rows)
{
cv::resize(l_oBGR, l_oBGR, cv::Size(l_oCloud.cols,l_oCloud.rows));
}
// remove background
cv::Mat l_oBGRForeGround = swImage::swUtil::removeBackground(l_oBGR, l_oCloud);
m_oMutex.lockForRead();
bool l_bDoDetect = !m_bWorkStarted;
cv::Rect l_oFaceRectangle = m_oFaceRect;
cv::Rect l_oNoseRectangle = m_oNoseRect;
m_oMutex.unlock();
if(l_bDoDetect)
{
// detect
m_CFaceDetectPtr->detectFace(l_oBGRForeGround);
// cloud
swCloud::SWCloud *l_pCloudToDisplay = new swCloud::SWCloud();
if(m_CFaceDetectPtr->faceRect().width > 0)
{
cv::Mat l_oFaceCloudMat = l_oCloud(m_CFaceDetectPtr->faceRect());
swCloud::convCloudMat2SWCloud(l_oFaceCloudMat, l_oBGR(m_CFaceDetectPtr->faceRect()), *l_pCloudToDisplay,0.f,1.2f);
if(l_pCloudToDisplay->size() > 0)
{
m_WCloudGL->setCloud(l_pCloudToDisplay);
}
else
{
deleteAndNullify(l_pCloudToDisplay);
}
}
else
{
deleteAndNullify(l_pCloudToDisplay);
}
// display image
m_CFaceDetectPtr->displayFace(l_oBGR, cv::Scalar(0,255,0));
}
else if(l_oFaceRectangle.width > 0)
{
// display face rectangle
if(swUtil::isInside(l_oFaceRectangle, l_oBGR))
{
cv::rectangle(l_oBGR, cv::Point(l_oFaceRectangle.x, l_oFaceRectangle.y),
cv::Point(l_oFaceRectangle.x+l_oFaceRectangle.width, l_oFaceRectangle.y+l_oFaceRectangle.height), RED,1);
}
// display nose rectangle
if(swUtil::isInside(l_oNoseRectangle,l_oBGR))
{
cv::rectangle(l_oBGR, cv::Point(l_oNoseRectangle.x, l_oNoseRectangle.y),
cv::Point(l_oNoseRectangle.x + l_oNoseRectangle.width, l_oNoseRectangle.y + l_oNoseRectangle.height), RED,1);
}
// display stasm feature points
for(uint ii = 0; ii < m_vP2IStasmPoints.size(); ++ii)
{
cv::circle(l_oBGR, m_vP2IStasmPoints[ii], 1, BLUE);
}
// display num cloud
std::ostringstream l_osNumCloud, l_osTotalCloud;
l_osNumCloud << m_i32NumCloud;
l_osTotalCloud << m_uiCreateAvatar->sbClouds->value();
swUtil::displayTextDownRight(l_oBGR, std::string(l_osNumCloud.str() + " / " + l_osTotalCloud.str() ));
}
// update display image widget
m_WRGBDisplay->refreshDisplay(swConv::mat2QImage(l_oBGR));
}
