virtual bool configure(ResourceFinder &rf) { this->rf=&rf; Time::turboBoost(); if (rf.check("name")) name=string("/")+rf.find("name").asString().c_str(); else name="/ctpservice"; rpcPort.open((name+string("/")+rf.find("part").asString().c_str()+"/rpc").c_str()); attach(rpcPort); Property portProp; portProp.put("name", name.c_str()); portProp.put("robot", rf.find("robot")); portProp.put("part", rf.find("part")); if (!posPort.configure(portProp)) { cerr<<"Error configuring position controller, check parameters"<<endl; return false; } if (!posPort.connect()) { cerr<<"Error cannot conenct to remote ports"<<endl; return false; } cout << "***** connect to rpc port and type \"help\" for commands list" << endl; thread.attachPosPort(&posPort); if (!thread.start()) { cerr<<"Thread did not start, queue will not work"<<endl; } velPort.open((name+string("/")+rf.find("part").asString().c_str()+"/vc:o").c_str()); velInitPort.open((name+string("/")+rf.find("part").asString().c_str()+"/vcInit:o").c_str()); velThread.attachVelPort(&velPort); velThread.attachVelInitPort(&velInitPort); cout << "Using parameters:" << endl << rf.toString() << endl; velThread.attachRF(&rf); cout << "***** starting the thread" << endl; velThread.start(); return true; }
virtual bool close() { rpcPort.interrupt(); rpcPort.close(); thread.stop(); velThread.stop(); velPort.interrupt(); velPort.close(); velInitPort.interrupt(); velInitPort.close(); return true; }
virtual bool configure(ResourceFinder &rf) { Time::turboBoost(); if (rf.check("name")) name=string("/")+rf.find("name").asString().c_str(); else name="/trajectoryPlayer"; rpcPort.open((name+"/rpc").c_str()); attach(rpcPort); Property portProp; portProp.put("robot", rf.find("robot")); portProp.put("part", rf.find("part")); //*** start the robot driver if (!robot.configure(portProp)) { yError() << "Error configuring position controller, check parameters"; return false; } if (!robot.init()) { yError() << "Error cannot connect to remote ports" ; return false; } //*** attach the robot driver to the thread and start it w_thread.attachRobotDriver(&robot); b_thread.attachRobotDriver(&robot); if (!w_thread.start()) { yError() << "Working thread did not start, queue will not work"; } else { yInfo() << "Working thread started"; } if (rf.check("execute")==true) { yInfo() << "Enablig iPid->setReference() controller"; w_thread.enable_execute_joint_command = true; } else { yInfo() << "Not using iPid->setReference() controller"; w_thread.enable_execute_joint_command = false; } if (rf.check("period")==true) { int period = rf.find("period").asInt(); yInfo() << "Thread period set to " << period << "ms"; w_thread.setRate(period); } yInfo() << "Using parameters:" << rf.toString(); //*** open the position file yInfo() << "opening file..."; if (rf.check("filename")==true) { string filename = rf.find("filename").asString().c_str(); int req_joints = 0; //default value if (rf.check("joints")) { req_joints = rf.find("joints").asInt();} else { yError() << "Missing parameter 'joints' (number of joints to control)"; return false; } if (req_joints < robot.n_joints) { yWarning () << "changing n joints from" << robot.n_joints << "to" << req_joints; robot.n_joints = req_joints; } else if (req_joints > robot.n_joints) { yError() << "Requested number of joints exceeds the number of available joints on the robot!"; return false; } if (!w_thread.actions.openFile(filename,robot.n_joints)) { yError() << "Unable to parse file " << filename; return false; }; } else { yWarning() << "no sequence files load."; } yInfo() << "module succesffully configured. ready."; return true; }