align config with ros2rag

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"

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);
 
 

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;