improve trajectory test

src/MqttRosConnectionTestNode.cpp

 //
 // Created by sebastian on 31.03.20.
 //
+#include <sstream>
+
 #include "config.pb.h"
 #include "trajectory.pb.h"
 
 #include "ros/ros.h"
 
 #include "util/MqttUtil.h"
+#include "util/TrajectoryUtil.h"
 
 const std::string CLIENT_ID{"ros_mqtt_tester"};
 
 MqttUtil *mqttUtil = nullptr;
 
-void testTrajectoryUpdate(const ros::NodeHandle& n) {
+void testTrajectoryUpdate(bool loop, plan::Trajectory_PlanningMode mode) {
 
-  ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner.");
+  ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner. "
+                      << (loop ? "" : "non-") << "looping, "
+                      << (mode == plan::Trajectory_PlanningMode::Trajectory_PlanningMode_CARTESIAN
+                          ? TrajectoryUtil::CARTESIAN_PATH : TrajectoryUtil::FLUID_PATH));
 
   plan::Trajectory trajectory{};
-  trajectory.set_loop(false);
+  trajectory.set_loop(loop);
 
-  plan::Trajectory_Pose* pose = trajectory.add_pose();
-  pose->set_mode(plan::Trajectory_PlanningMode_FLUID);
+  plan::Trajectory_Pose *pose = trajectory.add_pose();
+  pose->set_mode(mode);
   pose->mutable_orientation()->set_w(0);
   pose->mutable_orientation()->set_x(0.7071067811865476);
   pose->mutable_orientation()->set_y(0.7071067811865476);
@@ -30,7 +36,7 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) {
   pose->mutable_position()->set_y(0.4);
   pose->mutable_position()->set_z(0.4);
 
-  plan::Trajectory_Pose* secondPose = trajectory.add_pose();
+  plan::Trajectory_Pose *secondPose = trajectory.add_pose();
   secondPose->CopyFrom(*pose);
   secondPose->mutable_position()->set_x(-0.4);
 
@@ -42,12 +48,6 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) {
 
 void testSpeedFactorChange(const ros::NodeHandle &n) {
 
-  std::string planning_mode;
-  if (!n.getParam("robot_planning_mode", planning_mode)) {
-    ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_planning_mode set. Aborting planning mode change test.");
-    return;
-  }
-
   config::RobotConfig rc;
 
   double motionSpeedFactor;
@@ -80,7 +80,20 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
       if (msg->get()->get_topic() == "test_speed_change") {
         testSpeedFactorChange(n);
       } else if (msg->get()->get_topic() == "test_trajectory_update") {
-        testTrajectoryUpdate(n);
+        std::istringstream buffer(msg->get()->get_payload_str());
+        std::vector<std::string> arguments((std::istream_iterator<std::string>(buffer)),std::istream_iterator<std::string>());
+
+        bool loop{false};
+        plan::Trajectory_PlanningMode mode{plan::Trajectory_PlanningMode::Trajectory_PlanningMode_FLUID};
+        for (auto arg : arguments) {
+          for (auto & c: arg) c = toupper(c);
+          if (arg == "LOOP") {
+            loop = true;
+          } else if (arg == "CARTESIAN") {
+            mode = plan::Trajectory_PlanningMode::Trajectory_PlanningMode_CARTESIAN;
+          }
+        }
+        testTrajectoryUpdate(loop, mode);
       }
     }