restructure trajectory update (uses messages), some cleanup.

CMakeLists.txt

@@ -68,6 +68,78 @@ catkin_package(CATKIN_DEPENDS
         # system_lib
         )
 
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+
 # ################################################################################################################################
 # Build ##
 # ################################################################################################################################
@@ -86,7 +158,7 @@ add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h)
 
 
 add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-add_executable(TimedPlanner src/TimedPlanner.cpp)
+add_executable(TimedPlanner src/TimedPlanner.cpp src/TimedPlanner.h)
 add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)

config/config.yaml

@@ -6,6 +6,7 @@ panda_mqtt_connector:
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"
+    trajectoryConfig: "trajectoryconfig"
   zone_size: 0.5
   zones:
     - "1 1"

src/MqttRosConnectionTestNode.cpp

@@ -10,23 +10,11 @@
 
 #include "util/MqttUtil.h"
 
-#include <mqtt/client.h>
-
-using namespace std;
-
-/*
- * 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_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 CLIENT_ID{"ros_mqtt_tester"};
+const std::string CLIENT_ID{"ros_mqtt_tester"};
 
 MqttUtil *mqttUtil = nullptr;
 
-void testTrajectoryUpdate(ros::NodeHandle n) {
+void testTrajectoryUpdate(const ros::NodeHandle& n) {
 
   ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<");
 
@@ -52,7 +40,6 @@ void testTrajectoryUpdate(ros::NodeHandle n) {
   auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
   mqttUtil->getClient().publish(pubmsg);
 
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<");
 
   std::vector<double> x_pos;
   std::vector<double> y_pos;
@@ -60,25 +47,85 @@ void testTrajectoryUpdate(ros::NodeHandle n) {
 
   ros::Duration(5.0).sleep();
 
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "x configured: " << n.getParam("trajectory_pos_x", x_pos));
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "y configured: " << n.getParam("trajectory_pos_y", y_pos));
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "z configured: " << n.getParam("trajectory_pos_z", z_pos));
+  ROS_INFO_STREAM("x configured: " << n.getParam("trajectory_pos_x", x_pos));
+  ROS_INFO_STREAM("y configured: " << n.getParam("trajectory_pos_y", y_pos));
+  ROS_INFO_STREAM("z configured: " << n.getParam("trajectory_pos_z", z_pos));
 
   for (int i = 0; i < x_pos.size(); i++) {
-    ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "X POS: " << x_pos.at(i));
-    ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Y POS: " << y_pos.at(i));
-    ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Z POS: " << z_pos.at(i));
+    ROS_INFO_STREAM("X POS: " << x_pos.at(i));
+    ROS_INFO_STREAM("Y POS: " << y_pos.at(i));
+    ROS_INFO_STREAM("Z POS: " << z_pos.at(i));
   }
 }
 
-void testPlanningModeChange(ros::NodeHandle n) {
+void testPlanningModeChange(const ros::NodeHandle &n) {
 
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING PLANNING MODE CHANGE TEST <<<<<<<");
+  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;
+  }
+
+  double motionSpeedFactor;
+  if (!n.getParam("robot_speed_factor", motionSpeedFactor)) {
+    ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test.");
+    return;
+  }
 
   config::RobotConfig rc;
 
-  rc.set_speed(1.0);
+  rc.set_speed(motionSpeedFactor);
+
+  if (planning_mode == "fluid_path") {
+    rc.set_planningmode(config::RobotConfig_PlanningMode_CARTESIAN);
+    ROS_INFO_STREAM("PLANNING MODE CHANGE TEST: changed from fluid_path to cartesian_path");
+  } else {
     rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
+    ROS_INFO_STREAM("PLANNING MODE CHANGE TEST: changed from cartesian_path to fluid_path");
+  }
+
+  rc.set_looptrajectory(true);
+
+  std::string mqtt_msg;
+  rc.SerializeToString(&mqtt_msg);
+
+  auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
+  mqttUtil->getClient().publish(pubmsg);
+}
+
+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;
+
+  if (planning_mode == "cartesian_path") {
+    rc.set_planningmode(config::RobotConfig_PlanningMode_CARTESIAN);
+  } else {
+    rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
+  }
+
+  double motionSpeedFactor;
+  if (!n.getParam("robot_speed_factor", motionSpeedFactor)) {
+    ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test.");
+    return;
+  }
+
+  if (motionSpeedFactor < 0.5) {
+    rc.set_speed(motionSpeedFactor);
+    ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.9");
+    rc.set_speed(0.9);
+  } else {
+    rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
+    ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.2");
+    rc.set_speed(0.2);
+  }
+
+  rc.set_looptrajectory(true);
 
   std::string mqtt_msg;
   rc.SerializeToString(&mqtt_msg);
@@ -87,9 +134,9 @@ void testPlanningModeChange(ros::NodeHandle n) {
   mqttUtil->getClient().publish(pubmsg);
 }
 
-void testConfig(ros::NodeHandle n) {
+void testConfig(const ros::NodeHandle &n) {
 
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING CONFIG UPDATE TEST <<<<<<<");
+  ROS_INFO_STREAM("DATA CONFIG TEST: publishing all data at a rate of 1000");
 
   config::DataConfig dc;
 
@@ -106,15 +153,18 @@ void testConfig(ros::NodeHandle n) {
   mqttUtil->getClient().publish(pubmsg);
 }
 
-void receiveMqttMessages(ros::NodeHandle n) {
+void receiveMqttMessages(const ros::NodeHandle &n) {
 
   if (mqttUtil->ensureConnection()) {
-    mqtt::const_message_ptr *msg = new mqtt::const_message_ptr;
+    auto *msg = new mqtt::const_message_ptr;
     if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds(500))) {
+      ROS_INFO_STREAM("Received message on topic " << msg->get()->get_topic());
       if (msg->get()->get_topic() == "test_config") {
         testConfig(n);
       } else if (msg->get()->get_topic() == "test_mode_change") {
         testPlanningModeChange(n);
+      } else if (msg->get()->get_topic() == "test_speed_change") {
+        testSpeedFactorChange(n);
       } else if (msg->get()->get_topic() == "test_trajectory_update") {
         testTrajectoryUpdate(n);
       }
@@ -141,6 +191,7 @@ int main(int argc, char **argv) {
 
   mqttUtil->addTopic("test_config");
   mqttUtil->addTopic("test_mode_change");
+  mqttUtil->addTopic("test_speed_change");
   mqttUtil->addTopic("test_trajectory_update");
 
   mqttUtil->connect();

src/MqttToRosNode.cpp

 #include "mqtt/client.h"
-#include <chrono>
-#include <iostream>
-#include <string>
-#include <thread>
 
 #include "ros/ros.h"
+#include "geometry_msgs/PoseArray.h"
 
 #include "util/MqttUtil.h"
 
@@ -12,63 +9,59 @@
 #include "robotconfig.pb.h"
 #include "trajectory.pb.h"
 
-using namespace std;
-using namespace std::chrono;
+const std::string CLIENT_ID{"mqtt_to_ros"};
 
-const string CLIENT_ID{"mqtt_to_ros"};
+const std::string ROBOT_CONFIG{"robotconfig"};
+const std::string DATA_CONFIG{"dataconfig"};
+const std::string TRAJECTORY_CONFIG{"trajectoryconfig"};
 
 MqttUtil *mqttUtil = nullptr;
 
-void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n) {
-  std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
+void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHandle &n) {
+  ROS_INFO_STREAM("Retrieved new target-speed: " << robotConfig.speed());
   n.setParam("robot_speed_factor", robotConfig.speed());
 
-  std::cout << "Retrieved new loop-mode: " << robotConfig.looptrajectory() << std::endl;
+  ROS_INFO_STREAM("Retrieved new loop-mode: " << robotConfig.looptrajectory());
   n.setParam("loop_trajectory", robotConfig.looptrajectory());
 
   std::cout << "Retrieved new planning-mode: " << std::endl;
   switch (robotConfig.planningmode()) {
     case config::RobotConfig_PlanningMode_FLUID:
       n.setParam("robot_planning_mode", "fluid_path");
-    std::cout << "Planning-mode: fluid" << std::endl;
+      ROS_INFO_STREAM("Planning-mode: fluid");
       break;
     case config::RobotConfig_PlanningMode_CARTESIAN:
       n.setParam("robot_planning_mode", "cartesian_path");
-    std::cout << "Planning-mode: cartesian" << std::endl;
+      ROS_INFO_STREAM("Planning-mode: cartesian");
       break;
   }
 }
 
-void handleNewTrajectory(plan::Trajectory trajectory, ros::NodeHandle n) {
-  ROS_INFO("Received new trajectory");
+void
+handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
 
-  std::vector<double> x_values;
-  std::vector<double> y_values;
-  std::vector<double> z_values;
+  geometry_msgs::PoseArray poseArray;
 
-  for (int i = 0; i < trajectory.pos().size(); i++) {
-    x_values.push_back(trajectory.pos().Get(i).x());
-    y_values.push_back(trajectory.pos().Get(i).y());
-    z_values.push_back(trajectory.pos().Get(i).z());
+  for (const auto &protoPose : trajectory.pos()) {
+    geometry_msgs::Pose pose;
+    pose.orientation.w = 1;
+    pose.position.x = protoPose.x();
+    pose.position.y = protoPose.y();
+    pose.position.z = protoPose.z();
+    poseArray.poses.push_back(pose);
   }
 
-  n.deleteParam("trajectory_pos_x");
-  n.deleteParam("trajectory_pos_y");
-  n.deleteParam("trajectory_pos_z");
+  posePublisher.publish(poseArray);
 
-  n.setParam("trajectory_pos_x", x_values);
-  n.setParam("trajectory_pos_y", y_values);
-  n.setParam("trajectory_pos_z", z_values);
-
-  n.setParam("new_trajectory_available", true);
 }
 
-void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) {
-  std::cout << "Retrieved new data-config: -- publish-rate: " << dataConfig.publishrate()
-            << " -- enablePosition: " << dataConfig.enableposition()
-            << " -- enableOrientation: " << dataConfig.enableorientation()
-            << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
-            << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl;
+void handleDataConfig(const config::DataConfig &dataConfig, const ros::NodeHandle &n) {
+  ROS_INFO_STREAM("Retrieved new data-config:" << std::endl
+                  << " -- publish-rate: " << dataConfig.publishrate() << std::endl
+                  << " -- enablePosition: " << dataConfig.enableposition() << std::endl
+                  << " -- enableOrientation: " << dataConfig.enableorientation() << std::endl
+                  << " -- enableTwistLinear: " << dataConfig.enabletwistlinear() << std::endl
+                  << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl);
 
   n.setParam("data_publish_rate", dataConfig.publishrate());
   n.setParam("data_enable_position", dataConfig.enableposition());
@@ -77,37 +70,28 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) {
   n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
 }
 
-void handleMessage(const ros::NodeHandle &n, const mqtt::const_message_ptr &msg) {
-
-  if (msg->get_topic() == "robotconfig") {
-    const string rc_payload = msg->get_payload_str();
+void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
+  if (mqttUtil->ensureConnection()) {
+    mqtt::const_message_ptr msg;
+    if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
+      ROS_INFO_STREAM("NEW MESSAGE ON TOPIC " << msg.get()->get_topic());
+      if (msg->get_topic() == ROBOT_CONFIG) {
+        const std::string rc_payload = msg->get_payload_str();
         config::RobotConfig robotConfig;
         robotConfig.ParseFromString(rc_payload);
         handleRobotConfig(robotConfig, n);
-  }
-
-  if (msg->get_topic() == "dataconfig") {
-    const string dc_payload = msg->get_payload_str();
+      } else if (msg->get_topic() == DATA_CONFIG) {
+        const std::string dc_payload = msg->get_payload_str();
         config::DataConfig dataConfig;
         dataConfig.ParseFromString(dc_payload);
         handleDataConfig(dataConfig, n);
-  }
-
-  if (msg->get_topic() == "trajectoryconfig") {
-    const string tc_payload = msg->get_payload_str();
+      } else if (msg->get_topic() == TRAJECTORY_CONFIG) {
+        const std::string tc_payload = msg->get_payload_str();
         plan::Trajectory trajectoryConfig;
         trajectoryConfig.ParseFromString(tc_payload);
-    handleNewTrajectory(trajectoryConfig, n);
+        handleNewTrajectory(trajectoryConfig, n, posePublisher);
       }
     }
-
-void receiveMqttMessages(ros::NodeHandle n) {
-  if (mqttUtil->ensureConnection()) {
-    mqtt::const_message_ptr msg;
-    if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
-      ROS_INFO_STREAM("NEW MESSAGE ON TOPIC " << msg.get()->get_topic());
-      handleMessage(n, msg);
-    }
   } else {
     ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
   }
@@ -117,17 +101,27 @@ int main(int argc, char *argv[]) {
   ros::init(argc, argv, "mqtt_listener");
   ros::NodeHandle n("panda_mqtt_connector");
 
+  ros::Publisher posePublisher = n.advertise<geometry_msgs::PoseArray>("poses", 1000);
+
   std::string server;
   if (!n.getParam("server", server)) {
-    ROS_ERROR("Could not get string value for panda_mqtt_connector/server from param server");
+    ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/server from param server");
     return -1;
   }
 
   mqttUtil = new MqttUtil(CLIENT_ID, server);
 
-  mqttUtil->addTopic("robotconfig");
-  mqttUtil->addTopic("dataconfig");
-  mqttUtil->addTopic("trajectoryconfig");
+  std::string robotConfig = "robotconfig";
+
+  std::map<std::string, std::string> topics;
+  if (!n.getParam("topics", topics)) {
+    ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/topics from param server");
+  }
+
+  for (const auto &topic : topics) {
+    ROS_INFO_STREAM("Listening to MQTT topic " << topic.second);
+    mqttUtil->addTopic(topic.second);
+  }
 
   if (!mqttUtil->connect()) {
     ROS_ERROR_STREAM("Unable to connect to MQTT server. Exiting...");
@@ -135,7 +129,7 @@ int main(int argc, char *argv[]) {
   }
 
   while (ros::ok()) {
-    receiveMqttMessages(n);
+    receiveMqttMessages(n, posePublisher);
     ros::spinOnce();
   }
 

src/TimedPlanner.cpp

@@ -10,24 +10,17 @@
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 #include <tf2/transform_datatypes.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "geometry_msgs/PoseArray.h"
+
+#include <boost/optional.hpp>
 
 #include "util/TrajectoryUtil.h"
+#include "TimedPlanner.h"
 
 #include <cmath>
+#include <utility>
 
 
-namespace PlannerState {
-
-  std::vector<geometry_msgs::Pose> raw_trajectory;
-  TrajectoryUtil traj_util;
-
-  const double default_velocity = 1.0;
-  const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
-
-  bool isLooping = true;
-
-}
-
 void
 constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
 
@@ -53,8 +46,9 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou
   collision_objects.push_back(plate);
 }
 
-void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double radius, geometry_msgs::Point origin,
-                            int granularity) {
+void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
+
+  nextPoses = geometry_msgs::PoseArray{};
 
   double step_size_rad = 2 * M_PI / granularity;
 
@@ -73,39 +67,23 @@ void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double
     nextPose.position.y = origin.y + radius * cos(next_rad);
     nextPose.position.z = origin.z;
 
-    waypoints.push_back(nextPose);
+    nextPoses.get().poses.push_back(nextPose);
   }
 }
 
-bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeHandle &node_handle,
-                   geometry_msgs::Pose &initialPose) {
-
-  // regardless if the loading fails, the trajectory is no longer "new" after calling this method
-  node_handle.setParam("new_trajectory_available", false);
-
-  std::vector<double> x_values;
-  std::vector<double> y_values;
-  std::vector<double> z_values;
-
-  // check if no trajectory is configured
-  if (!node_handle.getParam("trajectory_pos_x", x_values) ||
-      !node_handle.getParam("trajectory_pos_y", y_values) ||
-      !node_handle.getParam("trajectory_pos_z", z_values)) {
-    return false;
-  } else {
-    for (int i = 0; i < x_values.size(); i++) {
-      geometry_msgs::Pose pose = initialPose;
-      pose.position.x = x_values.at(i);
-      pose.position.y = y_values.at(i);
-      pose.position.z = z_values.at(i);
-      waypoints.push_back(pose);
-    }
+bool TimedPlanner::loadWaypoints() {
 
+  if (nextPoses.is_initialized()) {
+    poses = nextPoses.get();
+    nextPoses.reset();
     return true;
+  } else {
+    return false;
   }
 }
 
-static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, const moveit::planning_interface::MoveGroupInterface::Plan &plan) {
+std::vector<moveit_msgs::RobotTrajectory> TimedPlanner::split(moveit::planning_interface::MoveGroupInterface &group,
+                                                              const moveit::planning_interface::MoveGroupInterface::Plan &plan) {
 
   std::vector<moveit_msgs::RobotTrajectory> trajectories;
 
@@ -129,44 +107,34 @@ static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interfac
 
 }
 
-void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
+void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
 
-  bool newTrajectoryAvailable = false;
-  node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
+  loadWaypoints();
 
-  if (newTrajectoryAvailable) {
-    geometry_msgs::Pose current_pose = group.getCurrentPose().pose;
-    PlannerState::raw_trajectory.clear();
+  for (int i = 0; i < poses.poses.size(); i++) {
 
-    if (!loadWaypoints(PlannerState::raw_trajectory, node_handle, current_pose)) {
-      ROS_ERROR_STREAM("Unable to load new trajectory!");
-    }
-  }
-
-  if (PlannerState::raw_trajectory.empty()) {
-    return;
-  }
-
-  for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
-
-    node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
-    node_handle.getParam("loop_trajectory", PlannerState::isLooping);
+    node_handle.getParam("loop_trajectory", isLooping);
 
     std::string planning_mode;
     if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
-      planning_mode = PlannerState::default_planning_mode;
+      planning_mode = default_planning_mode;
     }
 
     ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode);
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
 
-    if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8,
+    if (traj_util.computePathToPose(group, plan, poses.poses.at(i), planning_mode, 0.8,
                                     0.8)) {
+
+      if (nextPoses.is_initialized()) {
+        return;
+      }
+
       for (auto trajectory : split(group, plan)) {
         double motionSpeedFactor;
         if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) {
-          motionSpeedFactor = PlannerState::default_velocity;
+          motionSpeedFactor = default_velocity;
         }
         if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
           ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
@@ -184,46 +152,49 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     }
 
     // make sure the robot moves in an infinite circle
-    if (i == (PlannerState::raw_trajectory.size() - 1)) {
-      if (PlannerState::isLooping) {
+    if (i == (poses.poses.size() - 1)) {
+      if (isLooping) {
         i = -1;
       }
     }
   }
 }
 
-int main(int argc, char **argv) {
+void TimedPlanner::newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg) {
+  ROS_INFO_STREAM("Received new pose list with " << msg->poses.size() << " poses.");
+  nextPoses = *msg;
+}
 
-  // setup this ros-node
-  ros::init(argc, argv, "timed_cartesian_planner");
-  ros::NodeHandle node_handle("panda_mqtt_connector");
-  ros::AsyncSpinner spinner(1);
-  spinner.start();
+TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
+                           std::string defaultPlanningMode) : default_velocity(
+    defaultVelocity), default_planning_mode(std::move(defaultPlanningMode)) {
 
-  // wait for robot init of robot_state_initializer
-  ros::Duration(3.0).sleep();
-  ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
-
-  // Initialize start state of robot and target trajectory.
-  moveit::planning_interface::MoveGroupInterface group("panda_arm");
-  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+  ros::NodeHandle node_handle("panda_mqtt_connector");
 
-  node_handle.getParam("loop_trajectory", PlannerState::isLooping);
+  node_handle.getParam("loop_trajectory", isLooping);
 
   std::string defaultTrajectory = "<none>";
   node_handle.getParam("default_trajectory", defaultTrajectory);
 
-
   geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
 
-  if (loadWaypoints(PlannerState::raw_trajectory, node_handle, base_pose)) {
-    ROS_INFO_STREAM("Ignoring default waypoints because there is a trajectory defined by parameters");
+  if (loadWaypoints()) {
+    ROS_INFO_STREAM("Ignoring default waypoints because poses were already received as a message");
   } else if (defaultTrajectory == "circle") {
     ROS_INFO_STREAM("loading default trajectory 'circle'");
     geometry_msgs::Point origin;
     origin.z = .3;
-    loadCircularTrajectory(PlannerState::raw_trajectory, 0.6, origin, 20);
+    loadCircularTrajectory(0.6, origin, 20);
   } else if (defaultTrajectory == "square") {
+    loadSquareTrajectory(base_pose);
+
+  } else {
+    ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + 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;
@@ -234,21 +205,40 @@ int main(int argc, char **argv) {
   geometry_msgs::Pose target_pose_3 = base_pose;
   geometry_msgs::Pose target_pose_4 = base_pose;
 
-    PlannerState::raw_trajectory.push_back(target_pose_1);
+  poses.poses.push_back(target_pose_1);
 
   target_pose_2.position.y *= -1;
-    PlannerState::raw_trajectory.push_back(target_pose_2);
+  poses.poses.push_back(target_pose_2);
 
   target_pose_3.position.x *= -1;
   target_pose_3.position.y *= -1;
-    PlannerState::raw_trajectory.push_back(target_pose_3);
+  poses.poses.push_back(target_pose_3);
 
   target_pose_4.position.x *= -1;
-    PlannerState::raw_trajectory.push_back(target_pose_4);
-  } else {
-    ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
+  poses.poses.push_back(target_pose_4);
 }
 
+
+int main(int argc, char **argv) {
+
+  // setup this ros-node
+  ros::init(argc, argv, "timed_planner");
+  ros::NodeHandle node_handle("panda_mqtt_connector");
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  // wait for robot init of robot_state_initializer
+  ros::Duration(3.0).sleep();
+  ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
+
+  // Initialize start state of robot and target trajectory.
+  moveit::planning_interface::MoveGroupInterface group("panda_arm");
+  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+  TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH);
+
+  ros::Subscriber sub = node_handle.subscribe("poses", 1000, &TimedPlanner::newPoseCallback, &planner);
+
   // add the ground plate
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   constructPlate(collision_objects, 5.0, 5.0);
@@ -257,7 +247,7 @@ int main(int argc, char **argv) {
   while (ros::ok()) {
 
     // execute a trajectory
-    doMotion(node_handle, group);
+    planner.doMotion(node_handle, group);
 
     ros::spinOnce();
   }

src/TimedPlanner.h

+//
+// Created by Johannes Mey on 13.07.20.
+//
+
+#ifndef SRC_TIMEDPLANNER_H
+#define SRC_TIMEDPLANNER_H
+
+
+#include <geometry_msgs/PoseArray.h>
+#include <boost/optional.hpp>
+#include "util/TrajectoryUtil.h"
+
+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);
+
+  void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group);
+
+  TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity,
+               std::string defaultPlanningMode);
+
+  void newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg);
+
+private:
+  geometry_msgs::PoseArray poses;
+  boost::optional<geometry_msgs::PoseArray> nextPoses;
+  TrajectoryUtil traj_util;
+  bool isLooping = true;
+
+  void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
+
+  bool loadWaypoints();
+
+  void loadSquareTrajectory(geometry_msgs::Pose base_pose);
+};
+
+
+#endif //SRC_TIMEDPLANNER_H