mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
functions for auto reconnect with pi and kobuki
This commit is contained in:
ishak jmilou.ishak
@@ -94,6 +94,10 @@ int CKobuki::connect(char *comportT) {
|
||||
}
|
||||
}
|
||||
|
||||
bool CKobuki::isConnected() {
|
||||
return (HCom != -1 && tcflush(HCom, TCOFLUSH) == 0);
|
||||
}
|
||||
|
||||
unsigned char *CKobuki::readKobukiMessage() {
|
||||
unsigned char buffer[1];
|
||||
ssize_t Pocet;
|
||||
|
||||
@@ -80,6 +80,7 @@ public:
|
||||
void robotSafety(); //overload
|
||||
void sendNullMessage();
|
||||
bool safetyActive = false;
|
||||
bool isConnected();
|
||||
KobukiParser parser;
|
||||
|
||||
|
||||
|
||||
@@ -1,318 +1,387 @@
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include "MQTT/MqttClient.h"
|
||||
#include "KobukiDriver/CKobuki.h"
|
||||
#include <opencv4/opencv2/opencv.hpp>
|
||||
#include "MQTT/MqttClient.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <opencv4/opencv2/core.hpp>
|
||||
#include <opencv4/opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
CKobuki robot;
|
||||
|
||||
std::string readMQTT();
|
||||
void parseMQTT(std::string message);
|
||||
void CapnSend();
|
||||
//ip, clientID, username, password
|
||||
MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
|
||||
void reconnectKobuki();
|
||||
void monitorKobukiConnection();
|
||||
// ip, clientID, username, password
|
||||
MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi",
|
||||
"rpiwachtwoordofzo"); // create a client object
|
||||
std::string message = "stop";
|
||||
std::string serializeKobukiData(const TKobukiData &data);
|
||||
void sendKobukiData(TKobukiData &data);
|
||||
|
||||
void setup()
|
||||
{
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
//connect mqtt server and sub to commands
|
||||
void setup() {
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
// connect mqtt server and sub to commands
|
||||
|
||||
client.connect();
|
||||
client.subscribe("home/commands");
|
||||
client.connect();
|
||||
client.subscribe("home/commands");
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
setup();
|
||||
std::thread image (CapnSend);
|
||||
std::thread safety([&]() { robot.robotSafety(&message); });
|
||||
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
|
||||
|
||||
while(true){
|
||||
std::string message = readMQTT();
|
||||
if (!message.empty()){
|
||||
parseMQTT(message);
|
||||
}
|
||||
|
||||
}
|
||||
int main() {
|
||||
setup();
|
||||
reconnectKobuki();
|
||||
|
||||
sendMqtt.join();
|
||||
safety.join();
|
||||
image.join();
|
||||
std::thread connectionMonitor(monitorKobukiConnection);
|
||||
std::thread image(CapnSend);
|
||||
std::thread safety([&]() { robot.robotSafety(&message); });
|
||||
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
|
||||
|
||||
while (true) {
|
||||
std::string message = readMQTT();
|
||||
if (!message.empty()) {
|
||||
parseMQTT(message);
|
||||
}
|
||||
}
|
||||
|
||||
sendMqtt.join();
|
||||
safety.join();
|
||||
image.join();
|
||||
}
|
||||
|
||||
std::string readMQTT()
|
||||
{
|
||||
static std::string lastMessage;
|
||||
|
||||
std::string message = client.getLastMessage();
|
||||
if (!message.empty() && message != lastMessage)
|
||||
{
|
||||
std::cout << "MQTT Message: " << message << std::endl;
|
||||
lastMessage = message;
|
||||
void reconnectKobuki() {
|
||||
while (true) {
|
||||
if (robot.startCommunication("/dev/ttyUSB0") != -1) {
|
||||
std::cout << "Kobuki opnieuw verbonden!" << std::endl;
|
||||
break; // Verlaat de loop als de verbinding succesvol is
|
||||
} else {
|
||||
std::cerr << "Kobuki niet verbonden. Probeer opnieuw over 3 seconden..." << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(3));
|
||||
}
|
||||
|
||||
// Add a small delay to avoid busy-waiting
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
return message;
|
||||
}
|
||||
}
|
||||
|
||||
void parseMQTT(std::string message)
|
||||
{
|
||||
if (message == "up")
|
||||
{
|
||||
robot.forward(350);
|
||||
}
|
||||
else if (message == "left")
|
||||
{
|
||||
robot.setRotationSpeed(4);
|
||||
}
|
||||
else if (message == "right")
|
||||
{
|
||||
robot.setRotationSpeed(-4);
|
||||
}
|
||||
else if (message == "down")
|
||||
{
|
||||
robot.forward(-350);
|
||||
}
|
||||
else if (message == "stop")
|
||||
{
|
||||
robot.sendNullMessage();
|
||||
robot.sendNullMessage();
|
||||
}
|
||||
else if (message == "estop")
|
||||
{
|
||||
robot.forward(-400);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Invalid command" << std::endl;
|
||||
void monitorKobukiConnection() {
|
||||
while (true) {
|
||||
// Check regelmatig of de verbinding actief is
|
||||
if (!robot.isConnected()) {
|
||||
std::cerr << "Kobuki verbinding verloren. Reconnectie starten..." << std::endl;
|
||||
reconnectKobuki();
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(5)); // Check iedere 5 seconden
|
||||
}
|
||||
}
|
||||
|
||||
void logToFile()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
TKobukiData robotData = robot.parser.data;
|
||||
std::ofstream outputFile("log", std::ios_base::app); // Open file in append mode to not overwrite own content
|
||||
if (outputFile.is_open())
|
||||
{ // check if the file was opened successfully
|
||||
// Get current time
|
||||
std::time_t now = std::time(nullptr);
|
||||
outputFile << "Timestamp: " << std::ctime(&now);
|
||||
// Write data to the file
|
||||
outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n";
|
||||
outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n";
|
||||
outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n";
|
||||
outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n";
|
||||
outputFile << "digitalInput: " << robotData.digitalInput << "\n";
|
||||
outputFile << "timestamp: " << robotData.timestamp << "\n";
|
||||
outputFile << "BumperCenter: " << robotData.BumperCenter << "\n";
|
||||
outputFile << "BumperLeft: " << robotData.BumperLeft << "\n";
|
||||
outputFile << "BumperRight: " << robotData.BumperRight << "\n";
|
||||
outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n";
|
||||
outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n";
|
||||
outputFile << "CliffCenter: " << robotData.CliffCenter << "\n";
|
||||
outputFile << "CliffLeft: " << robotData.CliffLeft << "\n";
|
||||
outputFile << "CliffRight: " << robotData.CliffRight << "\n";
|
||||
outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n";
|
||||
outputFile << "EncoderRight: " << robotData.EncoderRight << "\n";
|
||||
outputFile << "PWMleft: " << robotData.PWMleft << "\n";
|
||||
outputFile << "PWMright: " << robotData.PWMright << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n";
|
||||
outputFile << "Charger: " << robotData.Charger << "\n";
|
||||
outputFile << "Battery: " << robotData.Battery << "\n";
|
||||
outputFile << "overCurrent: " << robotData.overCurrent << "\n";
|
||||
outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n";
|
||||
outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n";
|
||||
outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n";
|
||||
outputFile << "GyroAngle: " << robotData.GyroAngle << "\n";
|
||||
outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n";
|
||||
outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n";
|
||||
outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter << "\n";
|
||||
outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n";
|
||||
outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n";
|
||||
outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight << "\n";
|
||||
outputFile << "frameId: " << robotData.frameId << "\n";
|
||||
outputFile << "HardwareVersionPatch: " << robotData.extraInfo.HardwareVersionPatch << "\n";
|
||||
outputFile << "HardwareVersionMinor: " << robotData.extraInfo.HardwareVersionMinor << "\n";
|
||||
outputFile << "HardwareVersionMajor: " << robotData.extraInfo.HardwareVersionMajor << "\n";
|
||||
outputFile << "FirmwareVersionPatch: " << robotData.extraInfo.FirmwareVersionPatch << "\n";
|
||||
outputFile << "FirmwareVersionMinor: " << robotData.extraInfo.FirmwareVersionMinor << "\n";
|
||||
outputFile << "FirmwareVersionMajor: " << robotData.extraInfo.FirmwareVersionMajor << "\n";
|
||||
outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n";
|
||||
outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
|
||||
outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
|
||||
outputFile.close();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "Error opening file\n";
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds
|
||||
std::string readMQTT() {
|
||||
static std::string lastMessage;
|
||||
|
||||
std::string message = client.getLastMessage();
|
||||
if (!message.empty() && message != lastMessage) {
|
||||
std::cout << "MQTT Message: " << message << std::endl;
|
||||
lastMessage = message;
|
||||
}
|
||||
|
||||
// Add a small delay to avoid busy-waiting
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
return message;
|
||||
}
|
||||
|
||||
void parseMQTT(std::string message) {
|
||||
if (message == "up") {
|
||||
robot.forward(350);
|
||||
} else if (message == "left") {
|
||||
robot.setRotationSpeed(4);
|
||||
} else if (message == "right") {
|
||||
robot.setRotationSpeed(-4);
|
||||
} else if (message == "down") {
|
||||
robot.forward(-350);
|
||||
} else if (message == "stop") {
|
||||
robot.sendNullMessage();
|
||||
robot.sendNullMessage();
|
||||
} else if (message == "estop") {
|
||||
robot.forward(-400);
|
||||
} else {
|
||||
std::cout << "Invalid command" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void logToFile() {
|
||||
while (true) {
|
||||
TKobukiData robotData = robot.parser.data;
|
||||
std::ofstream outputFile("log",
|
||||
std::ios_base::app); // Open file in append mode to
|
||||
// not overwrite own content
|
||||
if (outputFile.is_open()) { // check if the file was opened successfully
|
||||
// Get current time
|
||||
std::time_t now = std::time(nullptr);
|
||||
outputFile << "Timestamp: " << std::ctime(&now);
|
||||
// Write data to the file
|
||||
outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n";
|
||||
outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n";
|
||||
outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n";
|
||||
outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n";
|
||||
outputFile << "digitalInput: " << robotData.digitalInput << "\n";
|
||||
outputFile << "timestamp: " << robotData.timestamp << "\n";
|
||||
outputFile << "BumperCenter: " << robotData.BumperCenter << "\n";
|
||||
outputFile << "BumperLeft: " << robotData.BumperLeft << "\n";
|
||||
outputFile << "BumperRight: " << robotData.BumperRight << "\n";
|
||||
outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n";
|
||||
outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n";
|
||||
outputFile << "CliffCenter: " << robotData.CliffCenter << "\n";
|
||||
outputFile << "CliffLeft: " << robotData.CliffLeft << "\n";
|
||||
outputFile << "CliffRight: " << robotData.CliffRight << "\n";
|
||||
outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n";
|
||||
outputFile << "EncoderRight: " << robotData.EncoderRight << "\n";
|
||||
outputFile << "PWMleft: " << robotData.PWMleft << "\n";
|
||||
outputFile << "PWMright: " << robotData.PWMright << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n";
|
||||
outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n";
|
||||
outputFile << "Charger: " << robotData.Charger << "\n";
|
||||
outputFile << "Battery: " << robotData.Battery << "\n";
|
||||
outputFile << "overCurrent: " << robotData.overCurrent << "\n";
|
||||
outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n";
|
||||
outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n";
|
||||
outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n";
|
||||
outputFile << "GyroAngle: " << robotData.GyroAngle << "\n";
|
||||
outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n";
|
||||
outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n";
|
||||
outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter
|
||||
<< "\n";
|
||||
outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n";
|
||||
outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n";
|
||||
outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight
|
||||
<< "\n";
|
||||
outputFile << "frameId: " << robotData.frameId << "\n";
|
||||
outputFile << "HardwareVersionPatch: "
|
||||
<< robotData.extraInfo.HardwareVersionPatch << "\n";
|
||||
outputFile << "HardwareVersionMinor: "
|
||||
<< robotData.extraInfo.HardwareVersionMinor << "\n";
|
||||
outputFile << "HardwareVersionMajor: "
|
||||
<< robotData.extraInfo.HardwareVersionMajor << "\n";
|
||||
outputFile << "FirmwareVersionPatch: "
|
||||
<< robotData.extraInfo.FirmwareVersionPatch << "\n";
|
||||
outputFile << "FirmwareVersionMinor: "
|
||||
<< robotData.extraInfo.FirmwareVersionMinor << "\n";
|
||||
outputFile << "FirmwareVersionMajor: "
|
||||
<< robotData.extraInfo.FirmwareVersionMajor << "\n";
|
||||
outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n";
|
||||
outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
|
||||
outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
|
||||
outputFile.close();
|
||||
} else {
|
||||
std::cerr << "Error opening file\n";
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds
|
||||
}
|
||||
}
|
||||
|
||||
void sendIndividualKobukiData(const TKobukiData &data) {
|
||||
while (true) {
|
||||
std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: " << data.timestamp << std::endl;
|
||||
client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp));
|
||||
client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter));
|
||||
client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft));
|
||||
client.publishMessage("kobuki/data/BumperRight", std::to_string(data.BumperRight));
|
||||
client.publishMessage("kobuki/data/WheelDropLeft", std::to_string(data.WheelDropLeft));
|
||||
client.publishMessage("kobuki/data/WheelDropRight", std::to_string(data.WheelDropRight));
|
||||
client.publishMessage("kobuki/data/CliffCenter", std::to_string(data.CliffCenter));
|
||||
client.publishMessage("kobuki/data/CliffLeft", std::to_string(data.CliffLeft));
|
||||
client.publishMessage("kobuki/data/CliffRight", std::to_string(data.CliffRight));
|
||||
client.publishMessage("kobuki/data/EncoderLeft", std::to_string(data.EncoderLeft));
|
||||
client.publishMessage("kobuki/data/EncoderRight", std::to_string(data.EncoderRight));
|
||||
client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft));
|
||||
client.publishMessage("kobuki/data/PWMright", std::to_string(data.PWMright));
|
||||
client.publishMessage("kobuki/data/ButtonPress1", std::to_string(data.ButtonPress1));
|
||||
client.publishMessage("kobuki/data/ButtonPress2", std::to_string(data.ButtonPress2));
|
||||
client.publishMessage("kobuki/data/ButtonPress3", std::to_string(data.ButtonPress3));
|
||||
client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger));
|
||||
client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery));
|
||||
client.publishMessage("kobuki/data/overCurrent", std::to_string(data.overCurrent));
|
||||
client.publishMessage("kobuki/data/IRSensorRight", std::to_string(data.IRSensorRight));
|
||||
client.publishMessage("kobuki/data/IRSensorCenter", std::to_string(data.IRSensorCenter));
|
||||
client.publishMessage("kobuki/data/IRSensorLeft", std::to_string(data.IRSensorLeft));
|
||||
client.publishMessage("kobuki/data/GyroAngle", std::to_string(data.GyroAngle));
|
||||
client.publishMessage("kobuki/data/GyroAngleRate", std::to_string(data.GyroAngleRate));
|
||||
client.publishMessage("kobuki/data/CliffSensorRight", std::to_string(data.CliffSensorRight));
|
||||
client.publishMessage("kobuki/data/CliffSensorCenter", std::to_string(data.CliffSensorCenter));
|
||||
client.publishMessage("kobuki/data/CliffSensorLeft", std::to_string(data.CliffSensorLeft));
|
||||
client.publishMessage("kobuki/data/wheelCurrentLeft", std::to_string(data.wheelCurrentLeft));
|
||||
client.publishMessage("kobuki/data/wheelCurrentRight", std::to_string(data.wheelCurrentRight));
|
||||
client.publishMessage("kobuki/data/digitalInput", std::to_string(data.digitalInput));
|
||||
client.publishMessage("kobuki/data/analogInputCh0", std::to_string(data.analogInputCh0));
|
||||
client.publishMessage("kobuki/data/analogInputCh1", std::to_string(data.analogInputCh1));
|
||||
client.publishMessage("kobuki/data/analogInputCh2", std::to_string(data.analogInputCh2));
|
||||
client.publishMessage("kobuki/data/analogInputCh3", std::to_string(data.analogInputCh3));
|
||||
client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch", std::to_string(data.extraInfo.HardwareVersionPatch));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor", std::to_string(data.extraInfo.HardwareVersionMinor));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor", std::to_string(data.extraInfo.HardwareVersionMajor));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch", std::to_string(data.extraInfo.FirmwareVersionPatch));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor", std::to_string(data.extraInfo.FirmwareVersionMinor));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor", std::to_string(data.extraInfo.FirmwareVersionMajor));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID0", std::to_string(data.extraInfo.UDID0));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2));
|
||||
while (true) {
|
||||
std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
|
||||
<< data.timestamp << std::endl;
|
||||
client.publishMessage("kobuki/data/timestamp",
|
||||
std::to_string(data.timestamp));
|
||||
client.publishMessage("kobuki/data/BumperCenter",
|
||||
std::to_string(data.BumperCenter));
|
||||
client.publishMessage("kobuki/data/BumperLeft",
|
||||
std::to_string(data.BumperLeft));
|
||||
client.publishMessage("kobuki/data/BumperRight",
|
||||
std::to_string(data.BumperRight));
|
||||
client.publishMessage("kobuki/data/WheelDropLeft",
|
||||
std::to_string(data.WheelDropLeft));
|
||||
client.publishMessage("kobuki/data/WheelDropRight",
|
||||
std::to_string(data.WheelDropRight));
|
||||
client.publishMessage("kobuki/data/CliffCenter",
|
||||
std::to_string(data.CliffCenter));
|
||||
client.publishMessage("kobuki/data/CliffLeft",
|
||||
std::to_string(data.CliffLeft));
|
||||
client.publishMessage("kobuki/data/CliffRight",
|
||||
std::to_string(data.CliffRight));
|
||||
client.publishMessage("kobuki/data/EncoderLeft",
|
||||
std::to_string(data.EncoderLeft));
|
||||
client.publishMessage("kobuki/data/EncoderRight",
|
||||
std::to_string(data.EncoderRight));
|
||||
client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft));
|
||||
client.publishMessage("kobuki/data/PWMright",
|
||||
std::to_string(data.PWMright));
|
||||
client.publishMessage("kobuki/data/ButtonPress1",
|
||||
std::to_string(data.ButtonPress1));
|
||||
client.publishMessage("kobuki/data/ButtonPress2",
|
||||
std::to_string(data.ButtonPress2));
|
||||
client.publishMessage("kobuki/data/ButtonPress3",
|
||||
std::to_string(data.ButtonPress3));
|
||||
client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger));
|
||||
client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery));
|
||||
client.publishMessage("kobuki/data/overCurrent",
|
||||
std::to_string(data.overCurrent));
|
||||
client.publishMessage("kobuki/data/IRSensorRight",
|
||||
std::to_string(data.IRSensorRight));
|
||||
client.publishMessage("kobuki/data/IRSensorCenter",
|
||||
std::to_string(data.IRSensorCenter));
|
||||
client.publishMessage("kobuki/data/IRSensorLeft",
|
||||
std::to_string(data.IRSensorLeft));
|
||||
client.publishMessage("kobuki/data/GyroAngle",
|
||||
std::to_string(data.GyroAngle));
|
||||
client.publishMessage("kobuki/data/GyroAngleRate",
|
||||
std::to_string(data.GyroAngleRate));
|
||||
client.publishMessage("kobuki/data/CliffSensorRight",
|
||||
std::to_string(data.CliffSensorRight));
|
||||
client.publishMessage("kobuki/data/CliffSensorCenter",
|
||||
std::to_string(data.CliffSensorCenter));
|
||||
client.publishMessage("kobuki/data/CliffSensorLeft",
|
||||
std::to_string(data.CliffSensorLeft));
|
||||
client.publishMessage("kobuki/data/wheelCurrentLeft",
|
||||
std::to_string(data.wheelCurrentLeft));
|
||||
client.publishMessage("kobuki/data/wheelCurrentRight",
|
||||
std::to_string(data.wheelCurrentRight));
|
||||
client.publishMessage("kobuki/data/digitalInput",
|
||||
std::to_string(data.digitalInput));
|
||||
client.publishMessage("kobuki/data/analogInputCh0",
|
||||
std::to_string(data.analogInputCh0));
|
||||
client.publishMessage("kobuki/data/analogInputCh1",
|
||||
std::to_string(data.analogInputCh1));
|
||||
client.publishMessage("kobuki/data/analogInputCh2",
|
||||
std::to_string(data.analogInputCh2));
|
||||
client.publishMessage("kobuki/data/analogInputCh3",
|
||||
std::to_string(data.analogInputCh3));
|
||||
client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch",
|
||||
std::to_string(data.extraInfo.HardwareVersionPatch));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor",
|
||||
std::to_string(data.extraInfo.HardwareVersionMinor));
|
||||
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor",
|
||||
std::to_string(data.extraInfo.HardwareVersionMajor));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch",
|
||||
std::to_string(data.extraInfo.FirmwareVersionPatch));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor",
|
||||
std::to_string(data.extraInfo.FirmwareVersionMinor));
|
||||
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor",
|
||||
std::to_string(data.extraInfo.FirmwareVersionMajor));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID0",
|
||||
std::to_string(data.extraInfo.UDID0));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID1",
|
||||
std::to_string(data.extraInfo.UDID1));
|
||||
client.publishMessage("kobuki/data/extraInfo/UDID2",
|
||||
std::to_string(data.extraInfo.UDID2));
|
||||
|
||||
if (!data.gyroData.empty()) {
|
||||
const auto& latestGyro = data.gyroData.back();
|
||||
client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x));
|
||||
client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y));
|
||||
client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z));
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
if (!data.gyroData.empty()) {
|
||||
const auto &latestGyro = data.gyroData.back();
|
||||
client.publishMessage("kobuki/data/gyroData/x",
|
||||
std::to_string(latestGyro.x));
|
||||
client.publishMessage("kobuki/data/gyroData/y",
|
||||
std::to_string(latestGyro.y));
|
||||
client.publishMessage("kobuki/data/gyroData/z",
|
||||
std::to_string(latestGyro.z));
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
}
|
||||
|
||||
std::string serializeKobukiData(const TKobukiData &data) {
|
||||
std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) +
|
||||
",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
|
||||
",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
|
||||
",\"BumperRight\":" + std::to_string(data.BumperRight) +
|
||||
",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) +
|
||||
",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) +
|
||||
",\"CliffCenter\":" + std::to_string(data.CliffCenter) +
|
||||
",\"CliffLeft\":" + std::to_string(data.CliffLeft) +
|
||||
",\"CliffRight\":" + std::to_string(data.CliffRight) +
|
||||
",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) +
|
||||
",\"EncoderRight\":" + std::to_string(data.EncoderRight) +
|
||||
",\"PWMleft\":" + std::to_string(data.PWMleft) +
|
||||
",\"PWMright\":" + std::to_string(data.PWMright) +
|
||||
",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) +
|
||||
",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) +
|
||||
",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) +
|
||||
",\"Charger\":" + std::to_string(data.Charger) +
|
||||
",\"Battery\":" + std::to_string(data.Battery) +
|
||||
",\"overCurrent\":" + std::to_string(data.overCurrent) +
|
||||
",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) +
|
||||
",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) +
|
||||
",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) +
|
||||
",\"GyroAngle\":" + std::to_string(data.GyroAngle) +
|
||||
",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) +
|
||||
",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) +
|
||||
",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) +
|
||||
",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) +
|
||||
",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) +
|
||||
",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) +
|
||||
",\"digitalInput\":" + std::to_string(data.digitalInput) +
|
||||
",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) +
|
||||
",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) +
|
||||
",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
|
||||
",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
|
||||
",\"frameId\":" + std::to_string(data.frameId) +
|
||||
",\"extraInfo\":{\"HardwareVersionPatch\":" + std::to_string(data.extraInfo.HardwareVersionPatch) +
|
||||
",\"HardwareVersionMinor\":" + std::to_string(data.extraInfo.HardwareVersionMinor) +
|
||||
",\"HardwareVersionMajor\":" + std::to_string(data.extraInfo.HardwareVersionMajor) +
|
||||
",\"FirmwareVersionPatch\":" + std::to_string(data.extraInfo.FirmwareVersionPatch) +
|
||||
",\"FirmwareVersionMinor\":" + std::to_string(data.extraInfo.FirmwareVersionMinor) +
|
||||
",\"FirmwareVersionMajor\":" + std::to_string(data.extraInfo.FirmwareVersionMajor) +
|
||||
",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
|
||||
",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
|
||||
",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
|
||||
std::string json =
|
||||
"{\"timestamp\":" + std::to_string(data.timestamp) +
|
||||
",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
|
||||
",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
|
||||
",\"BumperRight\":" + std::to_string(data.BumperRight) +
|
||||
",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) +
|
||||
",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) +
|
||||
",\"CliffCenter\":" + std::to_string(data.CliffCenter) +
|
||||
",\"CliffLeft\":" + std::to_string(data.CliffLeft) +
|
||||
",\"CliffRight\":" + std::to_string(data.CliffRight) +
|
||||
",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) +
|
||||
",\"EncoderRight\":" + std::to_string(data.EncoderRight) +
|
||||
",\"PWMleft\":" + std::to_string(data.PWMleft) +
|
||||
",\"PWMright\":" + std::to_string(data.PWMright) +
|
||||
",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) +
|
||||
",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) +
|
||||
",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) +
|
||||
",\"Charger\":" + std::to_string(data.Charger) +
|
||||
",\"Battery\":" + std::to_string(data.Battery) +
|
||||
",\"overCurrent\":" + std::to_string(data.overCurrent) +
|
||||
",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) +
|
||||
",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) +
|
||||
",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) +
|
||||
",\"GyroAngle\":" + std::to_string(data.GyroAngle) +
|
||||
",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) +
|
||||
",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) +
|
||||
",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) +
|
||||
",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) +
|
||||
",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) +
|
||||
",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) +
|
||||
",\"digitalInput\":" + std::to_string(data.digitalInput) +
|
||||
",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) +
|
||||
",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) +
|
||||
",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
|
||||
",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
|
||||
",\"frameId\":" + std::to_string(data.frameId) +
|
||||
",\"extraInfo\":{\"HardwareVersionPatch\":" +
|
||||
std::to_string(data.extraInfo.HardwareVersionPatch) +
|
||||
",\"HardwareVersionMinor\":" +
|
||||
std::to_string(data.extraInfo.HardwareVersionMinor) +
|
||||
",\"HardwareVersionMajor\":" +
|
||||
std::to_string(data.extraInfo.HardwareVersionMajor) +
|
||||
",\"FirmwareVersionPatch\":" +
|
||||
std::to_string(data.extraInfo.FirmwareVersionPatch) +
|
||||
",\"FirmwareVersionMinor\":" +
|
||||
std::to_string(data.extraInfo.FirmwareVersionMinor) +
|
||||
",\"FirmwareVersionMajor\":" +
|
||||
std::to_string(data.extraInfo.FirmwareVersionMajor) +
|
||||
",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
|
||||
",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
|
||||
",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
|
||||
|
||||
if (!data.gyroData.empty()) {
|
||||
const auto& latestGyro = data.gyroData.back();
|
||||
json += "{\"x\":" + std::to_string(latestGyro.x) +
|
||||
",\"y\":" + std::to_string(latestGyro.y) +
|
||||
",\"z\":" + std::to_string(latestGyro.z) + "}";
|
||||
}
|
||||
if (!data.gyroData.empty()) {
|
||||
const auto &latestGyro = data.gyroData.back();
|
||||
json += "{\"x\":" + std::to_string(latestGyro.x) +
|
||||
",\"y\":" + std::to_string(latestGyro.y) +
|
||||
",\"z\":" + std::to_string(latestGyro.z) + "}";
|
||||
}
|
||||
|
||||
json += "]}";
|
||||
return json;
|
||||
json += "]}";
|
||||
return json;
|
||||
}
|
||||
//create extra function to send the message every 100ms
|
||||
//needed it so it can be threaded
|
||||
// create extra function to send the message every 100ms
|
||||
// needed it so it can be threaded
|
||||
void sendKobukiData(TKobukiData &data) {
|
||||
while (true) {
|
||||
client.publishMessage("kobuki/data", serializeKobukiData(data));
|
||||
std::cout << "Sent data" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
while (true) {
|
||||
client.publishMessage("kobuki/data", serializeKobukiData(data));
|
||||
std::cout << "Sent data" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
}
|
||||
|
||||
void CapnSend() {
|
||||
VideoCapture cap(0);
|
||||
if (!cap.isOpened()) {
|
||||
cerr << "Error: Could not open camera" << endl;
|
||||
return;
|
||||
VideoCapture cap(0);
|
||||
if (!cap.isOpened()) {
|
||||
cerr << "Error: Could not open camera" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
Mat frame;
|
||||
while (true) {
|
||||
cap >> frame; // Capture a new image frame
|
||||
if (frame.empty()) {
|
||||
cerr << "Error: Could not capture image" << endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
Mat frame;
|
||||
while (true) {
|
||||
cap >> frame; // Capture a new image frame
|
||||
if (frame.empty()) {
|
||||
cerr << "Error: Could not capture image" << endl;
|
||||
continue;
|
||||
}
|
||||
// Convert the image to a byte array
|
||||
vector<uchar> buf;
|
||||
imencode(".jpg", frame, buf);
|
||||
auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data());
|
||||
|
||||
// Convert the image to a byte array
|
||||
vector<uchar> buf;
|
||||
imencode(".jpg", frame, buf);
|
||||
auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
|
||||
// Publish the image data
|
||||
client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
|
||||
cout << "Sent image" << endl;
|
||||
|
||||
// Publish the image data
|
||||
client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
|
||||
cout << "Sent image" << endl;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(300)); // Send image every 1000ms
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(300)); // Send image every 1000ms
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user