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