configuration of looped trajectory via mqtt

src/MqttRosConnectionTestNode.cpp

@@ -17,9 +17,9 @@ using namespace std::chrono;
  * mqtt-topics for all links
  * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
  */
-std::string topics[11] = {"panda_link_0", "panda_link_1", "panda_link_2", "panda_link_3",
-                          "panda_link_4", "panda_link_5", "panda_link_6", "panda_link_7",
-                          "panda_link_8", "panda_link_9", "panda_link_10"};
+std::string topics[11] = {"panda_ground", "panda_link_0", "panda_link_1", "panda_link_2",
+                          "panda_link_3", "panda_link_4", "panda_link_5", "panda_link_6",
+                          "panda_link_7", "panda_link_8", "panda_link_9"};
 
 const string SERVER_ADDRESS	{ "tcp://localhost:1883" };
 const string CLIENT_ID		{ "ros_mqtt_tester" };
@@ -65,27 +65,57 @@ bool try_reconnect(mqtt::client& cli)
     return false;
 }
 
+void testTrajectoryUpdate(ros::NodeHandle n){
 
-void testConfig(ros::NodeHandle n) {
-
-    std::cout << ">>>>>>>>>>>> STARTING TEST <<<<<<<<<<<<<" << std::endl;
+    std::cout << ">>>>>>>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<<<<<<<" << std::endl;
     setupMqtt();
 
-    /*config::DataConfig dc;
+    plan::Trajectory traj;
 
-    dc.set_enableposition(true);
-    dc.set_enableorientation(true);
-    dc.set_enabletwistangular(true);
-    dc.set_enabletwistlinear(true);
-    dc.set_publishrate(1000);
+    plan::Trajectory_Position pos1;
+    pos1.set_x(0.6);
+    pos1.set_y(0.0);
+    pos1.set_z(0.6);
+
+    traj.add_pos()->CopyFrom(pos1);
+
+    plan::Trajectory_Position pos2;
+    pos2.set_x(-0.6);
+    pos2.set_y(0.0);
+    pos2.set_z(0.6);
+
+    traj.add_pos()->CopyFrom(pos2);
 
     std::string mqtt_msg;
-    dc.SerializeToString(&mqtt_msg);
+    traj.SerializeToString(&mqtt_msg);
 
-    auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
+    auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
     pubmsg->set_qos(QOS);
-    client.publish(pubmsg);*/
+    client.publish(pubmsg);
 
+    std::cout << ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<" << std::endl;
+
+    std::vector<double> x_pos;
+    std::vector<double> y_pos;
+    std::vector<double> z_pos;
+
+    ros::Duration(5.0).sleep();
+
+    std::cout << "x configured: " << n.getParam("trajectory_pos_x", x_pos) << std::endl;
+    std::cout << "y configured: " << n.getParam("trajectory_pos_y", y_pos) << std::endl;
+    std::cout << "z configured: " << n.getParam("trajectory_pos_z", z_pos) << std::endl;
+
+    for(int i = 0; i < x_pos.size(); i++)
+    {
+        std::cout << "X POS: " << x_pos.at(i) << std::endl;
+        std::cout << "Y POS: " << y_pos.at(i) << std::endl;
+        std::cout << "Z POS: " << z_pos.at(i) << std::endl;
+    }
+}
+
+void testPlanningModeChange(ros::NodeHandle n){
+
+    std::cout << ">>>>>>>>>>>> STARTING PLANNING TEST <<<<<<<<<<<<<" << std::endl;
     setupMqtt();
 
     config::RobotConfig rc;
@@ -99,34 +129,27 @@ void testConfig(ros::NodeHandle n) {
     auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
     pubmsg->set_qos(QOS);
     client.publish(pubmsg);
+}
 
-    /*plan::Trajectory traj;
+void testConfig(ros::NodeHandle n) {
+
+    std::cout << ">>>>>>>>>>>> STARTING CONFIG TEST <<<<<<<<<<<<<" << std::endl;
+    setupMqtt();
 
-    plan::Trajectory_Position pos;
-    pos.set_x(0.6);
-    pos.set_y(0.0);
-    pos.set_z(0.6);
+    config::DataConfig dc;
 
-    traj.add_pos()->CopyFrom(pos);
+    dc.set_enableposition(true);
+    dc.set_enableorientation(true);
+    dc.set_enabletwistangular(true);
+    dc.set_enabletwistlinear(true);
+    dc.set_publishrate(1000);
 
     std::string mqtt_msg;
-    traj.SerializeToString(&mqtt_msg);
+    dc.SerializeToString(&mqtt_msg);
 
-    auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
+    auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
     pubmsg->set_qos(QOS);
     client.publish(pubmsg);
-
-    std::cout << ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<" << std::endl;
-
-    std::vector<double> x_pos;
-    ros::Duration(5.0).sleep();
-    std:cout << n.getParam("trajectory_pos_x", x_pos) << std::endl;
-
-    for(int i = 0; i < x_pos.size(); i++)
-    {
-        std::cout << "X POS: " << x_pos.at(i) << std::endl;
-    }*/
-
 }
 
 void receiveMqttMessages(ros::NodeHandle n)
@@ -143,7 +166,9 @@ void receiveMqttMessages(ros::NodeHandle n)
 
         if (!rsp.is_session_present()) {
             ROS_INFO_STREAM("TEST Subscribing to topics.");
-            cli.subscribe("nodetest", 1);
+            cli.subscribe("configtest", 1);
+            cli.subscribe("modetest", 1);
+            cli.subscribe("trajectorytest", 1);
             ROS_INFO_STREAM("TEST Subscribed to topics.");
         }else {
             ROS_WARN_STREAM("TEST Session already present. Skipping subscribe.");
@@ -172,10 +197,20 @@ void receiveMqttMessages(ros::NodeHandle n)
                 break;
             }
 
-            if (msg->get_topic() == "nodetest")
+            if (msg->get_topic() == "configtest")
             {
                 testConfig(n);
             }
+
+            if (msg->get_topic() == "modetest")
+            {
+                testPlanningModeChange(n);
+            }
+
+            if (msg->get_topic() == "trajectorytest")
+            {
+                testTrajectoryUpdate(n);
+            }
         }
     }
     catch (const mqtt::exception& exc) {

src/robot_state_initializer.cpp

@@ -5,6 +5,7 @@
 void initSystemVariables(ros::NodeHandle node_handle)
 {
     node_handle.setParam("new_trajectory_available", false);
+    node_handle.setParam("loop_trajectory", true);
 }
 
 int main(int argc, char **argv) {

src/util/TrajectoryUtil.h

@@ -21,9 +21,6 @@ public:
     static const std::string CARTESIAN_PATH;
     static const std::string FLUID_PATH;
 
-    //const std::string LIMITED_EXECUTION = "limited_execution";
-    //const std::string UNLIMITED_EXECUTION = "unlimited_execution";
-
     bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                            moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
                            std::string pathType, double velocity);