Obbart/RoboGlue_QT
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

aggiunto topic interface, cambiato modo di subscribe mqtt

Browse Source
This commit is contained in:
Emanuele
2019-10-30 16:33:36 +01:00
parent 117cf20765
commit 9483e265f3
7 changed files with 59 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
com/roboglue_com.cpp
View File

@@ -5,8 +5,8 @@ RoboGlue_COM::RoboGlue_COM (RoboGlue_SHARED *mem): m(mem) {
//////// SETUP LOGGER //////////
liblog = spdlog::stdout_logger_mt("URcom_liblog");
modlog = spdlog::stdout_logger_mt("RoboGlue_comlog");
liblog->set_level(spdlog::level::debug);
modlog->set_level(spdlog::level::debug);
liblog->set_level(spdlog::level::warn);
modlog->set_level(spdlog::level::info);
///////// INITIALIZE TCP SERVER ///////////
server = new QTcpServer(this);
@@ -16,7 +16,7 @@ RoboGlue_COM::RoboGlue_COM (RoboGlue_SHARED *mem): m(mem) {
//////// INITIALIZE MQTT CONNECTOR ///////
client = new mqtt::async_client(std::string("tcp://localhost:1883"),"");
callback = new mqtt_callback(client,modlog);////////////////////////////////////////////////
callback = new mqtt_callback(client,modlog,m);////////////////////////////////////////////////
//////// END EXTERNAL PUBLIC SLOTS /////////////
////////////////////////////////////////////////
baseMsg = m->commonSettings.baseMessage;
@@ -35,9 +35,8 @@ void RoboGlue_COM::run() {
client->set_callback(*callback);
client->connect()->wait();
// subscrbe to mqtt topics
std::vector<std::string> subList = m->comSettings.subList;
for (uint i=0; i < subList.size(); i++){
client->subscribe(subList[i],0);
for (auto it=m->comSettings.subMap.begin(); it != m->comSettings.subMap.end(); ++it){
client->subscribe(it->second,0);
}
client->start_consuming();
@@ -270,7 +269,7 @@ void RoboGlue_COM::timed_sendCoordinates(){
baseMsg["value"]["rx"] = sp[3];
baseMsg["value"]["ry"] = sp[4];
baseMsg["value"]["rz"] = sp[5];
MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COORD]);
MQTTpublish(baseMsg, m->comSettings.pubMap.find("coordinates")->second);
}
}
////////////////////////////////////////////////
@@ -329,7 +328,7 @@ void RoboGlue_COM::on_sendROScommand(QString command, QVariantMap params) {
baseMsg["command"] = command.toStdString();
QJsonDocument jparam = QJsonDocument(QJsonObject::fromVariantMap(params));
baseMsg["params"] = json::parse(jparam.toJson().data());
MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COMM]);
MQTTpublish(baseMsg, m->comSettings.pubMap.find("commands")->second);
}
void RoboGlue_COM::on_sendROScoordinates(QList<double> sp) {
@@ -350,41 +349,3 @@ void RoboGlue_COM::on_sendROSstate(QMap<std::string,bool> digital, QMap<std::str
////////////////////////////////////////////////
//////// END EXTERNAL PUBLIC SLOTS /////////////
////////////////////////////////////////////////
//void RoboGlue_COM::on_sendSetpointOLD(QList<double> sp) {
// std::string setpoint;
// struct {
// union{
// struct{
// uint32_t x,y,z,a,b,c,keepalive;
// };
// char bytes[28];
// };
// } setPointBytes;
// setPointBytes.keepalive=1;
// int32_t v;
// setpoint = "(";
// for(uchar i=0; i<JOINT_NUMBER; i++){
// v=static_cast<int32_t>(sp[i]*1000);
// if (i>2) v=boost::algorithm::clamp(v, -2680,2680);
// setpoint.append(std::to_string(v));
// boost::algorithm::replace_all(setpoint, ",",".");
// if(i<JOINT_NUMBER) setpoint.append("|");
// }
// setpoint.append("1");
// boost::algorithm::replace_all(setpoint,"|",",");
// setpoint.append(")");
// setPointBytes.x=htonl(static_cast<uint32_t>(sp[0]*1000));
// setPointBytes.y=htonl(static_cast<uint32_t>(sp[1]*1000));
// setPointBytes.z=htonl(static_cast<uint32_t>(sp[2]*1000));
// setPointBytes.a=htonl(static_cast<uint32_t>(sp[3]*1000));
// setPointBytes.b=htonl(static_cast<uint32_t>(sp[4]*1000));
// setPointBytes.c=htonl(static_cast<uint32_t>(sp[5]*1000));
// setPointBytes.keepalive=htonl(setPointBytes.keepalive);
// if(robotIN->write(setPointBytes.bytes,28))
// modlog->trace("Sent Setpoint {}",setpoint.c_str());
// robotIN->flush();
//}