re-added robotsafety

src/C++/Driver/src/KobukiDriver/CKobuki.cpp

@@ -781,6 +781,20 @@ void CKobuki::Rotate(int degrees) {
 }
 
 
+void CKobuki::robotSafety(std::string *pointerToMessage) {
+    while (true) {
+
+        if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || 
+            parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) {
+            std::cout << "Safety condition triggered!" << std::endl; // Debug print
+            *pointerToMessage = "stop";
+            forward(-100); // reverse the robot
+
+        }
+
+    }
+}
+
 void CKobuki::robotSafety() {
     while (true) {
 

src/C++/Driver/src/KobukiDriver/CKobuki.h

@@ -76,7 +76,8 @@ public:
 	void goToXy(long double xx, long double yy);
 	void Rotate(int degrees);
 	std::ofstream odometry_log;
-	void robotSafety();
+	void robotSafety(std::string *pointerToMessage);
+	void robotSafety(); //overload
 	void sendNullMessage();
 	KobukiParser parser;
 

src/C++/Driver/src/main.cpp

@@ -12,26 +12,29 @@ int movement();
 std::string readMQTT();
 void parseMQTT(std::string message);
 MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object
-
+std::string message;
 
 void setup(){
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
     client.connect();
     client.subscribe("home/commands");
+
 }
 
 int main(){
 	setup();
+    std::thread safety([&]() { robot.robotSafety(&message); });
 	while(true){
     parseMQTT(readMQTT());
 	}
+    safety.join();
 	return 0;
 }
 
 std::string readMQTT()
 {
-        std::string message = client.getLastMessage();
+        message = client.getLastMessage();
         if (!message.empty()) {
             std::cout << "MQTT Message: " << message << std::endl;
         }