diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp
index fffae607e1c0d216924219a643ee6509c4f74975..2ad53cc3cbec4a19f519a7de2b739b894fe33e57 100644
--- a/src/dummy_sorting_controller.cpp
+++ b/src/dummy_sorting_controller.cpp
@@ -56,20 +56,8 @@ int main(int argc, char** argv)
 
   ros::WallDuration(5).sleep();  // wait 5 secs to init scene (to give moveit time to load)
 
-  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/con"
-                                                                                                                  "fig/"
-                                                                                                                  "conf"
-                                                                                                                  "ig_"
-                                                                                                                  "scen"
-                                                                                                                  "e_"
-                                                                                                                  "ceti"
-                                                                                                                  "-tab"
-                                                                                                                  "le-"
-                                                                                                                  "plac"
-                                                                                                                  "ewor"
-                                                                                                                  "ld."
-                                                                                                                  "jso"
-                                                                                                                  "n"));
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+      "/config/config_scene_ceti-table-placeworld.json"));
 
   std::optional<std::string> currentlyPickedBox{};
 
@@ -83,51 +71,51 @@ int main(int argc, char** argv)
     const PickAndPlace& pick_place = command.pickandplace();
     if (command.has_pickandplace())
     {
-      ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into "
-                                                    << pick_place.idplace());
+      ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
       Object* pick_object = controller.resolveObject(pick_place.idpick());
       if (!pick_object)
       {
-        ROS_ERROR_STREAM("Selected unknown pick object '" << pick_place.idpick() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'");
         return;
       }
       Object* place_object = controller.resolveObject(pick_place.idplace());
       if (!place_object)
       {
-        ROS_ERROR_STREAM("Selected unknown place object '" << pick_place.idplace() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'");
         return;
       }
       if (pick_object->type() != Object::BOX)
       {
-        ROS_WARN_STREAM("Selected unsupported pick object of type " << pick_object->type());
+        ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported pick object of type " << pick_object->type());
       }
       if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION &&
           place_object->type() != Object::COLLABORATION_ZONE)
       {
-        ROS_WARN_STREAM("Selected unsupported place object of type " << place_object->type());
+        ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported place object of type " << place_object->type());
       }
       if (command.pickandplace().idrobot() != controller.getRobotName())
       {
-        ROS_WARN_STREAM("Ignoring command pick-place command for robot "
-                        << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
+        ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot "
+                         << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
         return;
       }
 
       Object* robot = controller.resolveObject(command.pickandplace().idrobot());
       if (!robot)
       {
-        ROS_ERROR_STREAM("Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
+                                                                         << "' not found!");
         return;
       }
       if (place_object->type() == Object::BIN)
       {
         if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false))
         {
-          ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!");
         }
         else
         {
-          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << pick_place.idpick() << "' is no more.");
           controller.sendScene();
         }
       }
@@ -135,28 +123,31 @@ int main(int argc, char** argv)
       {
         if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false))
         {
-          ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!");
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << pick_place.idpick() << "'!");
         }
         else
         {
-          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location.");
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! " << pick_place.idpick() << "' is at its new location.");
           controller.sendScene();
         }
       }
     }
     else if (command.has_evacuate())
     {
+      ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone "
+                                            << command.evacuate().idcollaborationzone());
       if (command.evacuate().idrobot() != controller.getRobotName())
       {
-        ROS_WARN_STREAM("Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is "
-                                                                         << controller.getRobotName() << ")");
+        ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot "
+                         << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
         return;
       }
 
       Object* robot = controller.resolveObject(command.evacuate().idrobot());
       if (!robot)
       {
-        ROS_ERROR_STREAM("Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
+                                                                         << "' not found!");
         return;
       }
 
@@ -165,28 +156,38 @@ int main(int argc, char** argv)
       pose.position.x = robot->pos().x() + .4;
       pose.position.y = robot->pos().y();
       pose.position.z = robot->pos().z() + .4;
