void mtsTeleOperation::ClutchEventHandler(const prmEventButton & button)
{
mtsExecutionResult executionResult;
executionResult = Master.GetPositionCartesian(Master.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "EventHandlerClutched: call to Master.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
}
executionResult = Slave.GetPositionCartesian(Slave.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "EventHandlerClutched: call to Slave.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
}
if (button.Type() == prmEventButton::PRESSED) {
this->IsClutched = true;
Master.PositionCartesianDesired.Goal().Rotation().FromNormalized(
Slave.PositionCartesianCurrent.Position().Rotation());
Master.PositionCartesianDesired.Goal().Translation().Assign(
Master.PositionCartesianCurrent.Position().Translation());
MessageEvents.Status(this->GetName() + ": master clutch pressed");
} else {
this->IsClutched = false;
MessageEvents.Status(this->GetName() + ": master clutch released");
}
SetMasterControlState();
}
void RegistrationBehavior::PrimaryMasterButtonCallback(const prmEventButton & event)
{
if (this->Manager->MastersAsMice() &&
event.Type() == prmEventButton::RELEASED) {
prmPositionCartesianGet position;
this->GetPrimaryMasterPosition(position);
size_t fiducialID;
if (VirtualFiducials.size() < NUM_FIDUCIALS) {
fiducialID = VirtualFiducials.size();
VirtualFiducials.resize(fiducialID + 1);
VirtualFiducials[fiducialID] = this->Widget3D->GetAbsoluteTransformation().Inverse() *
position.Position().Translation();
//RegistrationModelFiducials * modelFiducial = new RegistrationModelFiducials(fiducialID);
//this->ModelFiducials->Add(modelFiducial);
//this->ModelFiducials->SetPosition(vct3(0.0));
//this->ModelFiducials->GetLast()->SetPosition(VirtualFiducials[fiducialID]);
//this->ModelFiducials->GetLast()->Show();
CMN_LOG_CLASS_RUN_VERBOSE << "PrimaryMasterButtonCallback: added virtual fiducial: "
<< VirtualFiducials[fiducialID] << std::endl;
} else if (RealFiducials.size() < NUM_FIDUCIALS) {
fiducialID = RealFiducials.size();
RealFiducials.resize(fiducialID + 1);
RealFiducials[fiducialID] = this->Widget3D->GetAbsoluteTransformation().Inverse() *
position.Position().Translation();
CMN_LOG_CLASS_RUN_VERBOSE << "PrimaryMasterButtonCallback: added real fiducial: "
<< RealFiducials[fiducialID] << std::endl;
}
}
}
void mtsIntuitiveResearchKitConsole::CameraEventHandler(const prmEventButton & button)
{
ConsoleEvents.Camera(button);
if (button.Type() == prmEventButton::PRESSED) {
MessageEvents.Status(this->GetName() + ": camera pressed");
} else {
MessageEvents.Status(this->GetName() + ": camera released");
}
}
void mtsIntuitiveResearchKitConsole::OperatorPresentEventHandler(const prmEventButton & button)
{
ConsoleEvents.OperatorPresent(button);
if (button.Type() == prmEventButton::PRESSED) {
MessageEvents.Status(this->GetName() + ": operator present");
} else {
MessageEvents.Status(this->GetName() + ": operator not present");
}
}
void mtsIntuitiveResearchKitPSM::EventHandlerSUJClutch(const prmEventButton &button)
{
// Pass events
EventTriggers.SUJClutch(button);
// Log events
if (button.Type() == prmEventButton::PRESSED) {
CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": EventHandlerSUJClutch: pressed" << std::endl;
} else {
CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": EventHandlerSUJClutch: released" << std::endl;
}
}
void mtsIntuitiveResearchKitPSM::EventHandlerTool(const prmEventButton &button)
{
if (button.Type() == prmEventButton::PRESSED) {
SetState(PSM_ENGAGING_TOOL);
} else {
// this is "down" transition so we have to
// make sure we had a tool properly engaged before
if (RobotState >= PSM_READY) {
SetState(PSM_ADAPTER_ENGAGED);
}
}
}
void mtsIntuitiveResearchKitPSM::EventHandlerAdapter(const prmEventButton &button)
{
if (button.Type() == prmEventButton::PRESSED) {
SetState(PSM_ENGAGING_ADAPTER);
} else {
// this is "down" transition so we have to
// make sure we had an adapter properly engaged before
if (RobotState >= PSM_ADAPTER_ENGAGED) {
SetState(PSM_ARM_CALIBRATED);
}
}
}
void mtsTeleOperation::OperatorPresentEventHandler(const prmEventButton & button)
{
if (button.Type() == prmEventButton::PRESSED) {
this->IsOperatorPresent = true;
MessageEvents.Status(this->GetName() + ": operator present");
CMN_LOG_CLASS_RUN_DEBUG << "EventHandlerOperatorPresent: OperatorPresent pressed" << std::endl;
} else {
this->IsOperatorPresent = false;
MessageEvents.Status(this->GetName() + ": operator not present");
CMN_LOG_CLASS_RUN_DEBUG << "EventHandlerOperatorPresent: OperatorPresent released" << std::endl;
}
SetMasterControlState();
}
void mtsTeleOperation::CameraClutchEventHandler(const prmEventButton & button)
{
if (button.Type() == prmEventButton::PRESSED) {
Slave.IsManipClutched = true;
MessageEvents.Status(this->GetName() + ": camera clutch pressed");
} else {
Slave.IsManipClutched = false;
MessageEvents.Status(this->GetName() + ": camera clutch released");
}
// Align master
StartAlignMaster();
}
void prmQtWidgetEventButtonsComponent_ButtonData::EventHandler(const prmEventButton & payload)
{
Counter++;
QString counterString;
counterString.setNum(Counter);
QString payloadString;
if (payload.Type() == prmEventButton::PRESSED) {
payloadString = "pressed";
} else {
payloadString = "released";
}
// see default format in ctor
emit SetValueSignal(QString("%1: %2 [%3][%4]").arg(Name.c_str(), payloadString, counterString, QTime::currentTime().toString("hh:mm:ss")));
}
void mtsTeleOperationECM::ClutchEventHandler(const prmEventButton & button)
{
switch (button.Type()) {
case prmEventButton::PRESSED:
mIsClutched = true;
break;
case prmEventButton::RELEASED:
mIsClutched = false;
break;
default:
break;
}
// if the teleoperation is activated
if (mTeleopState.DesiredState() == "ENABLED") {
Clutch(mIsClutched);
}
}
void mtsTeleOperation::SlaveClutchEventHandler(const prmEventButton & button)
{
if (button.Type() == prmEventButton::PRESSED) {
Slave.IsManipClutched = true;
MessageEvents.Status(this->GetName() + ": slave clutch pressed");
} else {
Slave.IsManipClutched = false;
MessageEvents.Status(this->GetName() + ": slave clutch released");
}
// Slave State
if (IsEnabled && !IsOperatorPresent && Slave.IsManipClutched) {
Slave.SetRobotControlState(mtsStdString("Manual"));
} else if (IsEnabled) {
Slave.SetRobotControlState(mtsStdString("Teleop"));
}
// Align master
StartAlignMaster();
}
void mtsIntuitiveResearchKitPSM::EventHandlerManipClutch(const prmEventButton & button)
{
// Pass events
ClutchEvents.ManipClutch(button);
// Start manual mode but save the previous state
if (button.Type() == prmEventButton::PRESSED) {
ClutchEvents.ManipClutchPreviousState = this->RobotState;
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL);
} else {
if (RobotState == mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL) {
// Enable PID
PID.Enable(true);
// set command joint position to joint current
JointSet.ForceAssign(JointGet);
SetPositionJointLocal(JointSet);
// go back to state before clutching
SetState(ClutchEvents.ManipClutchPreviousState);
}
}
}
