bool mtsIntuitiveResearchKitConsole::Connect(void)
{
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
const ArmList::iterator armsEnd = mArms.end();
for (ArmList::iterator armIter = mArms.begin();
armIter != armsEnd;
++armIter) {
Arm * arm = armIter->second;
// IO
if (arm->IOInterfaceRequired) {
componentManager->Connect(this->GetName(), "IO-" + arm->Name(),
arm->IOComponentName(), arm->Name());
}
// PID
if (arm->mType != Arm::ARM_SUJ) {
if (arm->PIDInterfaceRequired) {
componentManager->Connect(this->GetName(), "PID-" + arm->Name(),
arm->PIDComponentName(), "Controller");
}
}
// arm interface
if (arm->ArmInterfaceRequired) {
componentManager->Connect(this->GetName(), arm->Name(),
arm->Name(), "Robot");
}
// arm specific interfaces
arm->Connect();
// connect to SUJ if needed
if (arm->SUJInterfaceRequiredFromIO && arm->SUJInterfaceRequiredToSUJ) {
componentManager->Connect(this->GetName(), arm->SUJInterfaceRequiredToSUJ->GetName(),
"SUJ", arm->Name());
componentManager->Connect(this->GetName(), arm->SUJInterfaceRequiredFromIO->GetName(),
arm->IOComponentName(), arm->Name() + "-SUJClutch");
}
}
const TeleopList::iterator teleopsEnd = mTeleops.end();
for (TeleopList::iterator teleopIter = mTeleops.begin();
teleopIter != teleopsEnd;
++teleopIter) {
TeleopPSM * teleop = teleopIter->second;
teleop->Connect();
}
// connect the foot pedals if needed
if (mHasFootpedals) {
this->ConnectFootpedalInterfaces();
}
// connect interfaces to retrieve base frame from ECM SUJ and send event to SUJ
if (mSUJECMInterfaceRequired
&& mECMBaseFrameInterfaceProvided) {
componentManager->Connect(this->GetName(), "BaseFrame", "SUJ", "ECM");
componentManager->Connect("SUJ", "BaseFrame", this->GetName(), "ECMBaseFrame");
}
return true;
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (SetupAndConnectInterfaces(newArm)) {
mArms.push_back(newArm);
return true;
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (AddArmInterfaces(newArm)) {
ArmList::iterator armIterator = mArms.find(newArm->mName);
if (armIterator != mArms.end()) {
mArms[newArm->mName] = newArm;
return true;
}
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}
void mtsIntuitiveResearchKitConsole::Configure(const std::string & filename)
{
mConfigured = false;
std::ifstream jsonStream;
jsonStream.open(filename.c_str());
Json::Value jsonConfig, jsonValue;
Json::Reader jsonReader;
if (!jsonReader.parse(jsonStream, jsonConfig)) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to parse configuration\n"
<< jsonReader.getFormattedErrorMessages();
this->mConfigured = false;
return;
}
// extract path of main json config file to search other files relative to it
cmnPath configPath(cmnPath::GetWorkingDirectory());
std::string fullname = configPath.Find(filename);
std::string configDir = fullname.substr(0, fullname.find_last_of('/'));
configPath.Add(configDir);
// IO default settings
double periodIO = 0.5 * cmn_ms;
int firewirePort = 0;
// get user preferences
jsonValue = jsonConfig["io"];
if (!jsonValue.empty()) {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: looking for user provided io:period and io:port" << std::endl;
jsonValue = jsonConfig["io"]["period"];
if (!jsonValue.empty()) {
periodIO = jsonValue.asDouble();
}
jsonValue = jsonConfig["io"]["port"];
if (!jsonValue.empty()) {
firewirePort = jsonValue.asInt();
}
} else {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using default io:period and io:port" << std::endl;
}
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: period IO is " << periodIO << std::endl
<< "Configure: FireWire port is " << firewirePort << std::endl;
// create IO
mtsRobotIO1394 * io = new mtsRobotIO1394(mIOComponentName, periodIO, firewirePort);
const Json::Value arms = jsonConfig["arms"];
for (unsigned int index = 0; index < arms.size(); ++index) {
if (!ConfigureArmJSON(arms[index], io->GetName(), configPath)) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure arms[" << index << "]" << std::endl;
return;
}
}
// loop over all arms to configure IO only
const ArmList::iterator end = mArms.end();
ArmList::iterator iter;
for (iter = mArms.begin(); iter != end; ++iter) {
std::string ioConfig = iter->second->mIOConfigurationFile;
if (ioConfig != "") {
io->Configure(ioConfig);
}
}
mtsComponentManager::GetInstance()->AddComponent(io);
// now can configure PID and Arms
for (iter = mArms.begin(); iter != end; ++iter) {
const std::string pidConfig = iter->second->mPIDConfigurationFile;
if (pidConfig != "") {
iter->second->ConfigurePID(pidConfig);
}
const std::string armConfig = iter->second->mArmConfigurationFile;
if (armConfig != "") {
iter->second->ConfigureArm(iter->second->mType, armConfig);
}
}
bool hasSUJ = false;
bool hasECM = false;
// now load all PSM teleops
const Json::Value psmTeleops = jsonConfig["psm-teleops"];
for (unsigned int index = 0; index < psmTeleops.size(); ++index) {
if (!ConfigurePSMTeleopJSON(psmTeleops[index])) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure psm-teleops[" << index << "]" << std::endl;
return;
}
}
// see which event is used for operator present
// find name of button event used to detect if operator is present
mOperatorPresentComponent = jsonConfig["operator-present"]["component"].asString();
mOperatorPresentInterface = jsonConfig["operator-present"]["interface"].asString();
//set defaults
if (mOperatorPresentComponent == "") {
mOperatorPresentComponent = mIOComponentName;
}
if (mOperatorPresentInterface == "") {
mOperatorPresentInterface = "COAG";
}
// look for footpedals in json config
jsonValue = jsonConfig["io"]["has-footpedals"];
if (jsonValue.empty()) {
mHasFootpedals = false;
} else {
mHasFootpedals = jsonValue.asBool();
}
// if we have any teleoperation component, we need to add the interfaces for the foot pedals
if (mTeleops.size() > 0) {
mHasFootpedals = true;
}
if (mHasFootpedals) {
this->AddFootpedalInterfaces();
}
// interface to ecm to get ECM frame and then push to PSM SUJs as base frame
mtsInterfaceRequired * ecmArmInterface = 0;
for (iter = mArms.begin(); iter != end; ++iter) {
if (iter->second->mType == Arm::ARM_ECM) {
hasECM = true;
ecmArmInterface = iter->second->ArmInterfaceRequired;
}
else if (iter->second->mType == Arm::ARM_SUJ) {
hasSUJ = true;
}
}
// add required and provided interfaces to grab positions from ECM SUJ and ECM
if (hasSUJ && hasECM) {
mSUJECMInterfaceRequired = AddInterfaceRequired("BaseFrame");
if (mSUJECMInterfaceRequired) {
mSUJECMInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler, this, "BaseFrameDesired");
}
if (ecmArmInterface) {
ecmArmInterface->AddFunction("GetPositionCartesianLocal", mGetPositionCartesianLocalFromECM);
} else {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: arm interface not yet added for ECM" << std::endl;
}
mECMBaseFrameInterfaceProvided = AddInterfaceProvided("ECMBaseFrame");
if (mECMBaseFrameInterfaceProvided) {
mECMBaseFrameInterfaceProvided->AddEventWrite(mECMBaseFrameEvent, "BaseFrameDesired", prmPositionCartesianGet());
}
}
// connect arm SUJ clutch button to SUJ
if (hasSUJ) {
for (iter = mArms.begin(); iter != end; ++iter) {
Arm * arm = iter->second;
if ((arm->mType == Arm::ARM_ECM) || (arm->mType == Arm::ARM_PSM)) {
arm->SUJInterfaceRequiredFromIO = this->AddInterfaceRequired("SUJ-" + arm->Name() + "-IO");
arm->SUJInterfaceRequiredFromIO->AddEventHandlerWrite(&Arm::SUJClutchEventHandlerFromIO, arm, "Button");
arm->SUJInterfaceRequiredToSUJ = this->AddInterfaceRequired("SUJ-" + arm->Name());
arm->SUJInterfaceRequiredToSUJ->AddFunction("Clutch", arm->SUJClutch);
} else {
arm->SUJInterfaceRequiredFromIO = 0;
arm->SUJInterfaceRequiredToSUJ = 0;
}
}
}
mConfigured = true;
}