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; }