int main(int argc, char ** argv)
{
// program deprecated
std::cout << "-----------------------------------------------------------" << std::endl
<< "- This program is deprecated: -" << std::endl
<< "- use dvrk_console_json instead -" << std::endl
<< "- examples can be found in share/jhu-dVRK/console*.json -" << std::endl
<< "-----------------------------------------------------------" << std::endl
<< std::endl;
// desired frequencies
const double ioPeriod = 0.5 * cmn_ms;
const double armPeriod = 2.0 * cmn_ms;
const double rosPeriod = 10.0 * cmn_ms;
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClass("mtsIntuitiveResearchKitECM", CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// ---- WARNING: hack to remove ros args ----
ros::V_string argout;
ros::removeROSArgs(argc, argv, argout);
argc = argout.size();
// ------------------------------------------
// parse options
cmnCommandLineOptions options;
int firewirePort = 0;
std::string gcmip = "-1";
typedef std::map<std::string, std::string> ConfigFilesType;
ConfigFilesType configFiles;
std::string armName;
options.AddOptionOneValue("i", "io",
"configuration file for robot IO (see sawRobotIO1394)",
cmnCommandLineOptions::REQUIRED_OPTION, &configFiles["io"]);
options.AddOptionOneValue("p", "pid",
"configuration file for PID controller (see sawControllers, mtsPID)",
cmnCommandLineOptions::REQUIRED_OPTION, &configFiles["pid"]);
options.AddOptionOneValue("k", "kinematic",
"configuration file for kinematic (see cisstRobot, robManipulator)",
cmnCommandLineOptions::REQUIRED_OPTION, &configFiles["kinematic"]);
options.AddOptionOneValue("n", "arm-name",
"arm name, i.e. PSM1, ... as found in sawRobotIO configuration file",
cmnCommandLineOptions::REQUIRED_OPTION, &armName);
options.AddOptionOneValue("f", "firewire",
"firewire port number(s)",
cmnCommandLineOptions::OPTIONAL_OPTION, &firewirePort);
options.AddOptionOneValue("g", "gcmip",
"global component manager IP address",
cmnCommandLineOptions::OPTIONAL_OPTION, &gcmip);
std::string errorMessage;
if (!options.Parse(argc, argv, errorMessage)) {
std::cerr << "Error: " << errorMessage << std::endl;
options.PrintUsage(std::cerr);
return -1;
}
for (ConfigFilesType::const_iterator iter = configFiles.begin();
iter != configFiles.end();
++iter) {
if (!cmnPath::Exists(iter->second)) {
std::cerr << "File not found for " << iter->first
<< ": " << iter->second << std::endl;
return -1;
} else {
std::cout << "Configuration file for " << iter->first
<< ": " << iter->second << std::endl;
}
}
std::cout << "FirewirePort: " << firewirePort << std::endl;
std::string processname = "dv-arm";
mtsManagerLocal * componentManager = 0;
if (gcmip != "-1") {
try {
componentManager = mtsManagerLocal::GetInstance(gcmip, processname);
} catch(...) {
std::cerr << "Failed to get GCM instance." << std::endl;
return -1;
}
} else {
componentManager = mtsManagerLocal::GetInstance();
}
// create a Qt application and tab to hold all widgets
mtsQtApplication * qtAppTask = new mtsQtApplication("QtApplication", argc, argv);
qtAppTask->Configure();
componentManager->AddComponent(qtAppTask);
// console
mtsIntuitiveResearchKitConsole * console = new mtsIntuitiveResearchKitConsole("console");
componentManager->AddComponent(console);
mtsIntuitiveResearchKitConsoleQtWidget * consoleGUI = new mtsIntuitiveResearchKitConsoleQtWidget("consoleGUI");
componentManager->AddComponent(consoleGUI);
// connect console GUI to console
componentManager->Connect("console", "Main", "consoleGUI", "Main");
// IO
mtsRobotIO1394 * io = new mtsRobotIO1394("io", ioPeriod, firewirePort);
io->Configure(configFiles["io"]);
componentManager->AddComponent(io);
// Create the arm
unsigned int numberOfAxis = 0;
mtsIntuitiveResearchKitConsole::Arm * arm
= new mtsIntuitiveResearchKitConsole::Arm(armName, io->GetName());
arm->ConfigurePID(configFiles["pid"]);
// Configure based on arm type assuming name
if ((armName == "PSM1") || (armName == "PSM2") || (armName == "PSM3")) {
arm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_PSM,
configFiles["kinematic"], armPeriod);
numberOfAxis = 7;
} else if ((armName == "MTML") || (armName == "MTMR")) {
arm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_MTM,
configFiles["kinematic"], armPeriod);
numberOfAxis = 8;
} else if (armName == "ECM") {
arm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_ECM,
configFiles["kinematic"], armPeriod);
numberOfAxis = 4;
} else {
std::cerr << "Arm name should be either PSM1, PSM2, PSM3, MTML, MTMR or ECM, not " << armName << std::endl;
return -1;
}
console->AddArm(arm);
// connect ioGUIMaster to io
mtsRobotIO1394QtWidgetFactory * robotWidgetFactory = new mtsRobotIO1394QtWidgetFactory("robotWidgetFactory");
componentManager->AddComponent(robotWidgetFactory);
componentManager->Connect("robotWidgetFactory", "RobotConfiguration", "io", "Configuration");
robotWidgetFactory->Configure();
// PID GUI
mtsPIDQtWidget * pidGUI = new mtsPIDQtWidget("PID", numberOfAxis);
pidGUI->Configure();
componentManager->AddComponent(pidGUI);
componentManager->Connect(pidGUI->GetName(), "Controller", arm->PIDComponentName(), "Controller");
// GUI
mtsIntuitiveResearchKitArmQtWidget * armGUI = new mtsIntuitiveResearchKitArmQtWidget("Arm");
armGUI->Configure();
componentManager->AddComponent(armGUI);
componentManager->Connect(armGUI->GetName(), "Manipulator", arm->Name(), "Robot");
// organize all widgets in a tab widget
QTabWidget * tabWidget = new QTabWidget;
tabWidget->addTab(consoleGUI, "Main");
tabWidget->addTab(armGUI, "Arm");
tabWidget->addTab(pidGUI, "PID");
mtsRobotIO1394QtWidgetFactory::WidgetListType::const_iterator iterator;
for (iterator = robotWidgetFactory->Widgets().begin();
iterator != robotWidgetFactory->Widgets().end();
++iterator) {
tabWidget->addTab(*iterator, (*iterator)->GetName().c_str());
}
tabWidget->addTab(robotWidgetFactory->ButtonsWidget(), "Buttons");
tabWidget->show();
// ros wrapper
mtsROSBridge robotBridge("RobotBridge", rosPeriod, true);
// populate interfaces
const dvrk_topics_version::version version = dvrk_topics_version::v1_3_0;
dvrk::add_topics_ecm(robotBridge, "/dvrk/" + armName, arm->Name(), version);
// Connect
componentManager->AddComponent(&robotBridge);
dvrk::connect_bridge_ecm(robotBridge, arm->Name());
//-------------- create the components ------------------
io->CreateAndWait(2.0 * cmn_s); // this will also create the pids as they are in same thread
io->StartAndWait(2.0 * cmn_s);
componentManager->GetComponent(arm->PIDComponentName())->StartAndWait(2.0 * cmn_s);
// start all other components
componentManager->CreateAllAndWait(2.0 * cmn_s);
componentManager->StartAllAndWait(2.0 * cmn_s);
// QtApplication will run in main thread and return control
// when exited.
componentManager->KillAllAndWait(2.0 * cmn_s);
componentManager->Cleanup();
// delete dvgc robot
delete pidGUI;
delete io;
delete robotWidgetFactory;
// stop all logs
cmnLogger::Kill();
return 0;
}
int main(int argc, char ** argv)
{
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClass("mtsIntuitiveResearchKitPSM", CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClass("mtsPID", CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// parse options
cmnCommandLineOptions options;
int firewirePort = 0;
std::string gcmip = "-1";
typedef std::map<std::string, std::string> ConfigFilesType;
ConfigFilesType configFiles;
std::string masterName, slaveName;
options.AddOptionOneValue("i", "io-master",
"configuration file for master robot IO (see sawRobotIO1394)",
cmnCommandLineOptions::REQUIRED, &configFiles["io-master"]);
options.AddOptionOneValue("p", "pid-master",
"configuration file for master PID controller (see sawControllers, mtsPID)",
cmnCommandLineOptions::REQUIRED, &configFiles["pid-master"]);
options.AddOptionOneValue("k", "kinematic-master",
"configuration file for master kinematic (see cisstRobot, robManipulator)",
cmnCommandLineOptions::REQUIRED, &configFiles["kinematic-master"]);
options.AddOptionOneValue("I", "io-slave",
"configuration file for slave robot IO (see sawRobotIO1394)",
cmnCommandLineOptions::REQUIRED, &configFiles["io-slave"]);
options.AddOptionOneValue("P", "pid-slave",
"configuration file for slave PID controller (see sawControllers, mtsPID)",
cmnCommandLineOptions::REQUIRED, &configFiles["pid-slave"]);
options.AddOptionOneValue("K", "kinematic-slave",
"configuration file for slave kinematic (see cisstRobot, robManipulator)",
cmnCommandLineOptions::REQUIRED, &configFiles["kinematic-slave"]);
options.AddOptionOneValue("n", "name-master",
"MTML or MTMR",
cmnCommandLineOptions::REQUIRED, &masterName);
options.AddOptionOneValue("N", "name-slave",
"PSM1 or PSM2",
cmnCommandLineOptions::REQUIRED, &slaveName);
options.AddOptionOneValue("f", "firewire",
"firewire port number(s)",
cmnCommandLineOptions::OPTIONAL, &firewirePort);
options.AddOptionOneValue("g", "gcmip",
"global component manager IP address",
cmnCommandLineOptions::OPTIONAL, &gcmip);
std::string errorMessage;
if (!options.Parse(argc, argv, errorMessage)) {
std::cerr << "Error: " << errorMessage << std::endl;
options.PrintUsage(std::cerr);
return -1;
}
for (ConfigFilesType::const_iterator iter = configFiles.begin();
iter != configFiles.end();
++iter) {
if (!cmnPath::Exists(iter->second)) {
std::cerr << "File not found for " << iter->first
<< ": " << iter->second << std::endl;
return -1;
} else {
std::cout << "Configuration file for " << iter->first
<< ": " << iter->second << std::endl;
}
}
std::cout << "FirewirePort: " << firewirePort << std::endl;
std::string processname = "dvTeleop";
mtsManagerLocal * componentManager = 0;
if (gcmip != "-1") {
try {
componentManager = mtsManagerLocal::GetInstance(gcmip, processname);
} catch(...) {
std::cerr << "Failed to get GCM instance." << std::endl;
return -1;
}
} else {
componentManager = mtsManagerLocal::GetInstance();
}
// create a Qt application and tab to hold all widgets
QApplication qtAppTask(argc, argv);
// console
mtsIntuitiveResearchKitConsole * console = new mtsIntuitiveResearchKitConsole("console");
componentManager->AddComponent(console);
mtsIntuitiveResearchKitConsoleQtWidget * consoleGUI = new mtsIntuitiveResearchKitConsoleQtWidget("consoleGUI");
componentManager->AddComponent(consoleGUI);
// connect teleGUI to tele
componentManager->Connect("console", "Main", "consoleGUI", "Main");
// IO
mtsRobotIO1394 * io = new mtsRobotIO1394("io", 1.0 * cmn_ms, firewirePort);
io->Configure("sawRobotIO1394-MTMR-28247.xml");
io->Configure("sawRobotIO1394-PSM1-28007.xml");
io->Configure("sawRobotIO1394-MTML-22723.xml");
io->Configure("sawRobotIO1394-PSM2-27374.xml");
componentManager->AddComponent(io);
mtsIntuitiveResearchKitConsole::Arm * mtm
= new mtsIntuitiveResearchKitConsole::Arm(masterName, io->GetName());
mtm->ConfigurePID("sawControllersPID-MTM.xml");
mtm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_MTM,
"dvmtm.rob", 3.0 * cmn_ms);
console->AddArm(mtm);
// MTML
mtsIntuitiveResearchKitConsole::Arm * mtml
= new mtsIntuitiveResearchKitConsole::Arm("MTML", io->GetName());
mtml->ConfigurePID("sawControllersPID-MTM.xml");
mtml->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_MTM,
"dvmtm.rob", 3.0 * cmn_ms);
console->AddArm(mtml);
mtsIntuitiveResearchKitConsole::Arm * psm
= new mtsIntuitiveResearchKitConsole::Arm("PSM1", io->GetName());
psm->ConfigurePID("sawControllersPID-PSM.xml");
psm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_PSM,
"dvpsm.rob", 3.0 * cmn_ms);
console->AddArm(psm);
mtsIntuitiveResearchKitConsole::Arm * psm2
= new mtsIntuitiveResearchKitConsole::Arm("PSM2", io->GetName());
psm2->ConfigurePID("sawControllersPID-PSM.xml");
psm2->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_PSM,
"dvpsm.rob", 3.0 * cmn_ms);
console->AddArm(psm2);
// connect ioGUIMaster to io
mtsRobotIO1394QtWidgetFactory * robotWidgetFactory = new mtsRobotIO1394QtWidgetFactory("robotWidgetFactory");
componentManager->AddComponent(robotWidgetFactory);
componentManager->Connect("robotWidgetFactory", "RobotConfiguration", "io", "Configuration");
robotWidgetFactory->Configure();
// PID Master GUI
mtsPIDQtWidget * pidMasterGUI = new mtsPIDQtWidget("PID Master", 8);
pidMasterGUI->Configure();
componentManager->AddComponent(pidMasterGUI);
componentManager->Connect(pidMasterGUI->GetName(), "Controller", mtm->PIDComponentName(), "Controller");
mtsPIDQtWidget * pidMasterGUI2 = new mtsPIDQtWidget("PID MTML", 8);
pidMasterGUI2->Configure();
componentManager->AddComponent(pidMasterGUI2);
componentManager->Connect(pidMasterGUI2->GetName(), "Controller", mtml->PIDComponentName(), "Controller");
// PID Slave GUI
mtsPIDQtWidget * pidSlaveGUI = new mtsPIDQtWidget("PID Slave", 7);
pidSlaveGUI->Configure();
componentManager->AddComponent(pidSlaveGUI);
componentManager->Connect(pidSlaveGUI->GetName(), "Controller", psm->PIDComponentName(), "Controller");
mtsPIDQtWidget * pidSlaveGUI2 = new mtsPIDQtWidget("PID PSM2", 7);
pidSlaveGUI2->Configure();
componentManager->AddComponent(pidSlaveGUI2);
componentManager->Connect(pidSlaveGUI2->GetName(), "Controller", psm2->PIDComponentName(), "Controller");
// Teleoperation
mtsTeleOperationQtWidget * teleGUI = new mtsTeleOperationQtWidget("teleGUI");
teleGUI->Configure();
componentManager->AddComponent(teleGUI);
mtsTeleOperation * tele = new mtsTeleOperation("tele", 5.0 * cmn_ms);
componentManager->AddComponent(tele);
// connect teleGUI to tele
componentManager->Connect("teleGUI", "TeleOperation", "tele", "Setting");
mtsTeleOperationQtWidget * teleGUI2 = new mtsTeleOperationQtWidget("teleGUI2");
teleGUI2->Configure();
componentManager->AddComponent(teleGUI2);
mtsTeleOperation * tele2 = new mtsTeleOperation("tele2", 5.0 * cmn_ms);
componentManager->AddComponent(tele2);
// connect teleGUI to tele
componentManager->Connect("teleGUI2", "TeleOperation", "tele2", "Setting");
// TextToSpeech
mtsTextToSpeech* textToSpeech = new mtsTextToSpeech;
textToSpeech->AddInterfaceRequiredForEventString("ErrorMsg", "RobotErrorMsg");
componentManager->AddComponent(textToSpeech);
componentManager->Connect(textToSpeech->GetName(), "ErrorMsg", psm->Name(), "Robot");
// connect teleop to Master + Slave + Clutch
componentManager->Connect("tele", "Master", mtm->Name(), "Robot");
componentManager->Connect("tele", "Slave", psm->Name(), "Robot");
componentManager->Connect("tele", "CLUTCH", "io", "CLUTCH");
componentManager->Connect("tele", "COAG", "io", "COAG");
componentManager->Connect("tele2", "Master", mtml->Name(), "Robot");
componentManager->Connect("tele2", "Slave", psm2->Name(), "Robot");
componentManager->Connect("tele2", "CLUTCH", "io", "CLUTCH");
componentManager->Connect("tele2", "COAG", "io", "COAG");
// organize all widgets in a tab widget
QTabWidget * tabWidget = new QTabWidget;
tabWidget->addTab(consoleGUI, "Main");
tabWidget->addTab(teleGUI, "Tele-op");
tabWidget->addTab(teleGUI2, "Tele-op2");
tabWidget->addTab(pidMasterGUI, "PID Master");
tabWidget->addTab(pidMasterGUI2, "PID MTML");
tabWidget->addTab(pidSlaveGUI, "PID Slave");
tabWidget->addTab(pidSlaveGUI2, "PID PSM2");
mtsRobotIO1394QtWidgetFactory::WidgetListType::const_iterator iterator;
for (iterator = robotWidgetFactory->Widgets().begin();
iterator != robotWidgetFactory->Widgets().end();
++iterator) {
tabWidget->addTab(*iterator, (*iterator)->GetName().c_str());
}
tabWidget->addTab(robotWidgetFactory->ButtonsWidget(), "Buttons");
tabWidget->show();
//-------------- create the components ------------------
io->CreateAndWait(2.0 * cmn_s); // this will also create the pids as they are in same thread
// start all other components
componentManager->CreateAllAndWait(2.0 * cmn_s);
componentManager->StartAllAndWait(2.0 * cmn_s);
// QtApplication will run in main thread and return control
// when exited.
qtAppTask.exec();
componentManager->KillAllAndWait(2.0 * cmn_s);
componentManager->Cleanup();
// delete dvgc robot
delete tele;
delete tele2;
delete pidMasterGUI;
delete pidMasterGUI2;
delete pidSlaveGUI;
delete pidSlaveGUI2;
delete io;
delete robotWidgetFactory;
// stop all logs
cmnLogger::Kill();
return 0;
}
