diff --git a/config/config.yaml b/config/config.yaml
index 7d0b038db200970d9ec725796a377c5b9d4b4284..e5c9b018dbe4bab7ffc0124edfb184cb81c48e02 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -6,7 +6,8 @@ panda_mqtt_connector:
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"
-    trajectoryConfig: "trajectoryconfig"
+    trajectory: "trajectory"
+    nextStep: "ros2rag/nextStep"
   zone_size: 0.5
   zones:
     - "1 1"
@@ -25,3 +26,10 @@ panda_mqtt_connector:
   end_effectors:
     panda:
       EndEffector: "panda::panda_link7"
+  goal_poses:
+    - position: "0.4 0.3 0.1"
+      wait: "5000"
+    - position: "0.4 0.0 0.1"
+      wait: "3000"
+    - position: "0.4 -0.3 0.1"
+      wait: "4000"
diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp
index 321af1ec4b27c41a92a8f38aef8aa7c738911028..0bf073ecf8968ffce2c65acab217a69cc0fbad22 100644
--- a/src/MqttRosConnectionTestNode.cpp
+++ b/src/MqttRosConnectionTestNode.cpp
@@ -37,7 +37,7 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) {
   std::string mqtt_msg;
   traj.SerializeToString(&mqtt_msg);
 
-  auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
+  auto pubmsg = mqtt::make_message("trajectory", mqtt_msg);
   mqttUtil->getClient().publish(pubmsg);
 
 
diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index d7c08ad69f10e5fa9c2359e604a270f6afbe6d09..da043954ca4264bf90eeebb5ec1fe80717bc1ff8 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -13,7 +13,7 @@ const std::string CLIENT_ID{"mqtt_to_ros"};
 
 const std::string ROBOT_CONFIG{"robotconfig"};
 const std::string DATA_CONFIG{"dataconfig"};
-const std::string TRAJECTORY_CONFIG{"trajectoryconfig"};
+const std::string TRAJECTORY_CONFIG{"trajectory"};
 
 MqttUtil *mqttUtil = nullptr;