updated formate of robotconfig, ability to configure initial waypoints (on the fly tbd)

CMakeLists.txt

@@ -75,6 +75,7 @@ add_library(TrajectoryUtil src/util/TrajectoryUtil.cpp src/util/TrajectoryUtil.h
 add_library(linkstate src/messages/linkstate.pb.cc src/messages/linkstate.pb.h)
 add_library(dataconfig src/messages/dataconfig.pb.cc src/messages/dataconfig.pb.h)
 add_library(robotconfig src/messages/robotconfig.pb.cc src/messages/robotconfig.pb.h)
+add_library(trajectory src/messages/trajectory.pb.cc src/messages/trajectory.pb.h)
 
 # Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
@@ -113,7 +114,8 @@ target_link_libraries(MqttToRosNode LINK_PUBLIC
         ${PahoMqttCpp}
         ${Protobuf_LIBRARIES}
         dataconfig
-        robotconfig)
+        robotconfig
+        trajectory)
 target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
         ${catkin_LIBRARIES}
         ${PahoMqtt3c}
@@ -122,5 +124,7 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
         ${PahoMqtt3as}
         ${PahoMqttCpp}
         ${Protobuf_LIBRARIES}
-        dataconfig)
+        dataconfig
+        robotconfig
+        trajectory)
 target_link_libraries(TimedCartesianPlanner ${catkin_LIBRARIES} TrajectoryUtil)
\ No newline at end of file

proto/trajectory.proto

+syntax = "proto3";
+
+package plan;
+
+message Trajectory {
+
+  message Position {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+  }
+
+  repeated Position pos = 1;
+}
\ No newline at end of file

src/MqttRosConnectionTestNode.cpp

@@ -8,6 +8,7 @@
 #include "messages/robotconfig.pb.h"
 #include "messages/dataconfig.pb.h"
 #include "mem_persistence.cpp"
+#include "messages/trajectory.pb.h"
 
 using namespace std;
 using namespace std::chrono;
