diff --git a/com/mqtt_callback.cpp b/com/mqtt_callback.cpp index a328759..4e6c533 100644 --- a/com/mqtt_callback.cpp +++ b/com/mqtt_callback.cpp @@ -1,9 +1,10 @@ #include "mqtt_callback.h" -mqtt_callback::mqtt_callback(mqtt::async_client *cli, std::shared_ptr log) { +mqtt_callback::mqtt_callback(mqtt::async_client *cli, std::shared_ptr log, RoboGlue_SHARED *mem) { //initialize client pointer cli_=cli; log_=log; + mem_= mem; } void mqtt_callback::connected(const mqtt::string &cause) { @@ -17,6 +18,17 @@ void mqtt_callback::connection_lost(const mqtt::string &cause) { void mqtt_callback::message_arrived(mqtt::const_message_ptr msg) { log_->debug("Message Arrived: topic->{} - message->{}", msg->get_topic(), msg->to_string()); + if(msg->get_topic() == mem_->comSettings.subMap.find("interface")->second) { + log_->debug("MQTT: Interface Message: {}", msg->get_payload().c_str()); + } else if (msg->get_topic() == mem_->comSettings.subMap.find("state")->second){ + log_->debug("MQTT: State Message: {}", msg->get_payload().c_str()); + } else if (msg->get_topic() == mem_->comSettings.subMap.find("commands")->second){ + log_->debug("MQTT: Command Message: {}", msg->get_payload().c_str()); + } else if (msg->get_topic() == mem_->comSettings.subMap.find("coordinates")->second){ + log_->debug("MQTT: Coordinate Message: {}", msg->get_payload().c_str()); + } else { + log_->error("Unrecognized topic"); + } } void mqtt_callback::delivery_complete(mqtt::delivery_token_ptr tok) { diff --git a/com/mqtt_callback.h b/com/mqtt_callback.h index e54bd80..f192d94 100644 --- a/com/mqtt_callback.h +++ b/com/mqtt_callback.h @@ -4,15 +4,17 @@ #include #include #include +#include class mqtt_callback : public virtual mqtt::callback { private: + RoboGlue_SHARED *mem_; mqtt::async_client *cli_ = nullptr; std::shared_ptr log_; public: - mqtt_callback(mqtt::async_client *cli, std::shared_ptr log); + mqtt_callback(mqtt::async_client *cli, std::shared_ptr log,RoboGlue_SHARED *m); void connected(const mqtt::string &cause) override; void connection_lost(const mqtt::string &cause) override; diff --git a/com/roboglue_com.cpp b/com/roboglue_com.cpp index 4196f7b..e495662 100644 --- a/com/roboglue_com.cpp +++ b/com/roboglue_com.cpp @@ -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 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 sp) { @@ -350,41 +349,3 @@ void RoboGlue_COM::on_sendROSstate(QMap digital, QMap 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(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(sp[0]*1000)); -// setPointBytes.y=htonl(static_cast(sp[1]*1000)); -// setPointBytes.z=htonl(static_cast(sp[2]*1000)); -// setPointBytes.a=htonl(static_cast(sp[3]*1000)); -// setPointBytes.b=htonl(static_cast(sp[4]*1000)); -// setPointBytes.c=htonl(static_cast(sp[5]*1000)); -// setPointBytes.keepalive=htonl(setPointBytes.keepalive); - -// if(robotIN->write(setPointBytes.bytes,28)) -// modlog->trace("Sent Setpoint {}",setpoint.c_str()); -// robotIN->flush(); -//} - - diff --git a/com/roboglue_com.h b/com/roboglue_com.h index 7735997..f4ee144 100644 --- a/com/roboglue_com.h +++ b/com/roboglue_com.h @@ -44,7 +44,8 @@ * della coda di destinazione. * 20190311 - La pubblicazione dei messaggi di coordinate verso ros deve essere a rateo costante * indipendentemente dalla frequenza di ingresso - */ + * 20191030 - Spostata la documentazione su GIT +*/ #include #include diff --git a/roboglue.conf b/roboglue.conf index 05ab103..2e46f49 100644 --- a/roboglue.conf +++ b/roboglue.conf @@ -12,30 +12,30 @@ connection\autoconnect=true connection\robotip=10.0.0.5 connection\robotport=30002 connection\robotretry=5 -kine\dhtable\1\dha=6.91558096714333e-310 -kine\dhtable\1\dhalpha=6.9155809676188e-310 -kine\dhtable\1\dhd=1.099340831232476e+248 -kine\dhtable\1\dhtheta=1.332e-320 +kine\dhtable\1\dha=0 +kine\dhtable\1\dhalpha=0 +kine\dhtable\1\dhd=0 +kine\dhtable\1\dhtheta=0 kine\dhtable\2\dha=0 -kine\dhtable\2\dhalpha=6.9155809676101e-310 -kine\dhtable\2\dhd=1.3849777432376169e+219 -kine\dhtable\2\dhtheta=6.9155809676599e-310 +kine\dhtable\2\dhalpha=0 +kine\dhtable\2\dhd=0 +kine\dhtable\2\dhtheta=0 kine\dhtable\3\dha=0 -kine\dhtable\3\dhalpha=5.53e-322 -kine\dhtable\3\dhd=2.44861994589e-312 -kine\dhtable\3\dhtheta=8e-323 -kine\dhtable\4\dha=2.37e-322 -kine\dhtable\4\dhalpha=2.37e-322 +kine\dhtable\3\dhalpha=0 +kine\dhtable\3\dhd=0 +kine\dhtable\3\dhtheta=0 +kine\dhtable\4\dha=0 +kine\dhtable\4\dhalpha=0 kine\dhtable\4\dhd=0 kine\dhtable\4\dhtheta=0 -kine\dhtable\5\dha=2.57e-322 -kine\dhtable\5\dhalpha=2.57e-322 -kine\dhtable\5\dhd=2.06e-321 -kine\dhtable\5\dhtheta=8.74e-322 -kine\dhtable\6\dha=6.9155809678283e-310 -kine\dhtable\6\dhalpha=6.915580967671e-310 -kine\dhtable\6\dhd=6.91558096770024e-310 -kine\dhtable\6\dhtheta=6.91558096770024e-310 +kine\dhtable\5\dha=0 +kine\dhtable\5\dhalpha=0 +kine\dhtable\5\dhd=0 +kine\dhtable\5\dhtheta=0 +kine\dhtable\6\dha=0 +kine\dhtable\6\dhalpha=0 +kine\dhtable\6\dhd=0 +kine\dhtable\6\dhtheta=0 kine\dhtable\size=6 kine\maxreach=@Variant(\0\0\0\x87?\x99\x99\x9a) kine\prefix=0 diff --git a/shared/roboglue_shared.cpp b/shared/roboglue_shared.cpp index 7d3f8a3..3c88b3a 100644 --- a/shared/roboglue_shared.cpp +++ b/shared/roboglue_shared.cpp @@ -2,10 +2,17 @@ RoboGlue_SHARED::RoboGlue_SHARED(QSettings *set){ settings=set; - + //FIXME: inserire la lettura di quesste informazioni da un file robotVariables.insert(std::pair("RDY",1)); + comSettings.subMap["commands"] = "roboglue_com/ros2com/commands"; + comSettings.subMap["coordinates"] = "roboglue_com/ros2com/coordinates"; + comSettings.subMap["state"] = "roboglue_com/ros2com/state"; + comSettings.subMap["interface"] = "roboglue_com/ros2com/interface"; - + comSettings.pubMap["commands"] = "roboglue_com/com2ros/commands"; + comSettings.pubMap["coordinates"] = "roboglue_com/com2ros/coordinates"; + comSettings.pubMap["state"] = "roboglue_com/com2ros/state"; + comSettings.pubMap["interface"] = "roboglue_com/com2ros/interface"; } //////////////////////////////////////////////////////////////// /////////////////////// RESTORE FUNCTIONS ///////////////////// diff --git a/shared/roboglue_shared.h b/shared/roboglue_shared.h index b4af30d..9eb5411 100644 --- a/shared/roboglue_shared.h +++ b/shared/roboglue_shared.h @@ -21,7 +21,7 @@ * QUESTA COSA VA GIÀ CAMBIATA PERCHÈ È TROPPO CONFUSIONARIA!! * 20190124 - Implementati tutti i metodi che salvano e ripristinano le configurazioni. * 20190311 - Aggiunta la struttura che tiene gli stati che devono rimanere comuni ai moduli. -* +* 20191030 - Documentazione spostata in GIT */ #include @@ -66,7 +66,7 @@ public: struct { struct { - std::string robotIp = std::string("192.168.0.31"); + std::string robotIp = std::string("localhost"); uint robotPort = 30002; unsigned char retry = 5; bool autoConnect = false; @@ -77,16 +77,9 @@ public: kineDH_t DHtable; float maxReach = 1.2; } kine; - std::vector subList { - {"roboglue_com/ros2com/commands"}, - {"roboglue_com/ros2com/coordinates"}, - {"roboglue_com/ros2com/state"} - }; - std::vector pubList { - {"roboglue_com/com2ros/commands"}, - {"roboglue_com/com2ros/coordinates"}, - {"roboglue_com/com2ros/state"} - }; + std::map subMap; + std::map pubMap; + //FIXME: riempire la mappa dinamicamente da un file, eliminare l'assegnazione manuale enum { COMM, COORD, STAT } tList;