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

14
com/mqtt_callback.cpp
View File

@@ -1,9 +1,10 @@
#include "mqtt_callback.h"
mqtt_callback::mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> log) {
mqtt_callback::mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> 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) {

4
com/mqtt_callback.h
View File

@@ -4,15 +4,17 @@
#include <mqtt/client.h>
#include <mqtt/callback.h>
#include <spdlog/logger.h>
#include <shared/roboglue_shared.h>
class mqtt_callback : public virtual mqtt::callback {
private:
RoboGlue_SHARED *mem_;
mqtt::async_client *cli_ = nullptr;
std::shared_ptr<spdlog::logger> log_;
public:
mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> log);
mqtt_callback(mqtt::async_client *cli, std::shared_ptr<spdlog::logger> log,RoboGlue_SHARED *m);
void connected(const mqtt::string &cause) override;
void connection_lost(const mqtt::string &cause) override;

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();
//}

3
com/roboglue_com.h
View File

@@ -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 <QObject>
#include <QThread>

40
roboglue.conf
View File

@@ -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

11
shared/roboglue_shared.cpp
View File

@@ -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<std::string, int>("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 /////////////////////

17
shared/roboglue_shared.h
View File

@@ -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 <QMutex>
@@ -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<std::string> subList {
{"roboglue_com/ros2com/commands"},
{"roboglue_com/ros2com/coordinates"},
{"roboglue_com/ros2com/state"}
};
std::vector<std::string> pubList {
{"roboglue_com/com2ros/commands"},
{"roboglue_com/com2ros/coordinates"},
{"roboglue_com/com2ros/state"}
};
std::map<std::string, std::string> subMap;
std::map<std::string, std::string> pubMap;
//FIXME: riempire la mappa dinamicamente da un file, eliminare l'assegnazione manuale
enum {
COMM, COORD, STAT
} tList;