void HttpControler::incomingConnection(int handle)
{
ControlThread *thread = new ControlThread(handle, this);
connect(thread, SIGNAL(finished()), thread, SLOT(deleteLater()));
thread->run();
}
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;
}
void TaskWareHouse::addNewTask(TASK task)
{
//qDebug()<<task.sourceConn<<task.srcConName<<task.action\
// <<task.method<<task.object<<task.args.list()\
// <<task.args.destConn<<task.type<<"addNewTask_TASK";
++counter;
QString _number = QString("").sprintf("%08d", counter);
if ( !task.method.startsWith("reload") ) {
QString _name = QString("#%1 %2 <%3> in <%4> connection")
.arg(_number)
.arg(task.method)
.arg(task.object)
.arg(task.srcConName);
QListWidgetItem *_item = new QListWidgetItem();
_item->setText(_name);
_item->setIcon(QIcon::fromTheme("ledlightgreen"));
QTime _time = QTime::currentTime();
QMap<QString, QVariant> itemData;
itemData.insert("Connection", task.srcConName);
itemData.insert("Object", task.object);
itemData.insert("Action", task.method);
itemData.insert("Start", QString("%1:%2:%3:%4")
.arg(QString("").sprintf("%02d", _time.hour()))
.arg(QString("").sprintf("%02d", _time.minute()))
.arg(QString("").sprintf("%02d", _time.second()))
.arg(QString("").sprintf("%03d", _time.msec())));
itemData.insert("End", "-");
itemData.insert("Arguments", task.args.list());
itemData.insert("Result", "Processing");
itemData.insert("Message", "-");
_item->setData(Qt::UserRole, itemData);
setNewTooltip(_item);
taskList->addItem(_item);
};
if ( task.type == "domain" ) {
threadPool->insert(
_number,
new DomControlThread(this));
} else if ( task.type == "network" ) {
threadPool->insert(
_number,
new NetControlThread(this));
} else if ( task.type == "pool" ) {
threadPool->insert(
_number,
new StoragePoolControlThread(this));
} else if ( task.type == "volume" ) {
threadPool->insert(
_number,
new StorageVolControlThread(this));
} else if ( task.type == "secret" ) {
threadPool->insert(
_number,
new SecretControlThread(this));
} else return;
ControlThread *cThread = static_cast<ControlThread*>(
threadPool->value(_number));
if ( NULL!=cThread ) {
connect(cThread, SIGNAL(errorMsg(QString&,uint)),
this, SLOT(msgRepeater(QString&, uint)));
connect(cThread, SIGNAL(resultData(Result)),
this, SLOT(taskResultReceiver(Result)));
cThread->execAction(counter, task);
};
//This function parses the user commands received through the RPC port
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("run");
reply.addString("idle");
reply.addString("reset_odometry");
reply.addString("set_prefilter 0/1/2/4/8");
reply.addString("set_motors_filter 0/1/2/4/8");
reply.addString("change_pid <identif> <kp> <ki> <kd>");
reply.addString("change_ctrl_mode <type_string>");
reply.addString("set_debug_mode 0/1");
return true;
}
else if (command.get(0).asString()=="set_debug_mode")
{
if (control_thr)
{
if (command.get(1).asInt()>0)
{control_thr->enable_debug(true); reply.addString("debug mode on");}
else
{control_thr->enable_debug(false); reply.addString("debug mode off");}
}
return true;
}
else if (command.get(0).asString()=="set_prefilter")
{
if (control_thr)
{
if (command.get(1).asInt()>0)
{control_thr->set_input_filter(command.get(1).asInt()); reply.addString("Prefilter on");}
else
{control_thr->set_input_filter(0); reply.addString("Prefilter off");}
}
return true;
}
else if (command.get(0).asString()=="set_motors_filter")
{
if (control_thr)
{
int f= command.get(1).asInt();
if (f==1)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_1); reply.addString("Motors filter on");}
if (f==2)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_2); reply.addString("Motors filter on");}
if (f==4)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_4); reply.addString("Motors filter on");}
if (f==8)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_8); reply.addString("Motors filter on");}
else
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::DISABLED); reply.addString("Motors filter off");}
}
return true;
}
else if (command.get(0).asString()=="run")
{
if (control_thr)
{
if (control_thr->get_control_type() == BASE_CONTROL_NONE) {control_thr->get_motor_handler()->set_control_idle();}
else if (control_thr->get_control_type() == BASE_CONTROL_VELOCITY_NO_PID) {control_thr->get_motor_handler()->set_control_velocity();}
else if (control_thr->get_control_type() == BASE_CONTROL_OPENLOOP_NO_PID) {control_thr->get_motor_handler()->set_control_openloop();}
else if (control_thr->get_control_type() == BASE_CONTROL_VELOCITY_PID) {control_thr->get_motor_handler()->set_control_velocity();}
else if (control_thr->get_control_type() == BASE_CONTROL_OPENLOOP_PID) {control_thr->get_motor_handler()->set_control_openloop();}
if (control_thr->get_motor_handler()->check_motors_on())
{reply.addString("Motors now on");}
else
{reply.addString("Unable to turn motors on! fault pressed?");}
}
return true;
}
else if (command.get(0).asString()=="idle")
{
if (control_thr)
{
control_thr->get_motor_handler()->set_control_idle();
{reply.addString("Motors now off.");}
}
return true;
}
else if (command.get(0).asString()=="change_ctrl_mode")
{
if (control_thr)
{
if (control_thr->set_control_type(command.get(1).asString().c_str()))
{reply.addString("control mode changed");}
else
{reply.addString("invalid control mode request");}
}
return true;
}
else if (command.get(0).asString()=="change_pid")
{
if (control_thr)
{
string identif = command.get(1).asString().c_str();
double kp = command.get(2).asDouble();
double ki = command.get(3).asDouble();
double kd = command.get(4).asDouble();
control_thr->set_pid(identif,kp,ki,kd);
reply.addString("New pid parameters set.");
yInfo("New pid parameters set.");
}
return true;
}
else if (command.get(0).asString()=="reset_odometry")
{
if (control_thr)
{
Odometry* pOdometry=0;
pOdometry = control_thr->get_odometry_handler();
if (pOdometry)
{pOdometry->reset_odometry(); reply.addString("Odometry reset done.");}
else
{reply.addString("No Odometry available.");}
}
return true;
}
reply.addString("Unknown command.");
return true;
}
