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