@@ -70,7 +71,7 @@ void testConfig(ros::NodeHandle n) {
     std::cout << ">>>>>>>>>>>> STARTING TEST <<<<<<<<<<<<<" << std::endl;
     setupMqtt();
 
-    config::DataConfig dc;
+    /*config::DataConfig dc;
 
     dc.set_enableposition(true);
     dc.set_enableorientation(true);
@@ -83,8 +84,49 @@ void testConfig(ros::NodeHandle n) {
 
     auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
     pubmsg->set_qos(QOS);
+    client.publish(pubmsg);*/
+
+    setupMqtt();
+
+    config::RobotConfig rc;
+
+    rc.set_speed(1.0);
+    rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
+
+    std::string mqtt_msg;
+    rc.SerializeToString(&mqtt_msg);
+
+    auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
+    pubmsg->set_qos(QOS);
+    client.publish(pubmsg);
+
+    /*plan::Trajectory traj;
+
+    plan::Trajectory_Position pos;
+    pos.set_x(0.6);
+    pos.set_y(0.0);
+    pos.set_z(0.6);
+
+    traj.add_pos()->CopyFrom(pos);
+
+    std::string mqtt_msg;
+    traj.SerializeToString(&mqtt_msg);
+
+    auto pubmsg = mqtt::make_message("trajectoryconfig", 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)

src/MqttToRosNode.cpp

@@ -9,6 +9,7 @@
 
 #include "messages/robotconfig.pb.h"
 #include "messages/dataconfig.pb.h"
+#include "messages/trajectory.pb.h"
 
 using namespace std;
 using namespace std::chrono;
@@ -16,7 +17,6 @@ using namespace std::chrono;
 const string SERVER_ADDRESS	{ "tcp://localhost:1883" };
 const string CLIENT_ID		{ "ros_mqtt_consumer" };
 
-
 // Simple function to manually reconnect a client.
 bool try_reconnect(mqtt::client& cli)
 {
@@ -39,8 +39,52 @@ void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n)
     std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
     n.setParam("robot_speed_factor", robotConfig.speed());
 
-    std::cout << "Retrieved new planning-mode: " << robotConfig.planningmode() << std::endl;
-    n.setParam("robot_planning_mode", robotConfig.planningmode());
+    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;
+            break;
+        case config::RobotConfig_PlanningMode_CARTESIAN:
+            n.setParam("robot_planning_mode", "cartesian_path");
+            std::cout << "Planning-mode: cartesian" << std::endl;
+            break;
+    }
+}
+
+void handleNewTrajectory(plan::Trajectory trajectory, ros::NodeHandle n)
+{
+    ROS_INFO("Received new trajectory");
+
+    /*XmlRpc::XmlRpcValue::ValueArray x_rpc_array;
+    XmlRpc::XmlRpcValue::ValueArray y_rpc_array;
+    XmlRpc::XmlRpcValue::ValueArray z_rpc_array;
+
+     // build  xml-rpc-lists for the parameter server
+    for(int i = 0; i < trajectory.pos().size(); i++)
+    {
+        x_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).x()));
+        y_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).y()));
+        z_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).z()));
+    }*/
+
+    std::vector<double> x_values;
+    std::vector<double> y_values;
+    std::vector<double> z_values;
+
+    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());
+    }
+
+    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)
@@ -59,6 +103,33 @@ 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();
+        config::RobotConfig robotConfig;
+        robotConfig.ParseFromString(rc_payload);
+        handleRobotConfig(robotConfig, n);
+    }
+
+    if (msg->get_topic() == "dataconfig")
+    {
+        const 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();
+        plan::Trajectory trajectoryConfig;
+        trajectoryConfig.ParseFromString(tc_payload);
+        handleNewTrajectory(trajectoryConfig, n);
+    }
+}
+
 void receiveMqttMessages(ros::NodeHandle n)
 {
     mqtt::connect_options connOpts;
@@ -67,8 +138,8 @@ void receiveMqttMessages(ros::NodeHandle n)
     connOpts.set_keep_alive_interval(20);
     connOpts.set_clean_session(true);
 
-    const vector<string> TOPICS { "robotconfig", "dataconfig" };
-    const vector<int> QOS { 0, 1 };
+    const vector<string> TOPICS { "robotconfig", "dataconfig", "trajectoryconfig" };
+    const vector<int> QOS { 1, 1, 1 };
 
     try {
         ROS_INFO_STREAM("Connecting to the MQTT server.");
@@ -106,22 +177,7 @@ void receiveMqttMessages(ros::NodeHandle n)
                 break;
             }
 
-            if (msg->get_topic() == "robotconfig")
-            {
-                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 std::string dc_payload = msg->get_payload_str();
-                config::DataConfig dataConfig;
-                dataConfig.ParseFromString(dc_payload);
-                handleDataConfig(dataConfig, n);
-            }
-
+            handleMessage(n, msg);
             //ROS_INFO_STREAM("NEW MESSAGE: " << msg->get_topic() << ": " << msg->to_string());
         }
     }
@@ -130,7 +186,6 @@ void receiveMqttMessages(ros::NodeHandle n)
         cli.disconnect();
         return;
     }
-
     cli.disconnect();
 }
 

src/TimedCartesianPlanner.cpp

@@ -21,6 +21,12 @@ const std::string default_planning_mode = TrajectoryUtil::CARTESIAN_PATH;
 bool isInitialized = false;
 
 void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
+
+    geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
+
+    if(!traj_util.initWaypoints(raw_trajectory, node_handle, current_pose))
+    {
+        // choose a default trajectory
         geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
         geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
         geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
@@ -46,6 +52,7 @@ void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::
         target_pose_4.position.x = group->getCurrentPose().pose.position.x;
         raw_trajectory.push_back(target_pose_4);
     }
