diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index 8854df7be4d9047a145cd4a4fb2e23b7f19dbfe6..0709efe25d72a402d386605f76353425b771fbb3 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -23,11 +23,11 @@ void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHa
   n.setParam("robot_speed_factor", robotConfig.speed());
 }
 
-void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) {
+void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Publisher &trajectoryPublisher) {
 
-  panda_mqtt_connector::Trajectory poseArray;
+  panda_mqtt_connector::Trajectory trajectory;
 
-  for (const auto &protoPose : trajectory.pose()) {
+  for (const auto &protoPose : protoTrajectory.pose()) {
     panda_mqtt_connector::Waypoint waypoint;
     waypoint.pose.orientation.w = protoPose.orientation().w();
     waypoint.pose.orientation.x = protoPose.orientation().x();
@@ -48,16 +48,16 @@ void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publishe
         ROS_WARN("Received pose has invalid mode!");
     }
 
-    poseArray.waypoints.push_back(waypoint);
+    trajectory.waypoints.push_back(waypoint);
   }
 
-  poseArray.loop = trajectory.loop();
+  trajectory.loop = protoTrajectory.loop();
 
-  posePublisher.publish(poseArray);
+  trajectoryPublisher.publish(trajectory);
 
 }
 
-void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
+void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher) {
   if (mqttUtil->ensureConnection()) {
     mqtt::const_message_ptr msg;
     if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
@@ -71,7 +71,7 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePub
         const std::string tc_payload = msg->get_payload_str();
         plan::Trajectory trajectoryConfig;
         trajectoryConfig.ParseFromString(tc_payload);
-        handleNewTrajectory(trajectoryConfig, posePublisher);
+        handleNewTrajectory(trajectoryConfig, trajectoryPublisher);
       }
     }
   } else {
@@ -83,7 +83,7 @@ int main(int argc, char *argv[]) {
   ros::init(argc, argv, "mqtt_listener");
   ros::NodeHandle n("panda_mqtt_connector");
 
-  ros::Publisher posePublisher = n.advertise<panda_mqtt_connector::Trajectory>("poses", 1000);
+  ros::Publisher trajectoryPublisher = n.advertise<panda_mqtt_connector::Trajectory>("trajectory_update", 1000);
 
   std::string server;
   if (!n.getParam("server", server)) {
@@ -111,7 +111,7 @@ int main(int argc, char *argv[]) {
   }
 
   while (ros::ok()) {
-    receiveMqttMessages(n, posePublisher);
+    receiveMqttMessages(n, trajectoryPublisher);
     ros::spinOnce();
   }
 
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index c31bd089929fc00412b145ee401191acbe919103..cb743cd0dce4379e69e1a9412483c179abdc789e 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -14,7 +14,6 @@
 #include "util/MqttUtil.h"
 
 namespace robot_state_provider {
-  bool export_to_log = false;
 
 /**
  * A map from the ROS element name to an MQTT topic
@@ -30,49 +29,6 @@ namespace robot_state_provider {
 
   MqttUtil *mqttUtil = nullptr;
 
-/*
- * logs to its own file in /.ros/logs (configured in launch-file)
- */
-  void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
-
-    ROS_INFO_STREAM("<<< LINK NAMES >>>");
-
-    for (int i = 0; i < msg.name.size(); i++) {
-      ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
-    }
-
-    ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
-
-    for (int i = 0; i < msg.pose.size(); i++) {
-      ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
-      ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
-      ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
-    }
-
-    ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
-
-    for (int i = 0; i < msg.pose.size(); i++) {
-      ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
-      ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
-      ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
-      ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
-    }
-
-    ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
-    for (int i = 0; i < msg.twist.size(); i++) {
-      ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
-      ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
-      ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
-    }
-
-    ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
-    for (int i = 0; i < msg.twist.size(); i++) {
-      ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
-      ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
-      ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
-    }
-  }
-
   std::vector<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
 
     std::vector<robot::RobotState> messages{};
@@ -159,9 +115,6 @@ namespace robot_state_provider {
     }
 
     if (current_redirect == 0) {
-      if (export_to_log) {
-        exportMessageToLog(msg);
-      }
       sendMqttMessages(msg);
     }
     current_redirect = (current_redirect + 1) % message_redirect_rate;
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 34b8d3bb2a375b874be4a2095a33e3cce61ddb9f..91c3d3702bd2f9be5e6141eb77be41824f5dc515 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -50,7 +50,7 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou
 
 void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
 
-  nextPoses = panda_mqtt_connector::Trajectory{};
+  nextTrajectory = panda_mqtt_connector::Trajectory{};
 
   double step_size_rad = 2 * M_PI / granularity;
 
@@ -63,21 +63,22 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
     tf2::Quaternion orientation; // the RPY constructor is deprecated
     orientation.setRPY(0, M_PI, next_rad);
 
-    panda_mqtt_connector::Waypoint nextPose{};
-    nextPose.pose.orientation = tf2::toMsg(orientation);
-    nextPose.pose.position.x = origin.x + radius * sin(next_rad);
-    nextPose.pose.position.y = origin.y + radius * cos(next_rad);
-    nextPose.pose.position.z = origin.z;
+    panda_mqtt_connector::Waypoint nextWaypoint{};
+    nextWaypoint.pose.orientation = tf2::toMsg(orientation);
+    nextWaypoint.pose.position.x = origin.x + radius * sin(next_rad);
+    nextWaypoint.pose.position.y = origin.y + radius * cos(next_rad);
+    nextWaypoint.pose.position.z = origin.z;
+    nextWaypoint.mode = TrajectoryUtil::FLUID_PATH;
 
-    nextPoses->waypoints.push_back(nextPose);
+    nextTrajectory->waypoints.push_back(nextWaypoint);
   }
 }
 
 bool TimedPlanner::loadWaypoints() {
 
-  if (nextPoses.is_initialized()) {
-    poses = nextPoses.get();
-    nextPoses.reset();
+  if (nextTrajectory.is_initialized()) {
+    currentTrajectory = nextTrajectory.get();
+    nextTrajectory.reset();
     return true;
   } else {
     return false;
@@ -113,18 +114,14 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
 
   loadWaypoints();
 
-  while (poses.loop) {
-    for (auto &waypoint : poses.waypoints) {
-
-      ROS_INFO_STREAM("Planning the next trajectory in " << waypoint.mode);
+  while (currentTrajectory.loop) {
+    for (auto &waypoint : currentTrajectory.waypoints) {
 
       moveit::planning_interface::MoveGroupInterface::Plan plan;
 
-      ROS_ERROR_STREAM(waypoint);
-
       if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
 
-        if (nextPoses.is_initialized()) {
+        if (nextTrajectory.is_initialized()) {
           return;
         }
 
@@ -137,7 +134,8 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
             ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
             return;
           }
-          ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor);
+          ROS_INFO_STREAM(
+              "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
           TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
           moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
           if (errorCode.val != errorCode.SUCCESS) {
@@ -145,15 +143,16 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
           }
         }
       } else {
-        ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
+        ROS_ERROR_STREAM(
+            "Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
       }
     }
   }
 }
 
-void TimedPlanner::newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
-  ROS_INFO_STREAM("Received new pose list with " << msg->waypoints.size() << " poses.");
-  nextPoses = *msg;
+void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
+  ROS_INFO_STREAM("Received new trajectory with " << msg->waypoints.size() << " waypoints.");
+  nextTrajectory = *msg;
 }
 
 TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
@@ -162,51 +161,48 @@ TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group
 
   ros::NodeHandle node_handle("panda_mqtt_connector");
 
-  std::string defaultTrajectory = "<none>";
+  std::string defaultTrajectory = "<no value provided>";
   node_handle.getParam("default_trajectory", defaultTrajectory);
 
-  geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
-
-  if (loadWaypoints()) {
-    ROS_INFO_STREAM("Ignoring default waypoints because poses were already received as a message");
-  } else if (defaultTrajectory == "circle") {
+  if (defaultTrajectory == "circle") {
     ROS_INFO_STREAM("loading default trajectory 'circle'");
     geometry_msgs::Point origin;
     origin.z = .3;
     loadCircularTrajectory(0.6, origin, 20);
   } else if (defaultTrajectory == "square") {
-    loadSquareTrajectory(base_pose);
-
+    ROS_INFO_STREAM("loading default trajectory 'square'");
+    loadSquareTrajectory();
   } else {
-    ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
+    ROS_WARN_STREAM("Not loading a default trajectory! There is no '" + defaultTrajectory + "'.");
   }
 
 }
 
-void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) {
-  ROS_INFO_STREAM("loading default trajectory 'square'");
-  // choose a default trajectory
-  base_pose.position.x = 0.4;
-  base_pose.position.y = 0.4;
-  base_pose.position.z = 0.3;
-  panda_mqtt_connector::Waypoint target_pose_1;
-  target_pose_1.pose = base_pose;
-  target_pose_1.mode = TrajectoryUtil::FLUID_PATH;
-  panda_mqtt_connector::Waypoint target_pose_2{target_pose_1};
-  panda_mqtt_connector::Waypoint target_pose_3{target_pose_1};
-  panda_mqtt_connector::Waypoint target_pose_4{target_pose_1};
-
-  poses.waypoints.push_back(target_pose_1);
-
-  target_pose_2.pose.position.y *= -1;
-  poses.waypoints.push_back(target_pose_2);
-
-  target_pose_3.pose.position.x *= -1;
-  target_pose_3.pose.position.y *= -1;
-  poses.waypoints.push_back(target_pose_3);
-
-  target_pose_4.pose.position.x *= -1;
-  poses.waypoints.push_back(target_pose_4);
+void TimedPlanner::loadSquareTrajectory() {
+  panda_mqtt_connector::Waypoint waypoint_1;
+  waypoint_1.pose.position.x = 0.4;
+  waypoint_1.pose.position.y = 0.4;
+  waypoint_1.pose.position.z = 0.3;
+  waypoint_1.pose.orientation.w = 0;
+  waypoint_1.pose.orientation.x = 0.7071067811865476;
+  waypoint_1.pose.orientation.y = 0.7071067811865476;
+  waypoint_1.pose.orientation.z = 0;
+  waypoint_1.mode = TrajectoryUtil::FLUID_PATH;
+  panda_mqtt_connector::Waypoint waypoint_2{waypoint_1};
+  panda_mqtt_connector::Waypoint waypoint_3{waypoint_1};
+  panda_mqtt_connector::Waypoint waypoint_4{waypoint_1};
+
+  currentTrajectory.waypoints.push_back(waypoint_1);
+
+  waypoint_2.pose.position.y *= -1;
+  currentTrajectory.waypoints.push_back(waypoint_2);
+
+  waypoint_3.pose.position.x *= -1;
+  waypoint_3.pose.position.y *= -1;
+  currentTrajectory.waypoints.push_back(waypoint_3);
+
+  waypoint_4.pose.position.x *= -1;
+  currentTrajectory.waypoints.push_back(waypoint_4);
 }
 
 
@@ -228,7 +224,8 @@ int main(int argc, char **argv) {
 
   TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH);
 
-  ros::Subscriber sub = node_handle.subscribe("poses", 1000, &TimedPlanner::newPoseCallback, &planner);
+  ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
+                                              &planner);
 
   // add the ground plate
   std::vector<moveit_msgs::CollisionObject> collision_objects;
diff --git a/src/TimedPlanner.h b/src/TimedPlanner.h
index 2031794fd6de26a254c9c46e68055d16841d5092..7c5ad38ea04c88f840ad0e75a9de795668e3529d 100644
--- a/src/TimedPlanner.h
+++ b/src/TimedPlanner.h
@@ -11,31 +11,32 @@
 
 #include "panda_mqtt_connector/Trajectory.h"
 
+using moveit::planning_interface::MoveGroupInterface;
+
 class TimedPlanner {
 
 public:
   const double default_velocity = 1.0;
   const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
 
-  static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group,
-                                                         const moveit::planning_interface::MoveGroupInterface::Plan &plan);
+  static std::vector<moveit_msgs::RobotTrajectory>
+  split(MoveGroupInterface &group, const MoveGroupInterface::Plan &plan);
 
-  void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group);
+  void doMotion(const ros::NodeHandle &node_handle, MoveGroupInterface &group);
 
-  TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity,
-               std::string defaultPlanningMode);
+  TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
 
-  void newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
+  void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
 
 private:
-  panda_mqtt_connector::Trajectory poses;
-  boost::optional<panda_mqtt_connector::Trajectory> nextPoses;
-
-  void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
+  panda_mqtt_connector::Trajectory currentTrajectory;
+  boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory;
 
   bool loadWaypoints();
 
-  void loadSquareTrajectory(geometry_msgs::Pose base_pose);
+  void loadSquareTrajectory();
+
+  void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
 };