fix: improve connection handling and logging in KobukiDriver

This commit is contained in:
ishak jmilou.ishak
2025-01-09 13:19:00 +01:00
parent d066f68ad2
commit 07d88eef7d
2 changed files with 10 additions and 17 deletions

View File

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

View File

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