diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py
index cf89136..b43382a 100644
--- a/software/roboter/raspy/functions.py
+++ b/software/roboter/raspy/functions.py
@@ -54,8 +54,8 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
     logging.info("Measuring Sensors")
     measure_send_data(plantID, actionID, client)
 
-    # FIXME Send Picture
-    # sendPicture()
+    logging.info("Taking Picture")
+    sendPicture(client)
 
     logging.info("Robot driving home")
     errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
@@ -74,7 +74,6 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
 
 #region MQTT callbacks
 
-# TODO Test with all sensors
 def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     """
     Function to drive to plant according to request
@@ -87,8 +86,6 @@ def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     """
     dictMessage = json.loads(str(message.payload.decode("UTF-8")))
 
-    print(dictMessage)
-
     plantID = dictMessage["PlantID"]
     actionID = dictMessage["ActionID"]
 
@@ -148,8 +145,9 @@ def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
         "Battery": batteryLevel
     }
     client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
+    logging.info(f"Battery done {batteryLevel}")
+
 
-# TODO Test needed
 def sendPicture(client: mqtt.Client):
     """
     Takes picture and publish via MQTT
@@ -162,12 +160,12 @@ def sendPicture(client: mqtt.Client):
     except Exception as e:
         logging.error(str(e))
         client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
+        return
 
-    with open("picture.txt", "rb") as f:
+    with open("picture.png", "rb") as f:
         file = f.read()
         byteArr = bytearray(file)
-        print(byteArr)
-        client.publish(Topics["ROBOT_DATA_IMAGE"], byteArr)
-        print("Published")
+        client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
+        logging.info("Picture Published")
 
 #endregion
\ No newline at end of file
diff --git a/software/roboter/raspy/raspy_sensors.py b/software/roboter/raspy/raspy_sensors.py
index 455ff5e..42ad204 100644
--- a/software/roboter/raspy/raspy_sensors.py
+++ b/software/roboter/raspy/raspy_sensors.py
@@ -136,7 +136,6 @@ def readSensors():
     return sensorData
 
 
-# TODO - Test needed
 def takePicture():
     """
     Take picture and return picture
@@ -147,11 +146,9 @@ def takePicture():
         camera = PiCamera()
     except:
         raise Exception("Camera not connected")
-        return
     
     camera.start_preview()
     camera.capture("picture.png")
-    print("Camera on")
     camera.stop_preview()
 
     return 
@@ -174,7 +171,7 @@ def readPosition():
 def main():
     value = SENSORDATA
     try:
-        takePicture()
+        readSensors()
     except Exception as e:
         print(str(e))