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