mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-04 12:24:57 +00:00
fix: improve connection handling and logging in KobukiDriver
This commit is contained in:
@@ -68,28 +68,14 @@ int CKobuki::connect(char *comportT) {
|
||||
HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
if (HCom == -1) {
|
||||
printf("Kobuki connected\n");
|
||||
printf("unable to connect\n");
|
||||
return HCom;
|
||||
} else {
|
||||
set_interface_attribs2(HCom, B115200,
|
||||
0); // set speed to 115,200 bps, 8n1 (no parity)
|
||||
set_interface_attribs2(HCom, B115200,0); // set speed to 115,200 bps, 8n1 (no parity)
|
||||
set_blocking2(HCom, 0); // set no blocking
|
||||
/* struct termios settings;
|
||||
tcgetattr(HCom, &settings);
|
||||
|
||||
cfsetospeed(&settings, B115200); // baud rate
|
||||
settings.c_cflag &= ~PARENB; // no parity
|
||||
settings.c_cflag &= ~CSTOPB; // 1 stop bit
|
||||
settings.c_cflag &= ~CSIZE;
|
||||
settings.c_cflag |= CS8 | CLOCAL; // 8 bits
|
||||
settings.c_lflag &= ~ICANON; // canonical mode
|
||||
settings.c_cc[VTIME]=1;
|
||||
settings.c_oflag &= ~OPOST; // raw output
|
||||
|
||||
tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
|
||||
tcflush(HCom, TCOFLUSH);
|
||||
|
||||
printf("Kobuki connected\n");
|
||||
std::cout<<"Kobuki connected" << std::endl;
|
||||
return HCom;
|
||||
}
|
||||
}
|
||||
|
@@ -325,6 +325,13 @@ std::string serializeKobukiData(const TKobukiData &data) {
|
||||
// needed it so it can be threaded
|
||||
void sendKobukiData(TKobukiData &data) {
|
||||
while (true) {
|
||||
if(!robot.connected()){
|
||||
std::cout << "Kobuki is not connected anymore" << std::endl;
|
||||
while(!robot.connected()){
|
||||
robot.startCommunication("/dev/ttyUSB0", true, nullptr);
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
client.publishMessage("kobuki/data", serializeKobukiData(data));
|
||||
std::cout << "Sent data" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
|
Reference in New Issue
Block a user