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::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 int firewirePort = 0; std::string config_io; std::string config_pid; std::string config_kinematics; std::string config_name; cmnCommandLineOptions options; options.AddOptionOneValue("i", "io-master", "config file for master robot IO", cmnCommandLineOptions::REQUIRED_OPTION, &config_io); options.AddOptionOneValue("p", "pid-master", "config file for master PID controller", cmnCommandLineOptions::REQUIRED_OPTION, &config_pid); options.AddOptionOneValue("k", "kinematic-master", "config file for master robot kinematic", cmnCommandLineOptions::REQUIRED_OPTION, &config_kinematics); options.AddOptionOneValue("n", "name-master", "config file for master robot name", cmnCommandLineOptions::REQUIRED_OPTION, &config_name); options.AddOptionOneValue("f", "firewire", "firewire port number(s)", cmnCommandLineOptions::OPTIONAL_OPTION, &firewirePort); options.AddOptionNoValue("I", "io-ros", "add ROS bridge for IO level"); std::string errorMessage; if (!options.Parse(argc, argv, errorMessage)) { std::cerr << "Error: " << errorMessage << std::endl; options.PrintUsage(std::cerr); return -1; } // now components mtsManagerLocal * 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); // IO mtsRobotIO1394 * io = new mtsRobotIO1394("io", ioPeriod, firewirePort); io->Configure(config_io); componentManager->AddComponent(io); // connect ioGUIMaster to io mtsRobotIO1394QtWidgetFactory * robotWidgetFactory = new mtsRobotIO1394QtWidgetFactory("robotWidgetFactory"); componentManager->AddComponent(robotWidgetFactory); componentManager->Connect("robotWidgetFactory", "RobotConfiguration", "io", "Configuration"); robotWidgetFactory->Configure(); // mtsPID mtsPID* pid = new mtsPID("pid", ioPeriod); // periodicity will be ignore because ExecIn/Out are connected pid->Configure(config_pid); componentManager->AddComponent(pid); componentManager->Connect(pid->GetName(), "RobotJointTorqueInterface", "io", config_name); componentManager->Connect(pid->GetName(), "ExecIn", "io", "ExecOut"); // PID GUI mtsPIDQtWidget * pidMasterGUI = new mtsPIDQtWidget("pid master", 7); pidMasterGUI->Configure(); componentManager->AddComponent(pidMasterGUI); componentManager->Connect(pidMasterGUI->GetName(), "Controller", pid->GetName(), "Controller"); // psm mtsIntuitiveResearchKitPSM * psm = new mtsIntuitiveResearchKitPSM(config_name, armPeriod); psm->Configure(config_kinematics); componentManager->AddComponent(psm); componentManager->Connect(psm->GetName(), "PID", pid->GetName(), "Controller"); componentManager->Connect(psm->GetName(), "RobotIO", "io", config_name); componentManager->Connect(psm->GetName(), "Adapter", "io", psm->GetName() + "-Adapter"); componentManager->Connect(psm->GetName(), "Tool", "io", psm->GetName() + "-Tool"); componentManager->Connect(psm->GetName(), "ManipClutch", "io", psm->GetName() + "-ManipClutch"); // psm GUI mtsIntuitiveResearchKitArmQtWidget * psmGUI = new mtsIntuitiveResearchKitArmQtWidget(config_name + "GUI"); componentManager->AddComponent(psmGUI); componentManager->Connect(psmGUI->GetName(), "Manipulator", psm->GetName(), "Robot"); ROS_INFO("\n Name is :%s \n :%s \n :%s \n :%s \n ",config_name.c_str(),config_pid.c_str(),config_io.c_str() ,config_kinematics.c_str()); // ros wrapper mtsROSBridge robotBridge("RobotBridge", rosPeriod, true); // populate interfaces const dvrk_topics_version::version version = dvrk_topics_version::v1_3_0; dvrk::add_topics_psm(robotBridge, "/dvrk/" + config_name, psm->GetName(), version); if (options.IsSet("io-ros")) { dvrk::add_topics_io(robotBridge, "/dvrk/" + config_name + "/io", psm->GetName(), version); } // add component and connect componentManager->AddComponent(&robotBridge); dvrk::connect_bridge_psm(robotBridge, psm->GetName()); if (options.IsSet("io-ros")) { dvrk::connect_bridge_io(robotBridge, io->GetName(), psm->GetName()); } // organize all widgets in a tab widget QTabWidget * tabWidget = new QTabWidget; // io gui mtsRobotIO1394QtWidgetFactory::WidgetListType::const_iterator iterator; for (iterator = robotWidgetFactory->Widgets().begin(); iterator != robotWidgetFactory->Widgets().end(); ++iterator) { tabWidget->addTab(*iterator, (*iterator)->GetName().c_str()); } // pid gui tabWidget->addTab(pidMasterGUI, (config_name + " PID").c_str()); // psm gui tabWidget->addTab(psmGUI, psm->GetName().c_str()); // button gui tabWidget->addTab(robotWidgetFactory->ButtonsWidget(), "Buttons"); // show widget tabWidget->show(); //-------------- 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); pid->StartAndWait(2.0 * cmn_s); // start all other components componentManager->CreateAllAndWait(2.0 * cmn_s); componentManager->StartAllAndWait(2.0 * cmn_s); // QtApp is now running componentManager->KillAllAndWait(2.0 * cmn_s); componentManager->Cleanup(); // delete dvgc robot delete pid; delete pidMasterGUI; delete psm; delete psmGUI; // stop all logs cmnLogger::Kill(); }
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::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 int firewirePort = 0; std::string config_io; std::string config_pid; std::string config_kinematics; std::string config_name; cmnCommandLineOptions options; options.AddOptionOneValue("i", "io-master", "config file for master robot IO", cmnCommandLineOptions::REQUIRED_OPTION, &config_io); options.AddOptionOneValue("p", "pid-master", "config file for master PID controller", cmnCommandLineOptions::REQUIRED_OPTION, &config_pid); options.AddOptionOneValue("k", "kinematic-master", "config file for master robot kinematic", cmnCommandLineOptions::REQUIRED_OPTION, &config_kinematics); options.AddOptionOneValue("n", "name-master", "config file for master robot name", cmnCommandLineOptions::REQUIRED_OPTION, &config_name); options.AddOptionOneValue("f", "firewire", "firewire port number(s)", cmnCommandLineOptions::OPTIONAL_OPTION, &firewirePort); std::string errorMessage; if (!options.Parse(argc, argv, errorMessage)) { std::cerr << "Error: " << errorMessage << std::endl; options.PrintUsage(std::cerr); return -1; } // now components mtsManagerLocal * 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", 1.0 * cmn_ms, firewirePort); io->Configure(config_io); componentManager->AddComponent(io); // connect ioGUIMaster to io mtsRobotIO1394QtWidgetFactory * robotWidgetFactory = new mtsRobotIO1394QtWidgetFactory("robotWidgetFactory"); componentManager->AddComponent(robotWidgetFactory); componentManager->Connect("robotWidgetFactory", "RobotConfiguration", "io", "Configuration"); robotWidgetFactory->Configure(); // mtsPID mtsPID* pid = new mtsPID("pid", 5 * cmn_ms); pid->Configure(config_pid); componentManager->AddComponent(pid); componentManager->Connect(pid->GetName(), "RobotJointTorqueInterface", "io", config_name); componentManager->Connect(pid->GetName(), "ExecIn", "io", "ExecOut"); // PID Master GUI mtsPIDQtWidget * pidMasterGUI = new mtsPIDQtWidget("pid master",7); pidMasterGUI->Configure(); componentManager->AddComponent(pidMasterGUI); componentManager->Connect(pidMasterGUI->GetName(), "Controller", pid->GetName(), "Controller"); // psm mtsIntuitiveResearchKitConsole::Arm *psm = new mtsIntuitiveResearchKitConsole::Arm(config_name, io->GetName()); psm->ConfigurePID(config_pid); psm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_PSM,config_kinematics, 5 * cmn_ms); console->AddArm(psm); console->Connect(); // psm GUI mtsIntuitiveResearchKitArmQtWidget* psmGUI = new mtsIntuitiveResearchKitArmQtWidget(config_name+"GUI"); psmGUI->Configure(); componentManager->AddComponent(psmGUI); componentManager->Connect(psmGUI->GetName(), "Manipulator", psm->Name(), "Robot"); //------------------------------------------------------- // Start ROS Bridge // ------------------------------------------------------ ROS_INFO("\n Name is :%s \n :%s \n :%s \n :%s \n ",config_name.c_str(),config_pid.c_str(),config_io.c_str() ,config_kinematics.c_str()); // ros wrapper mtsROSBridge robotBridge("RobotBridge", 20 * cmn_ms, true); // connect to psm robotBridge.AddPublisherFromReadCommand<prmPositionCartesianGet, geometry_msgs::Pose>( config_name, "GetPositionCartesian", "/dvrk_psm/cartesian_pose_current"); robotBridge.AddPublisherFromReadCommand<vctDoubleVec, cisst_msgs::vctDoubleVec>( pid->GetName(), "GetEffortJointDesired", "/dvrk_psm/joint_effort_current"); robotBridge.AddPublisherFromReadCommand<prmPositionJointGet, sensor_msgs::JointState>( pid->GetName(), "GetPositionJoint", "/dvrk_psm/joint_position_current"); robotBridge.AddPublisherFromEventWrite<prmEventButton, std_msgs::Bool>( "ManipClutch","Button","/dvrk_psm/manip_clutch_state"); robotBridge.AddPublisherFromReadCommand<std::string, std_msgs::String>( config_name,"GetRobotControlState","/dvrk_psm/robot_state_current"); // Finally Working Form; However it is still unsafe since there is no safety check. // Use with caution and with your hand on the E-Stop. robotBridge.AddSubscriberToWriteCommand<prmForceTorqueJointSet , sensor_msgs::JointState>( pid->GetName(), "SetTorqueJoint", "/dvrk_psm/set_joint_effort"); robotBridge.AddSubscriberToWriteCommand<std::string, std_msgs::String>( config_name, "SetRobotControlState", "/dvrk_psm/set_robot_state"); robotBridge.AddSubscriberToWriteCommand<bool, std_msgs::Bool>( pid->GetName(),"Enable","/dvrk_psm/enable_pid"); robotBridge.AddSubscriberToWriteCommand<prmPositionJointSet, sensor_msgs::JointState>( pid->GetName(),"SetPositionJoint","/dvrk_psm/set_position_joint"); robotBridge.AddSubscriberToWriteCommand<prmPositionCartesianSet, geometry_msgs::Pose>( config_name, "SetPositionCartesian", "/dvrk_psm/set_position_cartesian"); componentManager->AddComponent(&robotBridge); componentManager->Connect(robotBridge.GetName(), config_name, psm->Name(), "Robot"); componentManager->Connect(robotBridge.GetName(), pid->GetName(), pid->GetName(),"Controller"); componentManager->Connect(robotBridge.GetName(), "ManipClutch", "io", config_name + "-ManipClutch"); // componentManager->Connect(robotBridge.GetName(), "Clutch", "io", "CLUTCH"); //------------------------------------------------------- // End ROS Bridge // ------------------------------------------------------ // organize all widgets in a tab widget QTabWidget * tabWidget = new QTabWidget; // io gui mtsRobotIO1394QtWidgetFactory::WidgetListType::const_iterator iterator; for (iterator = robotWidgetFactory->Widgets().begin(); iterator != robotWidgetFactory->Widgets().end(); ++iterator) { tabWidget->addTab(*iterator, (*iterator)->GetName().c_str()); } tabWidget->addTab(consoleGUI, "Main"); // pid gui tabWidget->addTab(pidMasterGUI, (config_name + "PID").c_str()); // psm gui tabWidget->addTab(psmGUI, psm->Name().c_str()); // button gui tabWidget->addTab(robotWidgetFactory->ButtonsWidget(), "Buttons"); // show widget tabWidget->show(); //-------------- 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); pid->StartAndWait(2.0 * cmn_s); // start all other components componentManager->CreateAllAndWait(2.0 * cmn_s); componentManager->StartAllAndWait(2.0 * cmn_s); // QtApp will run here componentManager->KillAllAndWait(2.0 * cmn_s); componentManager->Cleanup(); // delete dvgc robot delete pid; delete pidMasterGUI; delete psm; delete psmGUI; delete robotWidgetFactory; // stop all logs cmnLogger::Kill(); }