-      controller.moveToPose(*robot, pose, false);
+      if (controller.moveToPose(*robot, pose, false))
+      {
+        ROS_INFO_STREAM("[COMMAND] SUCCESS! Evacuation complete!");
+      }
+      else
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Evacuation did not complete.");
+      }
     }
     else if (command.has_configchange())
     {
       Object* zone = controller.resolveObject(command.configchange().idcollaborationzone());
+      ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone << " to owner "
+                                                         << command.configchange().idrobotnewowner());
       if (!zone)
       {
-        ROS_ERROR_STREAM("Unable to configure unknown collaboration zone '" << command.evacuate().idrobot() << "'.");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to configure unknown collaboration zone '"
+                         << command.evacuate().idrobot() << "'.");
         return;
       }
 
       Object* robot = controller.resolveObject(command.configchange().idrobotnewowner());
       if (!robot)
       {
-        ROS_WARN_STREAM("Collaboration zone active for unknown robot '" << command.evacuate().idrobot()
-                                                                        << "', so blocked in all.");
+        ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot()
+                                                                                        << "', so blocked in all.");
       }
       controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
     }
     else
     {
-      ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
+      ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
     }
   };
   controller.reactToCommandMessage(pick_place_callback);
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index 03f2ca80a1f04d5a3238d878c10cdce1d1ada372..793bed6bb8df47123f095492ca764110630d129c 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -23,24 +23,23 @@ std::string NODE_NAME = "ceti_cell_placeworld";
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
-
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
   ros::init(argc, argv, NODE_NAME);
   NODE_NAME = ros::this_node::getName();
 
-  ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
+  ros::NodeHandle n("connector_node_ros_ccf");  // namespace where the connection_address is
 
   auto robotName = getPrivateParameter<std::string>("arm", "arm1");
   ROS_INFO_STREAM("This cell controls arm " << robotName);
 
-  MoveItRobotArmController controller{n, NODE_NAME, robotName};
+  MoveItRobotArmController controller{ n, NODE_NAME, robotName };
 
   auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
-  std::unique_ptr<MqttConnection>
-      mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
+  std::unique_ptr<MqttConnection> mqtt_connection =
+      std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
   mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
   auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command");
   mqtt_connection->listen(commandTopic);
@@ -55,91 +54,100 @@ int main(int argc, char **argv)
     controller.initScene(scene, false);
   });
 
-  ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+  ros::WallDuration(5).sleep();  // wait 5 secs to init scene (to give moveit time to load)
 
   controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
       "/config/config_scene_ceti-table-placeworld.json"));
 
   std::optional<std::string> currentlyPickedBox{};
 
-  ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
-      const std_msgs::EmptyConstPtr &msg)
-  {
-      ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
-      controller.sendScene();
-  });
+  ros::Subscriber sub =
+      n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) {
+        ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
+        controller.sendScene();
+      });
 
