QThread* ThreadPoolWithSignals::reserveThread()
{
// QMutexLocker l(&threadMutex);
ControlThread* t;
if(!idleThreads.isEmpty())
{
t= qobject_cast<ControlThread*>(*(idleThreads.begin()));
idleThreads.remove(t);
}
else
{
t = tFactory(this);
// t->moveToThread(control.data());
connect(t, SIGNAL(finished()), t, SLOT(deleteLater()), Qt::QueuedConnection);
t->start();
allThreads.insert(t);
}
activeThreads.insert(t);
qDebug()<<"idle threads: "<<idleThreads.count()
<<" active threads: "<<activeThreads.count()
<<" total threads: "<<allThreads.count();
return t;
}
virtual bool configure(ResourceFinder &rf)
{
string slash = "/";
string ctrlName;
string robotName;
// string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName = rf.check("local", Value("robotJoystickControl")).asString();
robotName = rf.check("robot", Value("cer")).asString();
localName = "/" + ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile = rf.findFile("from");
if (configFile == "") //--from torsoJoystickControl.ini
{
yWarning("Cannot find .ini configuration file. By default I'm searching for torsoJoystickControl.ini");
//return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("robot", robotName.c_str());
ctrl_options.put("local", localName.c_str());
//check for robotInterface availablity
yInfo("Checking for robotInterface availability");
Port startport;
startport.open(localName+"/robotInterfaceCheck:rpc");
Bottle cmd; cmd.addString("is_ready");
Bottle response;
int rc_count = 0;
int rp_count = 0;
int rf_count = 0;
double start_time = yarp::os::Time::now();
bool not_yet_connected = true;
bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
if (skip_robot_interface_check)
{
yInfo("skipping robotInterface check");
}
else
{
do
{
if (not_yet_connected)
{
bool rc = yarp::os::Network::connect(localName + "/robotInterfaceCheck:rpc", "/" + robotName + "/robotInterface");
if (rc == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rc_count++);
yarp::os::Time::delay(1.0);
continue;
}
else
{
not_yet_connected = false;
yDebug("Connection established with robotInterface");
}
}
bool rp = startport.write(cmd, response);
if (rp == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rp_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while trying to connect to robotInterface");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
if (response.get(0).asString() != "ok")
{
yWarning("RobotInterface is not ready yet, retrying... %d", rf_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while waiting for robotInterface availability");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
yInfo("RobotInterface is ready");
break;
}
}
} while (1);
}
//set the thread rate
int rate = rf.check("rate",Value(10)).asInt();
yInfo("baseCtrl thread rate: %d ms.",rate);
//start the control thread
control_thr = new ControlThread(rate, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
//check for debug mode
if (rf.check("no_motors"))
{
this->control_thr->motors_enabled = false;
yInfo() << "Motors disabled";
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
//Module initialization and configuration
virtual bool configure(ResourceFinder &rf)
{
string slash="/";
string ctrlName;
string robotName;
string partName;
string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName=rf.check("local",Value("baseControl")).asString();
robotName=rf.check("robot",Value("cer")).asString();
partName = rf.check("part", Value("mobile_base")).asString();
remoteName=slash+robotName+slash+partName;
localName=slash+ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile=rf.findFile("from");
if (configFile=="") //--from baseCtrl.ini
{
yError("Cannot find .ini configuration file. By default I'm searching for baseCtrl.ini");
return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("remote", remoteName.c_str());
ctrl_options.put("local", localName.c_str());
//check for robotInterface availability
yInfo("Checking for yarpRobotInterface availability");
Port startport;
startport.open (localName + "/yarpRobotInterfaceCheck:rpc");
Bottle cmd; cmd.addString("is_ready");
Bottle response;
int rc_count =0;
int rp_count =0;
int rf_count =0;
double start_time=yarp::os::Time::now();
bool not_yet_connected=true;
bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
if (skip_robot_interface_check)
{
yInfo("skipping yarpRobotInterface check");
}
else
{
do
{
if (not_yet_connected)
{
bool rc = yarp::os::Network::connect (localName + "/yarpRobotInterfaceCheck:rpc","/" + robotName + "/yarprobotinterface");
if (rc == false)
{
yWarning ("Problems trying to connect to %s %d", std::string("/" + robotName + "/yarprobotinterface").c_str(), rc_count ++);
yarp::os::Time::delay (1.0);
continue;
}
else
{
not_yet_connected = false;
yDebug ("Connection established with yarpRobotInterface");
}
}
bool rp = startport.write (cmd, response);
if (rp == false)
{
yWarning ("Problems trying to connect to yarpRobotInterface %d", rp_count ++);
if (yarp::os::Time::now()-start_time>30)
{
yError ("Timeout expired while trying to connect to yarpRobotInterface");
return false;
}
yarp::os::Time::delay (1.0);
continue;
}
else
{
if (response.get(0).asString() != "ok")
{
yWarning ("yarpRobotInterface is not ready yet, retrying... %d", rf_count++);
if (yarp::os::Time::now()-start_time>30)
{
yError ("Timeout expired while waiting for yarpRobotInterface availability");
return false;
}
yarp::os::Time::delay (1.0);
continue;
}
else
{
yInfo ("yarpRobotInterface is ready");
break;
}
}
} while (1);
}
//set the thread rate
int period = rf.check("period",Value(20)).asInt();
yInfo("baseCtrl thread rate: %d ms.",period);
// the motor control thread
bool motors_enabled=true;
if (rf.check("no_motors"))
{
yInfo("'no_motors' option found. Skipping motor control part.");
motors_enabled=false;
}
if (motors_enabled==true)
{
control_thr = new ControlThread(period, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
}
//try to connect to joystickCtrl output
if (rf.check("joystick_connect"))
{
int joystick_trials = 0;
do
{
yarp::os::Time::delay(1.0);
if (yarp::os::Network::connect("/joystickCtrl:o",localName+"/joystick1:i"))
{
yInfo("Joystick has been automatically connected");
break;
}
else
{
yWarning("Unable to find the joystick port, retrying (%d/5)...",joystick_trials);
joystick_trials++;
}
if (joystick_trials>=5)
{
yError("Unable to find the joystick port, giving up");
break;
}
}
while (1);
}
//check for debug mode
if (rf.check("debug"))
{
this->control_thr->enable_debug(true);
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