+}
 
 void moveRobotToInitialState(ros::NodeHandle node_handle) {
 

src/messages/trajectory.pb.h

+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: trajectory.proto
+
+#ifndef PROTOBUF_trajectory_2eproto__INCLUDED
+#define PROTOBUF_trajectory_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3000000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3000000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>
+#include <google/protobuf/extension_set.h>
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace plan {
+
+// Internal implementation detail -- do not call these.
+void protobuf_AddDesc_trajectory_2eproto();
+void protobuf_AssignDesc_trajectory_2eproto();
+void protobuf_ShutdownFile_trajectory_2eproto();
+
+class Trajectory;
+class Trajectory_Position;
+
+// ===================================================================
+
+class Trajectory_Position : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:plan.Trajectory.Position) */ {
+ public:
+  Trajectory_Position();
+  virtual ~Trajectory_Position();
+
+  Trajectory_Position(const Trajectory_Position& from);
+
+  inline Trajectory_Position& operator=(const Trajectory_Position& from) {
+    CopyFrom(from);
+    return *this;
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Trajectory_Position& default_instance();
+
+  void Swap(Trajectory_Position* other);
+
+  // implements Message ----------------------------------------------
+
+  inline Trajectory_Position* New() const { return New(NULL); }
+
+  Trajectory_Position* New(::google::protobuf::Arena* arena) const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const Trajectory_Position& from);
+  void MergeFrom(const Trajectory_Position& from);
+  void Clear();
+  bool IsInitialized() const;
+
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const {
+    return InternalSerializeWithCachedSizesToArray(false, output);
+  }
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  void InternalSwap(Trajectory_Position* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return _internal_metadata_.arena();
+  }
+  inline void* MaybeArenaPtr() const {
+    return _internal_metadata_.raw_arena_ptr();
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // optional double x = 1;
+  void clear_x();
+  static const int kXFieldNumber = 1;
+  double x() const;
+  void set_x(double value);
+
+  // optional double y = 2;
+  void clear_y();
+  static const int kYFieldNumber = 2;
+  double y() const;
+  void set_y(double value);
+
+  // optional double z = 3;
+  void clear_z();
+  static const int kZFieldNumber = 3;
+  double z() const;
+  void set_z(double value);
+
+  // @@protoc_insertion_point(class_scope:plan.Trajectory.Position)
+ private:
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  bool _is_default_instance_;
+  double x_;
+  double y_;
+  double z_;
+  mutable int _cached_size_;
+  friend void  protobuf_AddDesc_trajectory_2eproto();
+  friend void protobuf_AssignDesc_trajectory_2eproto();
+  friend void protobuf_ShutdownFile_trajectory_2eproto();
+
+  void InitAsDefaultInstance();
+  static Trajectory_Position* default_instance_;
+};
+// -------------------------------------------------------------------
+
+class Trajectory : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:plan.Trajectory) */ {
+ public:
+  Trajectory();
+  virtual ~Trajectory();
+
+  Trajectory(const Trajectory& from);
+
+  inline Trajectory& operator=(const Trajectory& from) {
+    CopyFrom(from);
+    return *this;
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Trajectory& default_instance();
+
+  void Swap(Trajectory* other);
+
+  // implements Message ----------------------------------------------
+
+  inline Trajectory* New() const { return New(NULL); }
+
+  Trajectory* New(::google::protobuf::Arena* arena) const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const Trajectory& from);
+  void MergeFrom(const Trajectory& from);
+  void Clear();
+  bool IsInitialized() const;
+
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const {
+    return InternalSerializeWithCachedSizesToArray(false, output);
+  }
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  void InternalSwap(Trajectory* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return _internal_metadata_.arena();
+  }
+  inline void* MaybeArenaPtr() const {
+    return _internal_metadata_.raw_arena_ptr();
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const;
+
+  // nested types ----------------------------------------------------
+
+  typedef Trajectory_Position Position;
+
+  // accessors -------------------------------------------------------
+
+  // repeated .plan.Trajectory.Position pos = 1;
+  int pos_size() const;
+  void clear_pos();
+  static const int kPosFieldNumber = 1;
+  const ::plan::Trajectory_Position& pos(int index) const;
+  ::plan::Trajectory_Position* mutable_pos(int index);
+  ::plan::Trajectory_Position* add_pos();
+  ::google::protobuf::RepeatedPtrField< ::plan::Trajectory_Position >*
+      mutable_pos();
+  const ::google::protobuf::RepeatedPtrField< ::plan::Trajectory_Position >&
+      pos() const;
+
+  // @@protoc_insertion_point(class_scope:plan.Trajectory)
+ private:
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  bool _is_default_instance_;
+  ::google::protobuf::RepeatedPtrField< ::plan::Trajectory_Position > pos_;
+  mutable int _cached_size_;
+  friend void  protobuf_AddDesc_trajectory_2eproto();
+  friend void protobuf_AssignDesc_trajectory_2eproto();
+  friend void protobuf_ShutdownFile_trajectory_2eproto();
+
+  void InitAsDefaultInstance();
+  static Trajectory* default_instance_;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#if !PROTOBUF_INLINE_NOT_IN_HEADERS
+// Trajectory_Position
+
+// optional double x = 1;
+inline void Trajectory_Position::clear_x() {
+  x_ = 0;
+}
+inline double Trajectory_Position::x() const {
+  // @@protoc_insertion_point(field_get:plan.Trajectory.Position.x)
+  return x_;
+}
+inline void Trajectory_Position::set_x(double value) {
+  
+  x_ = value;
+  // @@protoc_insertion_point(field_set:plan.Trajectory.Position.x)
+}
+
+// optional double y = 2;
+inline void Trajectory_Position::clear_y() {
+  y_ = 0;
+}
+inline double Trajectory_Position::y() const {
+  // @@protoc_insertion_point(field_get:plan.Trajectory.Position.y)
+  return y_;
+}
+inline void Trajectory_Position::set_y(double value) {
+  
+  y_ = value;
+  // @@protoc_insertion_point(field_set:plan.Trajectory.Position.y)
+}
+
+// optional double z = 3;
+inline void Trajectory_Position::clear_z() {
+  z_ = 0;
+}
+inline double Trajectory_Position::z() const {
+  // @@protoc_insertion_point(field_get:plan.Trajectory.Position.z)
+  return z_;
+}
+inline void Trajectory_Position::set_z(double value) {
+  
+  z_ = value;
+  // @@protoc_insertion_point(field_set:plan.Trajectory.Position.z)
+}
+
+// -------------------------------------------------------------------
+
+// Trajectory
+
+// repeated .plan.Trajectory.Position pos = 1;
+inline int Trajectory::pos_size() const {
+  return pos_.size();
+}
+inline void Trajectory::clear_pos() {
+  pos_.Clear();
+}
+inline const ::plan::Trajectory_Position& Trajectory::pos(int index) const {
+  // @@protoc_insertion_point(field_get:plan.Trajectory.pos)
+  return pos_.Get(index);
+}
+inline ::plan::Trajectory_Position* Trajectory::mutable_pos(int index) {
+  // @@protoc_insertion_point(field_mutable:plan.Trajectory.pos)
+  return pos_.Mutable(index);
+}
+inline ::plan::Trajectory_Position* Trajectory::add_pos() {
+  // @@protoc_insertion_point(field_add:plan.Trajectory.pos)
+  return pos_.Add();
+}
+inline ::google::protobuf::RepeatedPtrField< ::plan::Trajectory_Position >*
+Trajectory::mutable_pos() {
+  // @@protoc_insertion_point(field_mutable_list:plan.Trajectory.pos)
+  return &pos_;
+}
+inline const ::google::protobuf::RepeatedPtrField< ::plan::Trajectory_Position >&
+Trajectory::pos() const {
+  // @@protoc_insertion_point(field_list:plan.Trajectory.pos)
+  return pos_;
+}
+
+#endif  // !PROTOBUF_INLINE_NOT_IN_HEADERS
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace plan
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_trajectory_2eproto__INCLUDED

src/robot_state_initializer.cpp

@@ -2,10 +2,17 @@
 #include <ros/ros.h>
 #include <std_msgs/Float64MultiArray.h>
 
+void initSystemVariables(ros::NodeHandle node_handle)
+{
+    node_handle.setParam("new_trajectory_available", false);
+}
+
 int main(int argc, char **argv) {
   ros::init(argc, argv, "robot_state_initializer_node");
   ros::NodeHandle node_handle;
 
+  initSystemVariables(node_handle);
+
   std::vector<double> panda_ready_state{0, -0.785, 0, -2.356, 0, 1.571, 0.785};
 
   // define variables

src/util/TrajectoryUtil.cpp

@@ -42,6 +42,33 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
     return false;
 }
 
+bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose)
+{
+    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);
+        }
+
+        node_handle.setParam("new_trajectory_available", false);
+        return true;
+    }
+}
+
 bool TrajectoryUtil::computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
                                           moveit::planning_interface::MoveGroupInterface::Plan &plan,
                                           const geometry_msgs::Pose &targetPose) {

src/util/TrajectoryUtil.h

@@ -21,13 +21,15 @@ 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";
+    //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);
 
+    bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose);
+
 private:
 
     bool computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,