-  auto pick_place_callback = [&controller](const Command& command)
-  {
+  auto pick_place_callback = [&controller](const Command& command) {
     const PickAndPlace& pick_place = command.pickandplace();
-    if (command.has_pickandplace()) {
-      ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
-      Object *pick_object = controller.resolveObject(pick_place.idpick());
+    if (command.has_pickandplace())
+    {
+      ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
+      Object* pick_object = controller.resolveObject(pick_place.idpick());
       if (!pick_object)
       {
-        ROS_ERROR_STREAM(
-            "Selected unknown pick object '" << pick_place.idpick() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'");
         return;
       }
-      Object *place_object = controller.resolveObject(pick_place.idplace());
+      Object* place_object = controller.resolveObject(pick_place.idplace());
       if (!place_object)
       {
-        ROS_ERROR_STREAM(
-            "Selected unknown place object '" << pick_place.idplace() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'");
         return;
       }
       if (pick_object->type() != Object::BOX)
       {
-        ROS_WARN_STREAM(
-            "Selected unsupported pick object of type " << pick_object->type());
+        ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported pick object of type " << pick_object->type());
       }
-      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION && place_object->type() != Object::COLLABORATION_ZONE)
+      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION &&
+          place_object->type() != Object::COLLABORATION_ZONE)
       {
-        ROS_WARN_STREAM(
-            "Selected unsupported place object of type " << place_object->type());
+        ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported place object of type " << place_object->type());
       }
-      if (command.pickandplace().idrobot() != controller.getRobotName()) {
-        ROS_WARN_STREAM(
-            "Ignoring command pick-place command for robot " << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
+      if (command.pickandplace().idrobot() != controller.getRobotName())
+      {
+        ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot "
+                         << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
         return;
       }
 
-      Object *robot = controller.resolveObject(command.pickandplace().idrobot());
+      Object* robot = controller.resolveObject(command.pickandplace().idrobot());
       if (!robot)
       {
-        ROS_ERROR_STREAM(
-            "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
+                                                                         << "' not found!");
         return;
       }
-      if (place_object->type() == Object::BIN) {
-        if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
-          ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
-        } else {
-          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
+      if (place_object->type() == Object::BIN)
+      {
+        if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false))
+        {
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!");
+        }
+        else
+        {
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << pick_place.idpick() << "' is no more.");
           controller.sendScene();
         }
-      } else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE) {
-        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) {
-          ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!");
-        } else {
-          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location.");
+      }
+      else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE)
+      {
+        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false))
+        {
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << pick_place.idpick() << "'!");
+        }
+        else
+        {
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! " << pick_place.idpick() << "' is at its new location.");
           controller.sendScene();
         }
       }
-    } else if (command.has_evacuate()) {
-
-
-      if (command.evacuate().idrobot() != controller.getRobotName()) {
-        ROS_WARN_STREAM(
-            "Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
+    }
+    else if (command.has_evacuate())
+    {
+      ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone "
+                                            << command.evacuate().idcollaborationzone());
+      if (command.evacuate().idrobot() != controller.getRobotName())
+      {
+        ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot "
+                         << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
         return;
       }
 
-      Object *robot = controller.resolveObject(command.evacuate().idrobot());
+      Object* robot = controller.resolveObject(command.evacuate().idrobot());
       if (!robot)
       {
-        ROS_ERROR_STREAM(
-            "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
+                                                                         << "' not found!");
         return;
       }
 
@@ -148,31 +156,43 @@ int main(int argc, char **argv)
       pose.position.x = robot->pos().x() + .4;
       pose.position.y = robot->pos().y();
       pose.position.z = robot->pos().z() + .4;
-      controller.moveToPose(*robot, pose, false);
-
-    } else if (command.has_configchange()) {
-      Object *zone = controller.resolveObject(command.configchange().idcollaborationzone());
+      if (controller.moveToPose(*robot, pose, false))
+      {
+        ROS_INFO_STREAM("[COMMAND] SUCCESS! Evacuation complete!");
+      }
+      else
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Evacuation did not complete.");
+      }
+    }
+    else if (command.has_configchange())
+    {
+      Object* zone = controller.resolveObject(command.configchange().idcollaborationzone());
+      ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone->id() << " to owner "
+                                                         << command.configchange().idrobotnewowner());
       if (!zone)
       {
-        ROS_ERROR_STREAM("Unable to configure unknown collaboration zone '" << command.evacuate().idrobot() << "'.");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to configure unknown collaboration zone '"
+                         << command.evacuate().idrobot() << "'.");
         return;
       }
 
-      Object *robot = controller.resolveObject(command.configchange().idrobotnewowner());
+      Object* robot = controller.resolveObject(command.configchange().idrobotnewowner());
       if (!robot)
       {
-        ROS_WARN_STREAM(
-            "Collaboration zone active for unknown robot '" << command.evacuate().idrobot() << "', so blocked in all.");
+        ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot()
+                                                                                        << "', so blocked in all.");
       }
       controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
-    } else {
-      ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
+    }
+    else
+    {
+      ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
     }
   };
   controller.reactToCommandMessage(pick_place_callback);
 
-  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
-  {
+  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() {
     if (currentlyPickedBox.has_value())
     {
       auto resolved = controller.resolveObject(currentlyPickedBox.value());