void mtsMedtronicStealthlinkExampleComponent::AddStealthlinkInterface()
{
mtsInterfaceRequired * required = AddInterfaceRequired("Stealthlink");
if (required) {
#ifndef SAW_MEDTRONIC_WITHOUT_STEALTHLINK_TYPES
required->AddFunction("GetTool", Stealthlink.GetTool);
required->AddFunction("GetFrame", Stealthlink.GetFrame);
#endif
}
// the following two tools are using names normally defined in config.xml
AddToolInterface("Pointer", Pointer);
AddToolInterface("Frame", Frame);
// get registration information
required = AddInterfaceRequired("Registration");
if (required) {
required->AddFunction("GetTransformation", Registration.GetTransformation);
required->AddFunction("GetPredictedAccuracy", Registration.GetPredictedAccuracy);
required->AddFunction("GetValid", Registration.GetValid);
}
// get exam information
required = AddInterfaceRequired("ExamInformation");
if (required) {
required->AddFunction("RequestExamInformation", ExamInformation.RequestExamInformation);
required->AddFunction("GetVoxelScale", ExamInformation.GetVoxelScale);
required->AddFunction("GetSize", ExamInformation.GetSize);
required->AddFunction("GetValid", ExamInformation.GetValid);
}
}
mtsOSGBody::mtsOSGBody( const std::string& name,
const std::string& model,
osaOSGWorld* world,
const vctFrame4x4<double>& Rt,
double scale,
double alpha,
const vctFrame4x4<double>& Rtoffset,
const std::string& option ):
mtsComponent( name ),
body( model, world, Rt, scale, alpha, Rtoffset, option ){
// Create the IO interface and add read/write commands
body.input = AddInterfaceRequired( "Input", MTS_OPTIONAL );
if( body.input )
{ body.input->AddFunction( "GetPositionCartesian", body.GetPosition ); }
else{
CMN_LOG_CLASS_RUN_ERROR << "Failed to create the interface Input"
<< std::endl;
}
body.output = AddInterfaceRequired( "Output", MTS_OPTIONAL );
if( body.output )
{ body.output->AddFunction( "SetPositionCartesian", body.SetPosition ); }
else{
CMN_LOG_CLASS_RUN_ERROR << "Failed to create the interface Input"
<< std::endl;
}
}
guiTask::guiTask(const char *name) : mtsTaskContinuous(name){
mtsInterfaceRequired * required = AddInterfaceRequired("guiRequiresNDI");
if (required) {
required->AddFunction("Beep", NDI.Beep);
required->AddFunction("PortHandlesInitialize", NDI.Initialize);
required->AddFunction("PortHandlesQuery", NDI.Query);
required->AddFunction("PortHandlesEnable", NDI.Enable);
required->AddFunction("CalibratePivot", NDI.CalibratePivot);
required->AddFunction("ToggleTracking", NDI.Track);
required->AddFunction("ReportStrayMarkers", NDI.ReportStrayMarkers);
}
mtsInterfaceRequired * requiredRobotBase = AddInterfaceRequired("guiRequiresRobotBase");
if (requiredRobotBase) {
requiredRobotBase->AddFunction("GetPosition", RobotBase.GetPosition);
}
mtsInterfaceRequired * requiredRobotTip = AddInterfaceRequired("guiRequiresRobotTip");
if (requiredRobotTip) {
requiredRobotTip->AddFunction("GetPosition", RobotTip.GetPosition);
}
mtsInterfaceProvided * provided = AddInterfaceProvided("guiProvidesClarity");
if (provided) {
provided->AddCommandRead(&guiTask::GetRobotBasePosition, this, "GetRobotBasePosition", vct7());
provided->AddCommandRead(&guiTask::GetRobotTipPosition, this, "GetRobotTipPosition", vct7());
}
trackToggle = true;
robotBasePosition.SetAll(0.0);
robotTipPosition.SetAll(0.0);
}
void mtsIntuitiveResearchKitPSM::Init(void)
{
// main initialization from base type
mtsIntuitiveResearchKitArm::Init();
// initialize trajectory data
JointTrajectory.Velocity.SetAll(90.0 * cmnPI_180); // degrees per second
JointTrajectory.Velocity.Element(2) = 0.2; // m per second
JointTrajectory.Acceleration.SetAll(90.0 * cmnPI_180);
JointTrajectory.Acceleration.Element(2) = 0.2; // m per second
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
// high values for engage adapter/tool until these use a proper trajectory generator
PotsToEncodersTolerance.SetAll(100.0 * cmnPI_180); // 100 degrees for rotations
PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm
// for tool/adapter engage procedure
EngagingJointSet.SetSize(NumberOfJoints());
mtsInterfaceRequired * interfaceRequired;
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton());
RobotInterface->AddEventWrite(ClutchEvents.SUJClutch, "SUJClutch", prmEventButton());
// Event Adapter engage: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("Adapter");
if (interfaceRequired) {
Adapter.IsPresent = false;
interfaceRequired->AddFunction("GetButton", Adapter.GetButton);
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerAdapter, this, "Button");
}
// Event Tool engage: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("Tool");
if (interfaceRequired) {
Tool.IsPresent = false;
interfaceRequired->AddFunction("GetButton", Tool.GetButton);
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerTool, this, "Button");
}
// ManipClutch: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("ManipClutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerManipClutch, this, "Button");
}
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetJawPosition, this, "SetJawPosition");
// Initialize the optimizer
Optimizer = new mtsIntuitiveResearchKitOptimizer(6);
Optimizer->InitializeFollowVF(6,
"FollowVFSlave",
"CurrentSlaveKinematics",
"DesiredSlaveKinematics");
}
WAMprobe() : mtsTaskPeriodic( "WAMprobe", 0.002, true ){
mtsInterfaceRequired* input = AddInterfaceRequired( "Input" );
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
input->AddFunction( "GetPositionJoint", GetPositions );
output->AddFunction( "SetTorqueJoint", SetTorques );
}
BHMotion() : mtsTaskPeriodic( "BHMotion", 0.001, true ){
q.SetSize(4);
q.SetAll(0.0);
mtsInterfaceRequired* input = AddInterfaceRequired( "Input" );
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
input->AddFunction( "GetPositionJoint", GetPositions );
output->AddFunction( "SetPositionJoint", SetPositions );
}
////------------ Private Methods ----------------
void mtsRobotIO1394QtWidget::SetupCisstInterface(void)
{
// Required Interface
mtsInterfaceRequired * robotInterface = AddInterfaceRequired("Robot");
if (robotInterface) {
robotInterface->AddFunction("GetSerialNumber", Robot.GetSerialNumber);
robotInterface->AddFunction("GetPeriodStatistics", Robot.GetPeriodStatistics);
robotInterface->AddFunction("IsValid", Robot.IsValid);
robotInterface->AddFunction("EnablePower", Robot.EnablePower);
robotInterface->AddFunction("DisablePower", Robot.DisablePower);
robotInterface->AddFunction("GetPosition", Robot.GetPosition);
robotInterface->AddFunction("GetVelocity", Robot.GetVelocity);
robotInterface->AddFunction("GetAnalogInputVolts", Robot.GetAnalogInputVolts);
robotInterface->AddFunction("GetAnalogInputPosSI", Robot.GetAnalogInputPosSI);
robotInterface->AddFunction("GetActuatorRequestedCurrent", Robot.GetActuatorRequestedCurrent);
robotInterface->AddFunction("GetActuatorFeedbackCurrent", Robot.GetActuatorFeedbackCurrent);
robotInterface->AddFunction("GetActuatorCurrentMax", Robot.GetActuatorCurrentMax);
robotInterface->AddFunction("GetActuatorAmpTemperature", Robot.GetActuatorAmpTemperature);
robotInterface->AddFunction("GetJointType", Robot.GetJointType);
robotInterface->AddFunction("GetPowerStatus", Robot.GetPowerStatus);
robotInterface->AddFunction("GetSafetyRelay", Robot.GetSafetyRelay);
robotInterface->AddFunction("SetBrakeAmpEnable", Robot.SetBrakeAmpEnable);
robotInterface->AddFunction("GetBrakeAmpStatus", Robot.GetBrakeAmpStatus);
robotInterface->AddFunction("GetBrakeRequestedCurrent", Robot.GetBrakeRequestedCurrent);
robotInterface->AddFunction("GetBrakeFeedbackCurrent", Robot.GetBrakeFeedbackCurrent);
robotInterface->AddFunction("GetBrakeAmpTemperature", Robot.GetBrakeAmpTemperature);
robotInterface->AddFunction("BrakeRelease", Robot.BrakeRelease);
robotInterface->AddFunction("BrakeEngage", Robot.BrakeEngage);
robotInterface->AddFunction("SetActuatorCurrent", Robot.SetActuatorCurrent);
robotInterface->AddFunction("SetEncoderPosition", Robot.SetEncoderPosition);
robotInterface->AddFunction("SetWatchdogPeriod", Robot.SetWatchdogPeriod);
robotInterface->AddFunction("BiasEncoder", Robot.BiasEncoder);
// make sure the events are queued
robotInterface->AddEventHandlerWrite(&mtsRobotIO1394QtWidget::PowerStatusEventHandler, this, "PowerStatus");
robotInterface->AddEventHandlerWrite(&mtsRobotIO1394QtWidget::WatchdogStatusEventHandler, this, "WatchdogStatus");
}
mtsInterfaceRequired * actuatorInterface = AddInterfaceRequired("RobotActuators");
if (actuatorInterface) {
actuatorInterface->AddFunction("GetPositionActuator", Actuators.GetPositionActuator);
actuatorInterface->AddFunction("EnableBoardsPower", Actuators.EnableBoardsPower);
actuatorInterface->AddFunction("DisableBoardsPower", Actuators.DisableBoardsPower);
actuatorInterface->AddFunction("SetAmpEnable", Actuators.SetAmpEnable);
actuatorInterface->AddFunction("GetAmpEnable", Actuators.GetAmpEnable);
actuatorInterface->AddFunction("GetAmpStatus", Actuators.GetAmpStatus);
}
}
WAMMotion( double period ) :
mtsTaskPeriodic( "WAMMotion", period, true ){
q.SetSize(7);
q.SetAll(0.0);
mtsInterfaceRequired* input = AddInterfaceRequired( "Input" );
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
input->AddFunction( "GetPositionJoint", GetPositions );
output->AddFunction( "SetPositionJoint", SetPositions );
}
bool mtsIntuitiveResearchKitConsole::AddArmInterfaces(Arm * arm)
{
// IO
const std::string interfaceNameIO = "IO-" + arm->Name();
arm->IOInterfaceRequired = AddInterfaceRequired(interfaceNameIO);
if (arm->IOInterfaceRequired) {
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
} else {
CMN_LOG_CLASS_INIT_ERROR << "AddArmInterfaces: failed to add IO interface for arm \""
<< arm->Name() << "\"" << std::endl;
return false;
}
// PID
if (arm->mType != Arm::ARM_SUJ) {
const std::string interfaceNamePID = "PID-" + arm->Name();
arm->PIDInterfaceRequired = AddInterfaceRequired(interfaceNamePID);
if (arm->PIDInterfaceRequired) {
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
} else {
CMN_LOG_CLASS_INIT_ERROR << "AddArmInterfaces: failed to add PID interface for arm \""
<< arm->Name() << "\"" << std::endl;
return false;
}
}
// arm interface
const std::string interfaceNameArm = arm->Name();
arm->ArmInterfaceRequired = AddInterfaceRequired(interfaceNameArm);
if (arm->ArmInterfaceRequired) {
arm->ArmInterfaceRequired->AddFunction("SetRobotControlState", arm->SetRobotControlState);
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
// for ECM, we need to know when clutched so we can tell teleops to update master orientation
if (arm->mType == Arm::ARM_ECM) {
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ECMManipClutchEventHandler, this, "ManipClutch");
}
} else {
CMN_LOG_CLASS_INIT_ERROR << "AddArmInterfaces: failed to add Main interface for arm \""
<< arm->Name() << "\"" << std::endl;
return false;
}
return true;
}
mtsTeleOperationPSMQtWidget::mtsTeleOperationPSMQtWidget(const std::string & componentName, double periodInSeconds):
mtsComponent(componentName),
TimerPeriodInMilliseconds(periodInSeconds * 1000), // Qt timers are in milliseconds
LogEnabled(false)
{
// Setup cisst interface
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("TeleOperation");
if (interfaceRequired) {
interfaceRequired->AddFunction("SetScale", TeleOperation.SetScale);
interfaceRequired->AddFunction("LockRotation", TeleOperation.LockRotation);
interfaceRequired->AddFunction("LockTranslation", TeleOperation.LockTranslation);
interfaceRequired->AddFunction("GetPositionCartesianMTM", TeleOperation.GetPositionCartesianMTM);
interfaceRequired->AddFunction("GetPositionCartesianPSM", TeleOperation.GetPositionCartesianPSM);
interfaceRequired->AddFunction("GetRegistrationRotation", TeleOperation.GetRegistrationRotation);
interfaceRequired->AddFunction("GetPeriodStatistics", TeleOperation.GetPeriodStatistics);
// events
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::DesiredStateEventHandler,
this, "DesiredState");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::CurrentStateEventHandler,
this, "CurrentState");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::ScaleEventHandler,
this, "Scale");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::RotationLockedEventHandler,
this, "RotationLocked");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::TranslationLockedEventHandler,
this, "TranslationLocked");
// messages
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::ErrorEventHandler,
this, "Error");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::WarningEventHandler,
this, "Warning");
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationPSMQtWidget::StatusEventHandler,
this, "Status");
}
}
mtsMicronTrackerControllerQtComponent::mtsMicronTrackerControllerQtComponent(const std::string & taskName) :
mtsComponent(taskName)
{
ControllerWidget = new Ui::mtsMicronTrackerControllerQtWidget();
ControllerWidget->setupUi(&CentralWidget);
MTC.FrameLeft.SetSize(FrameSize);
MTC.FrameRight.SetSize(FrameSize);
FrameIndexed8 = QImage(FrameWidth, FrameHeight, QImage::Format_Indexed8);
Timer = new QTimer(this);
MTC.XPoints.resize(50);
MTC.XPointsProjectionLeft.resize(50);
MTC.XPointsProjectionRight.resize(50);
mtsInterfaceRequired * required = AddInterfaceRequired("Controller");
if (required) {
required->AddFunction("ToggleCapturing", MTC.Capture);
required->AddFunction("ToggleTracking", MTC.Track);
required->AddFunction("GetCameraFrameLeft", MTC.GetFrameLeft);
required->AddFunction("GetCameraFrameRight", MTC.GetFrameRight);
required->AddFunction("GetXPointsMaxNum", MTC.GetXPointsMaxNum);
required->AddFunction("GetXPoints", MTC.GetXPoints);
required->AddFunction("GetXPointsProjectionLeft", MTC.GetXPointsProjectionLeft);
required->AddFunction("GetXPointsProjectionRight", MTC.GetXPointsProjectionRight);
}
// connect Qt signals to slots
QObject::connect(ControllerWidget->ButtonTrack, SIGNAL(toggled(bool)),
this, SLOT(MTCTrackQSlot(bool)));
QObject::connect(ControllerWidget->ButtonScreenshot, SIGNAL(clicked()),
this, SLOT(ScreenshotQSlot()));
QObject::connect(this->Timer, SIGNAL(timeout()),
this, SLOT(UpdateFrames()));
}
void mtsIntuitiveResearchKitECM::Init(void)
{
// main initialization from base type
mtsIntuitiveResearchKitArm::Init();
// initialize trajectory data
JointTrajectory.Velocity.Assign(30.0 * cmnPI_180, // degrees per second
30.0 * cmnPI_180,
0.05, // m per second
20.0 * cmnPI_180);
JointTrajectory.Acceleration.Assign(30.0 * cmnPI_180,
30.0 * cmnPI_180,
0.05,
30.0 * cmnPI_180);
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations
PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm
mtsInterfaceRequired * interfaceRequired;
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton());
// ManipClutch: digital input button event from ECM
interfaceRequired = AddInterfaceRequired("ManipClutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitECM::EventHandlerManipClutch, this, "Button");
}
}
void mtsMedtronicStealthlinkExampleComponent::AddToolInterface(const std::string & toolName,
mtsMedtronicStealthlinkExampleComponent::ToolStruct & functionSet)
{
mtsInterfaceRequired * required = AddInterfaceRequired(toolName, MTS_OPTIONAL);
if (required) {
required->AddFunction("GetPositionCartesian", functionSet.GetPositionCartesian);
required->AddFunction("GetMarkerCartesian", functionSet.GetMarkerCartesian);
}
}
bool mtsIntuitiveResearchKitConsole::AddFootpedalInterfaces(void)
{
// Footpedal events, receive
mtsInterfaceRequired * clutchRequired = AddInterfaceRequired("Clutch");
if (clutchRequired) {
clutchRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ClutchEventHandler, this, "Button");
} else {
return false;
}
mtsInterfaceRequired * cameraRequired = AddInterfaceRequired("Camera");
if (cameraRequired) {
cameraRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::CameraEventHandler, this, "Button");
} else {
return false;
}
mtsInterfaceRequired * headRequired = AddInterfaceRequired("OperatorPresent");
if (headRequired) {
headRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::OperatorPresentEventHandler, this, "Button");
} else {
return false;
}
// Console events, send
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Clutch");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.Clutch, "Button", prmEventButton());
} else {
return false;
}
interfaceProvided = AddInterfaceProvided("Camera");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.Camera, "Button", prmEventButton());
} else {
return false;
}
interfaceProvided = AddInterfaceProvided("OperatorPresent");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.OperatorPresent, "Button", prmEventButton());
} else {
return false;
}
return true;
}
void mtsMedtronicStealthlinkExampleComponent::AddStateCollectionInterface()
{
mtsInterfaceRequired * required;
required = AddInterfaceRequired("CollectorState");
if (required) {
required->AddFunction("StartCollection", CollectorState.StartCollection);
required->AddFunction("StopCollection", CollectorState.StopCollection);
}
}
// Constructor
CANclient() :
mtsTaskContinuous( "client" ), // task name
cnt(0) { // reset counter
// Required interface with Read/Write function
mtsInterfaceRequired* IO = AddInterfaceRequired( "IO" );
if( IO != NULL ){
IO->AddFunction( "Write", write );
IO->AddFunction( "Read", read );
}
// Required interface with Read/Write function
mtsInterfaceRequired* CTL = AddInterfaceRequired( "CTL" );
if( CTL != NULL ){
CTL->AddFunction( "Open", open );
CTL->AddFunction( "Close", close );
}
}
// main constructor
mtsOSGManipulatorTask::mtsOSGManipulatorTask( const std::string& name,
double period,
osaOSGManipulator* manipulator,
osaCPUMask cpumask,
int priority,
InputType inputtype ) :
mtsTaskPeriodic( name, period, true ),
manipulator( manipulator ),
inputtype( inputtype ),
inputp( NULL ),
inputr( NULL ),
output( NULL ),
ctl( NULL ),
cpumask( cpumask ),
priority( priority ){
if( inputtype == mtsOSGManipulatorTask::PROVIDE_INPUT ){
inputp = AddInterfaceProvided( "Input" );
if( inputp ){
StateTable.AddData( prmqin, "PositionJointInput" );
inputp->AddCommandWriteState( StateTable, prmqin, "SetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
}
else{
inputr = AddInterfaceRequired( "Input" );
if( inputr )
{ inputr->AddFunction( "GetPositionJoint", GetPositionJoint ); }
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
}
output = AddInterfaceProvided( "Output" );
if( output ){
StateTable.AddData( prmqout, "PositionJointOutput" );
StateTable.AddData( prmRtout, "PositionCartesianOutput" );
output->AddCommandReadState( StateTable, prmRtout, "GetPositionCartesian" );
output->AddCommandReadState( StateTable, prmqout, "GetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
mtsSystemQtWidgetComponent::mtsSystemQtWidgetComponent(const std::string & componentName, double periodInSeconds):
mtsSystemQtWidget(componentName),
mtsComponent(componentName),
TimerPeriodInMilliseconds(periodInSeconds * 1000)
{
// Setup CISST Interface
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("Component");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPeriodStatistics", GetPeriodStatistics);
mtsSystemQtWidget::SetInterfaceRequired(interfaceRequired);
}
setupUi();
}
void ireTask::Initialize(void)
{
EnableDynamicComponentManagement();
// For receiving system-wide logs
mtsInterfaceRequired * required = AddInterfaceRequired(
mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerRequired,
MTS_OPTIONAL);
if (required) {
required->AddEventHandlerWrite(&ireTask::Log, this,
mtsManagerComponentBase::EventNames::PrintLog);
}
SetInitializationDelay(30.0); // Allow up to 30 seconds for it to start
}
clientTask::clientTask(const std::string & taskName, double period):
mtsTaskPeriodic(taskName, period, false, 5000),
BenchmarkDoneMember(false),
NumberOfskippedElement(0),
SamplesCollected(0)
{
// to communicate with the interface of the resource
mtsInterfaceRequired * required = AddInterfaceRequired("Required1");
if (required) {
required->AddFunction("Write", this->WriteServer);
required->AddFunction("Read", this->ReadServer);
}
required = AddInterfaceRequired("Required2");
if (required) {
required->AddFunction("TriggerEvent", this->TriggerEvent);
required->AddEventHandlerWrite(&clientTask::EventWriteHandler, this, "EventWrite");
}
// Get a pointer to the time server
this->TimeServer = &mtsTaskManager::GetInstance()->GetTimeServer();
// Allocates space for samples
this->Samples.SetSize(confNumberOfSamples);
}
bool mtsIntuitiveResearchKitConsole::SetupAndConnectInterfaces(Arm * arm)
{
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
// create interfaces
const std::string interfaceNameIO = "IO" + arm->Name();
arm->IOInterfaceRequired = AddInterfaceRequired(interfaceNameIO);
const std::string interfaceNamePID = "PID" + arm->Name();
arm->PIDInterfaceRequired = AddInterfaceRequired(interfaceNamePID);
const std::string interfaceNameArm = arm->Name();
arm->ArmInterfaceRequired = AddInterfaceRequired(interfaceNameArm);
// check if all interfaces are correct
if (arm->ArmInterfaceRequired && arm->PIDInterfaceRequired && arm->IOInterfaceRequired) {
// IO
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->IOInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
componentManager->Connect(this->GetName(), interfaceNameIO,
arm->IOComponentName(), arm->Name());
// PID
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->PIDInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
componentManager->Connect(this->GetName(), interfaceNamePID,
arm->PIDComponentName(), "Controller");
// arm interface
arm->ArmInterfaceRequired->AddFunction("SetRobotControlState", arm->SetRobotControlState);
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ErrorEventHandler, this, "Error");
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::WarningEventHandler, this, "Warning");
arm->ArmInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::StatusEventHandler, this, "Status");
componentManager->Connect(this->GetName(), interfaceNameArm,
arm->Name(), "Robot");
return true;
}
return false;
}
// ====================== Constructors and Destructors ========================
// ----------------------------------------------------------------------------
mtsTestComponentClient::mtsTestComponentClient(const std::string & componentName):
mtsComponent(componentName)
{
// Required Interface
mtsInterfaceRequired* required = AddInterfaceRequired("ControllerClient");
if (required) {
required->AddFunction("TestFunction1", TestComp.TestFunction1);
required->AddFunction("TestFunction2", TestComp.TestFunction2);
required->AddFunction("TestFunction3", TestComp.TestFunction3);
required->AddFunction("TestFunction4", TestComp.TestFunction4);
required->AddFunction("TestFunction5", TestComp.TestFunction5);
required->AddFunction("TestFunction6", TestComp.TestFunction6);
}
}
mtsIntuitiveResearchKitConsoleQtWidget::mtsIntuitiveResearchKitConsoleQtWidget(const std::string & componentName):
mtsComponent(componentName)
{
mtsInterfaceRequired * interfaceRequiredMain = AddInterfaceRequired("Main");
if (interfaceRequiredMain) {
interfaceRequiredMain->AddFunction("SetRobotsControlState", Console.SetRobotsControlState);
interfaceRequiredMain->AddFunction("TeleopEnable", Console.TeleopEnable);
interfaceRequiredMain->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsoleQtWidget::ErrorEventHandler,
this, "Error");
interfaceRequiredMain->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsoleQtWidget::WarningEventHandler,
this, "Warning");
interfaceRequiredMain->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsoleQtWidget::StatusEventHandler,
this, "Status");
}
setupUi();
}
clientTask::clientTask(const std::string & taskName, double period):
mtsTaskPeriodic(taskName, period, false, 5000),
NumberOfSamplesSkipped(0),
NumberOfSamplesCollected(0),
BenchmarkCompleted(false)
{
// to communicate with the interface of the resource
mtsInterfaceRequired * requiredInterface = AddInterfaceRequired("Required");
if (requiredInterface) {
requiredInterface->AddFunction("QualifiedRead", QualifiedReadFunction);
}
// Get a pointer to the time server
TimeServer = &mtsTaskManager::GetInstance()->GetTimeServer();
// Reserve memory space for results
Results.SetSize(confNumberOfSamples);
}
clientTask<_dataType>::clientTask(const std::string & taskName):
clientTaskBase(taskName, 50.0 * cmn_ms)
{
// to communicate with the interface of the resource
mtsInterfaceRequired * required = AddInterfaceRequired("Required");
if (required) {
required->AddFunction("Void", this->Void);
required->AddFunction("Write", this->Write);
required->AddFunction("Read", this->Read);
required->AddFunction("QualifiedRead", this->QualifiedRead);
required->AddFunction("VoidSlow", this->VoidSlow);
required->AddFunction("WriteSlow", this->WriteSlow);
required->AddFunction("VoidReturn", this->VoidReturn);
required->AddFunction("WriteReturn", this->WriteReturn);
required->AddEventHandlerVoid(&clientTask<_dataType>::EventVoidHandler, this, "EventVoid");
required->AddEventHandlerWrite(&clientTask<_dataType>::EventWriteHandler, this, "EventWrite");
}
}
void mtsPIDQtWidget::Init(void)
{
PID.StateJoint.Position().SetSize(NumberOfAxis);
PID.StateJoint.Velocity().SetSize(NumberOfAxis);
PID.StateJoint.Effort().SetSize(NumberOfAxis);
PID.StateJointDesired.Position().SetSize(NumberOfAxis);
PID.StateJointDesired.Velocity().SetSize(0);
PID.StateJointDesired.Effort().SetSize(NumberOfAxis);
DesiredPosition.SetSize(NumberOfAxis);
DesiredPosition.SetAll(0.0);
UnitFactor.SetSize(NumberOfAxis);
UnitFactor.SetAll(1.0);
DirectControl = false;
PlotIndex = 0;
// Setup cisst interface
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("Controller");
if (interfaceRequired) {
interfaceRequired->AddFunction("ResetController", PID.ResetController);
interfaceRequired->AddFunction("Enable", PID.Enable);
interfaceRequired->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint);
interfaceRequired->AddFunction("GetStateJoint", PID.GetStateJoint);
interfaceRequired->AddFunction("GetStateJointDesired", PID.GetStateJointDesired);
interfaceRequired->AddFunction("GetJointType", PID.GetJointType);
interfaceRequired->AddFunction("GetPGain", PID.GetPGain);
interfaceRequired->AddFunction("GetDGain", PID.GetDGain);
interfaceRequired->AddFunction("GetIGain", PID.GetIGain);
interfaceRequired->AddFunction("SetPGain", PID.SetPGain);
interfaceRequired->AddFunction("SetDGain", PID.SetDGain);
interfaceRequired->AddFunction("SetIGain", PID.SetIGain);
// Events
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::JointLimitEventHandler, this, "JointLimit");
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::ErrorEventHandler, this, "Error");
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::EnableEventHandler, this, "Enabled");
}
setupUi();
startTimer(TimerPeriodInMilliseconds); // ms
}
mtsIntuitiveResearchKitArmQtWidget::mtsIntuitiveResearchKitArmQtWidget(const std::string & componentName, double periodInSeconds):
mtsComponent(componentName),
TimerPeriodInMilliseconds(periodInSeconds)
{
DirectControl = false;
// Setup CISST Interface
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("Manipulator");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPositionCartesian", Arm.GetPositionCartesian);
interfaceRequired->AddFunction("GetRobotControlState", Arm.GetRobotControlState);
interfaceRequired->AddFunction("SetRobotControlState", Arm.SetRobotControlState);
interfaceRequired->AddFunction("GetPeriodStatistics", Arm.GetPeriodStatistics);
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitArmQtWidget::ErrorEventHandler,
this, "Error");
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitArmQtWidget::WarningEventHandler,
this, "Warning");
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitArmQtWidget::StatusEventHandler,
this, "Status");
}
setupUi();
startTimer(TimerPeriodInMilliseconds); // ms
}
SetPoints( const std::string& robotfilename,
const vctFrame4x4<double>& Rtw0,
const vctDynamicVector<double>& qinit ) :
mtsTaskPeriodic( "setpoint", 0.1, true ),
manipulator( NULL ),
q( qinit ),
t( 0.0 ){
manipulator = new robManipulator( robotfilename, Rtw0 );
Rt = manipulator->ForwardKinematics( q );
x = Rt[0][3];
mtsInterfaceRequired* output = AddInterfaceRequired( "Output" );
if( output ){
output->AddFunction( "SetPositionCartesian", SetPositionCartesian );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
bool mtsManagerComponentServer::AddNewClientProcess(const std::string & clientProcessName)
{
if (InterfaceGCMFunctionMap.FindItem(clientProcessName)) {
CMN_LOG_CLASS_INIT_VERBOSE << "AddNewClientProcess: process is already known" << std::endl;
return true;
}
// Create a new set of function objects
InterfaceGCMFunctionType * newFunctionSet = new InterfaceGCMFunctionType;
const std::string interfaceName = mtsManagerComponentBase::GetNameOfInterfaceGCMRequiredFor(clientProcessName);
mtsInterfaceRequired * required = AddInterfaceRequired(interfaceName);
if (!required) {
CMN_LOG_CLASS_INIT_ERROR << "AddNewClientProcess: failed to create \"GCM\" required interface: " << interfaceName << std::endl;
return false;
}
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentCreate,
newFunctionSet->ComponentCreate);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentConfigure,
newFunctionSet->ComponentConfigure);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentConnect,
newFunctionSet->ComponentConnect);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentDisconnect,
newFunctionSet->ComponentDisconnect);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentStart,
newFunctionSet->ComponentStart);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentStop,
newFunctionSet->ComponentStop);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentResume,
newFunctionSet->ComponentResume);
required->AddFunction(mtsManagerComponentBase::CommandNames::ComponentGetState,
newFunctionSet->ComponentGetState);
required->AddFunction(mtsManagerComponentBase::CommandNames::GetInterfaceProvidedDescription,
newFunctionSet->GetInterfaceProvidedDescription);
required->AddFunction(mtsManagerComponentBase::CommandNames::GetInterfaceRequiredDescription,
newFunctionSet->GetInterfaceRequiredDescription);
required->AddFunction(mtsManagerComponentBase::CommandNames::LoadLibrary,
newFunctionSet->LoadLibrary);
required->AddFunction(mtsManagerComponentBase::CommandNames::SetLogForwarding,
newFunctionSet->SetLogForwarding);
required->AddFunction(mtsManagerComponentBase::CommandNames::GetLogForwardingState,
newFunctionSet->GetLogForwardingState);
required->AddFunction(mtsManagerComponentBase::CommandNames::GetAbsoluteTimeInSeconds,
newFunctionSet->GetAbsoluteTimeInSeconds);
required->AddFunction(mtsManagerComponentBase::CommandNames::GetListOfComponentClasses,
newFunctionSet->GetListOfComponentClasses);
required->AddEventHandlerWrite(&mtsManagerComponentServer::HandleChangeStateEvent, this,
mtsManagerComponentBase::EventNames::ChangeState);
// Remember a required interface (InterfaceGCM's required interface) to
// connect it to the provided interface (InterfaceLCM's provided interface).
if (!InterfaceGCMFunctionMap.AddItem(clientProcessName, newFunctionSet)) {
CMN_LOG_CLASS_INIT_ERROR << "AddNewClientProcess: failed to add \"GCM\" required interface: "
<< "\"" << clientProcessName << "\", " << interfaceName << std::endl;
return false;
}
// Connect InterfaceGCM's required interface to InterfaceLCM's provided interface
mtsManagerLocal * LCM = mtsManagerLocal::GetInstance();
const std::string serverComponentName = mtsManagerComponentBase::GetNameOfManagerComponentClientFor(clientProcessName);
const std::string serverInterfaceName = mtsManagerComponentBase::GetNameOfInterfaceLCMProvided();
#if CISST_MTS_HAS_ICE
if (!LCM->Connect(LCM->GetProcessName(), this->GetName(), interfaceName,
clientProcessName, serverComponentName, serverInterfaceName))
{
CMN_LOG_CLASS_INIT_ERROR << "AddNewClientProcess: failed to connect: "
<< mtsManagerGlobal::GetInterfaceUID(LCM->GetProcessName(), this->GetName(), interfaceName)
<< " - "
<< mtsManagerGlobal::GetInterfaceUID(clientProcessName, serverComponentName, serverInterfaceName)
<< std::endl;
return false;
}
#else
if (!LCM->Connect(this->GetName(), interfaceName, serverComponentName, serverInterfaceName)) {
CMN_LOG_CLASS_INIT_ERROR << "AddNewClientProcess: failed to connect: "
<< this->GetName() << ":" << interfaceName
<< " - "
<< serverComponentName << ":" << serverInterfaceName
<< std::endl;
return false;
}
#endif
CMN_LOG_CLASS_INIT_VERBOSE << "AddNewClientProcess: creation and connection success" << std::endl;
return true;
}
void mtsIntuitiveResearchKitArm::Init(void)
{
mCounter = 0;
IsGoalSet = false;
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
// initialize trajectory data
JointGet.SetSize(NumberOfJoints());
JointVelocityGet.SetSize(NumberOfJoints());
JointSet.SetSize(NumberOfJoints());
JointSetParam.Goal().SetSize(NumberOfAxes());
JointTrajectory.Velocity.SetSize(NumberOfJoints());
JointTrajectory.Acceleration.SetSize(NumberOfJoints());
JointTrajectory.Start.SetSize(NumberOfJoints());
JointTrajectory.Goal.SetSize(NumberOfJoints());
JointTrajectory.GoalError.SetSize(NumberOfJoints());
JointTrajectory.GoalTolerance.SetSize(NumberOfJoints());
JointTrajectory.EndTime = 0.0;
PotsToEncodersTolerance.SetSize(NumberOfAxes());
// initialize velocity
CartesianVelocityGetParam.SetVelocityLinear(vct3(0.0));
CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0));
CartesianVelocityGetParam.SetTimestamp(0.0);
CartesianVelocityGetParam.SetValid(false);
// base frame, mostly for cases where no base frame is set by user
BaseFrame = vctFrm4x4::Identity();
BaseFrameValid = true;
// cartesian position are timestamped using timestamps provided by PID
CartesianGetParam.SetAutomaticTimestamp(false);
CartesianGetDesiredParam.SetAutomaticTimestamp(false);
this->StateTable.AddData(CartesianGetLocalParam, "CartesianPositionLocal");
this->StateTable.AddData(CartesianGetLocalDesiredParam, "CartesianPositionLocalDesired");
this->StateTable.AddData(CartesianGetParam, "CartesianPosition");
this->StateTable.AddData(CartesianGetDesiredParam, "CartesianPositionDesired");
this->StateTable.AddData(BaseFrame, "BaseFrame");
this->StateTable.AddData(JointGetParam, "JointPosition");
this->StateTable.AddData(JointGetDesired, "JointPositionDesired");
this->StateTable.AddData(CartesianVelocityGetParam, "CartesianVelocityGetParam");
this->StateTable.AddData(StateJointParam, "StateJoint");
this->StateTable.AddData(StateJointDesiredParam, "StateJointDesired");
// PID
PIDInterface = AddInterfaceRequired("PID");
if (PIDInterface) {
PIDInterface->AddFunction("Enable", PID.Enable);
PIDInterface->AddFunction("GetPositionJoint", PID.GetPositionJoint);
PIDInterface->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
PIDInterface->AddFunction("GetStateJoint", PID.GetStateJoint);
PIDInterface->AddFunction("GetStateJointDesired", PID.GetStateJointDesired);
PIDInterface->AddFunction("SetPositionJoint", PID.SetPositionJoint);
PIDInterface->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit);
PIDInterface->AddFunction("GetVelocityJoint", PID.GetVelocityJoint);
PIDInterface->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
PIDInterface->AddFunction("SetTorqueJoint", PID.SetTorqueJoint);
PIDInterface->AddFunction("SetTorqueOffset", PID.SetTorqueOffset);
PIDInterface->AddFunction("EnableTrackingError", PID.EnableTrackingError);
PIDInterface->AddFunction("SetTrackingErrorTolerances", PID.SetTrackingErrorTolerance);
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::JointLimitEventHandler, this, "JointLimit");
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Robot IO
IOInterface = AddInterfaceRequired("RobotIO");
if (IOInterface) {
IOInterface->AddFunction("GetSerialNumber", RobotIO.GetSerialNumber);
IOInterface->AddFunction("EnablePower", RobotIO.EnablePower);
IOInterface->AddFunction("DisablePower", RobotIO.DisablePower);
IOInterface->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus);
IOInterface->AddFunction("GetBrakeAmpStatus", RobotIO.GetBrakeAmpStatus);
IOInterface->AddFunction("BiasEncoder", RobotIO.BiasEncoder);
IOInterface->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder);
IOInterface->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI);
IOInterface->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent);
IOInterface->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck);
IOInterface->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance);
IOInterface->AddFunction("BrakeRelease", RobotIO.BrakeRelease);
IOInterface->AddFunction("BrakeEngage", RobotIO.BrakeEngage);
}
// Setup Joints
SUJInterface = AddInterfaceRequired("BaseFrame", MTS_OPTIONAL);
if (SUJInterface) {
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::SetBaseFrame, this, "BaseFrameDesired");
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Arm
RobotInterface = AddInterfaceProvided("Robot");
if (RobotInterface) {
// Get
RobotInterface->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint");
RobotInterface->AddCommandReadState(this->StateTable, JointGetDesired, "GetPositionJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, StateJointParam, "GetStateJoint");
RobotInterface->AddCommandReadState(this->StateTable, StateJointDesiredParam, "GetStateJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalParam, "GetPositionCartesianLocal");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalDesiredParam, "GetPositionCartesianLocalDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired");
RobotInterface->AddCommandReadState(this->StateTable, BaseFrame, "GetBaseFrame");
RobotInterface->AddCommandReadState(this->StateTable, CartesianVelocityGetParam, "GetVelocityCartesian");
// Set
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionJoint, this, "SetPositionJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalJoint, this, "SetPositionGoalJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionCartesian, this, "SetPositionCartesian");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalCartesian, this, "SetPositionGoalCartesian");
// Trajectory events
RobotInterface->AddEventWrite(JointTrajectory.GoalReachedEvent, "GoalReached", bool());
// Robot State
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetRobotControlState,
this, "SetRobotControlState", std::string(""));
RobotInterface->AddCommandRead(&mtsIntuitiveResearchKitArm::GetRobotControlState,
this, "GetRobotControlState", std::string(""));
// Human readable messages
RobotInterface->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.RobotState, "RobotState", std::string(""));
// Stats
RobotInterface->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics");
}
}
