new protobuf definitions

CMakeLists.txt

@@ -26,6 +26,8 @@ find_package(catkin REQUIRED COMPONENTS
         controller_interface
         hardware_interface
         simulation_util
+        message_runtime
+        geometry_msgs
         )
 
 # System dependencies are found with CMake's conventions
@@ -34,10 +36,7 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
 find_package(Protobuf 3 REQUIRED)
 include_directories(${Protobuf_INCLUDE_DIRS})
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto proto/linkstate.proto proto/robotconfig.proto proto/trajectory.proto)
+protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robot_state.proto proto/config.proto proto/trajectory.proto)
 
 SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
 
@@ -52,22 +51,6 @@ set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
 
 add_subdirectory(lib/paho.mqtt.cpp)
 
-# ################################################################################################################################
-# catkin specific configuration ##
-# ################################################################################################################################
-# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
-catkin_package(CATKIN_DEPENDS
-        moveit_core
-        moveit_visual_tools
-        moveit_ros_planning_interface
-        controller_interface
-        hardware_interface
-        pluginlib
-        simulation_util
-        DEPENDS
-        # system_lib
-        )
-
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
@@ -93,11 +76,11 @@ catkin_package(CATKIN_DEPENDS
 ##   * 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
-# )
+add_message_files(
+        FILES
+        Trajectory.msg
+        Waypoint.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -114,10 +97,28 @@ catkin_package(CATKIN_DEPENDS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
+generate_messages(
+        DEPENDENCIES
+        geometry_msgs
         #   std_msgs  # Or other packages containing msgs
-# )
+)
+
+# ################################################################################################################################
+# catkin specific configuration ##
+# ################################################################################################################################
+# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
+catkin_package(CATKIN_DEPENDS
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        pluginlib
+        simulation_util
+        geometry_msgs
+        DEPENDS
+        # system_lib
+        )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

msg/Trajectory.msg

+# a (potentially looping) trajectory of Waypoints (a pose and the information of which planner to use to get to it)
+Waypoint[] waypoints          # the waypoints thta make up the trajectory
+bool loop                     # determines if the trajectory should be run in a loop
\ No newline at end of file

msg/Waypoint.msg

+# a waypoint in a high-level planning trajectory
+string mode                 # mode in which the point should be moved to
+geometry_msgs/Pose pose     # desired pose in world frame
\ No newline at end of file

package.xml

@@ -43,6 +43,9 @@
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
   <depend>simulation_util</depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
 
   <exec_depend>pluginlib</exec_depend>
 

proto/robotconfig.proto → proto/config.proto

@@ -3,14 +3,10 @@ syntax = "proto3";
 package config;
 
 message RobotConfig {
-
   double speed = 1;
-  bool loopTrajectory = 2;
-
-  enum PlanningMode {
-    FLUID = 0;
-    CARTESIAN = 1;
 }
 
-  PlanningMode planningMode = 3;
+message DataConfig {
+  repeated string links = 1;
+  double publishRate = 2;
 }
\ No newline at end of file

proto/dataconfig.proto

-syntax = "proto3";
-
-package config;
-
-message DataConfig {
-
-  bool enablePosition = 1;
-  bool enableOrientation = 2;
-  bool enableTwistLinear = 3;
-  bool enableTwistAngular = 4;
-
-  int32 publishRate = 5;
-}
\ No newline at end of file

proto/linkstate.proto

-syntax = "proto3";
-
-package panda;
-
-message PandaLinkState {
-
-  string name = 1;
-
-  message Position {
-    float positionX = 1;
-    float positionY = 2;
-    float positionZ = 3;
-  }
-
-  message Orientation {
-    float orientationX = 1;
-    float orientationY = 2;
-    float orientationZ = 3;
-    float orientationW = 4;
-  }
-
-  message TwistLinear {
-    float twistLinearX = 1;
-    float twistLinearY = 2;
-    float twistLinearZ = 3;
-  }
-
-  message TwistAngular {
-    float twistAngularX = 1;
-    float twistAngularY = 2;
-    float twistAngularZ = 3;
-  }
-
-  Position pos = 2;
-  Orientation orient = 3;
-  TwistLinear tl = 4;
-  TwistAngular ta = 5;
-}
\ No newline at end of file

proto/robot_state.proto

+syntax = "proto3";
+
+package robot;
+
+message RobotState {
+
+  message Position {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+  }
+
+  message Orientation {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+    double w = 4;
+  }
+
+  message LinearTwist {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+  }
+
+  message AngularTwist {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+  }
+
+  string name = 1;
+  Position position = 2;
+  Orientation orientation = 3;
+  LinearTwist linear_twist = 4;
+  AngularTwist angular_twist = 5;
+}
\ No newline at end of file

proto/trajectory.proto

@@ -10,5 +10,24 @@ message Trajectory {
     double z = 3;
   }
 
-  repeated Position pos = 1;
+  message Orientation {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+    double w = 4;
+  }
+
+  enum PlanningMode {
+    FLUID = 0;
+    CARTESIAN = 1;
+  }
+
+  message Pose {
+    Position position = 1;
+    Orientation orientation = 2;
+    PlanningMode mode = 3;
+  }
+
+  repeated Pose pose = 1;
+  bool loop = 2;
 }
\ No newline at end of file

src/MqttRosConnectionTestNode.cpp

 //
 // Created by sebastian on 31.03.20.
 //
-#include "dataconfig.pb.h"
-#include "robotconfig.pb.h"
+#include "config.pb.h"
 #include "trajectory.pb.h"
 
 #include "ros/ros.h"
-#include <gazebo_msgs/LinkStates.h>
 
 #include "util/MqttUtil.h"
 
@@ -16,81 +14,30 @@ MqttUtil *mqttUtil = nullptr;
 
 void testTrajectoryUpdate(const ros::NodeHandle& n) {
 
-  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<");
+  ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner.");
 
-  plan::Trajectory traj;
+  plan::Trajectory trajectory{};
+  trajectory.set_loop(true);
 
-  plan::Trajectory_Position pos1;
-  pos1.set_x(0.6);
-  pos1.set_y(0.0);
-  pos1.set_z(0.6);
+  plan::Trajectory_Pose* pose = trajectory.add_pose();
+  pose->set_mode(plan::Trajectory_PlanningMode_FLUID);
+  pose->mutable_orientation()->set_w(0);
+  pose->mutable_orientation()->set_x(0.7071067811865476);
+  pose->mutable_orientation()->set_y(0.7071067811865476);
+  pose->mutable_orientation()->set_z(0);
 
-  traj.add_pos()->CopyFrom(pos1);
+  pose->mutable_position()->set_x(0.6);
+  pose->mutable_position()->set_y(0.4);
+  pose->mutable_position()->set_z(0.6);
 
-  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;
-  traj.SerializeToString(&mqtt_msg);
+  plan::Trajectory_Pose* secondPose = trajectory.add_pose();
+  secondPose->CopyFrom(*pose);
+  secondPose->mutable_position()->set_x(-0.6);
 
+  std::string mqtt_msg{};
+  trajectory.SerializeToString(&mqtt_msg);
   auto pubmsg = mqtt::make_message("trajectory", mqtt_msg);
   mqttUtil->getClient().publish(pubmsg);
-
-
-  std::vector<double> x_pos;
-  std::vector<double> y_pos;
-  std::vector<double> z_pos;
-
-  ros::Duration(5.0).sleep();
-
-  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("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(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;
-  }
-
-  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(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) {
@@ -103,12 +50,6 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
 
   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.");
@@ -116,17 +57,13 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
   }
 
   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);
 
@@ -134,16 +71,10 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
   mqttUtil->getClient().publish(pubmsg);
 }
 
-void testConfig(const ros::NodeHandle &n) {
-
+void testDataConfig(const ros::NodeHandle &n) {
   ROS_INFO_STREAM("DATA CONFIG TEST: publishing all data at a rate of 1000");
 
   config::DataConfig dc;
-
-  dc.set_enableposition(true);
-  dc.set_enableorientation(true);
-  dc.set_enabletwistangular(true);
-  dc.set_enabletwistlinear(true);
   dc.set_publishrate(1000);
 
   std::string mqtt_msg;
@@ -160,9 +91,7 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
     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);
+        testDataConfig(n);
       } else if (msg->get()->get_topic() == "test_speed_change") {
         testSpeedFactorChange(n);
       } else if (msg->get()->get_topic() == "test_trajectory_update") {
@@ -171,7 +100,7 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
     }
 
   } else {
-    ROS_ERROR_STREAM_NAMED("MqttRosConnectionTestNode", "Not connected! Unable to listen to messages.");
+    ROS_ERROR_STREAM("Not connected! Unable to listen to messages.");
   }
 }
 
@@ -190,7 +119,6 @@ int main(int argc, char **argv) {
   mqttUtil = new MqttUtil(CLIENT_ID, server);
 
   mqttUtil->addTopic("test_config");
-  mqttUtil->addTopic("test_mode_change");
   mqttUtil->addTopic("test_speed_change");
   mqttUtil->addTopic("test_trajectory_update");
 

src/MqttToRosNode.cpp

 #include "mqtt/client.h"
 
 #include "ros/ros.h"
-#include "geometry_msgs/PoseArray.h"
 
 #include "util/MqttUtil.h"
 
-#include "dataconfig.pb.h"
-#include "robotconfig.pb.h"
+#include "panda_mqtt_connector/Trajectory.h"
+#include "panda_mqtt_connector/Waypoint.h"
+
+#include "config.pb.h"
 #include "trajectory.pb.h"
+#include "util/TrajectoryUtil.h"
 
 const std::string CLIENT_ID{"mqtt_to_ros"};
 
@@ -20,60 +22,52 @@ MqttUtil *mqttUtil = nullptr;
 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());
+}
 
-  ROS_INFO_STREAM("Retrieved new loop-mode: " << robotConfig.looptrajectory());
-  n.setParam("loop_trajectory", robotConfig.looptrajectory());
+void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) {
 
-  ROS_INFO_STREAM("Retrieved new planning-mode.");
-  switch (robotConfig.planningmode()) {
-    case config::RobotConfig_PlanningMode_FLUID:
-      n.setParam("robot_planning_mode", "fluid_path");
-      ROS_INFO_STREAM("Planning-mode: fluid");
-      break;
-    case config::RobotConfig_PlanningMode_CARTESIAN:
-      n.setParam("robot_planning_mode", "cartesian_path");
-      ROS_INFO_STREAM("Planning-mode: cartesian");
-      break;
-    case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MIN_SENTINEL_DO_NOT_USE_:
-      ROS_ERROR_STREAM("Planning-mode: INT_MIN_SENTINEL_DO_NOT_USE_");
+  panda_mqtt_connector::Trajectory poseArray;
+
+  for (const auto &protoPose : trajectory.pose()) {
+    panda_mqtt_connector::Waypoint waypoint;
+    waypoint.pose.orientation.w = protoPose.orientation().w();
+    waypoint.pose.orientation.x = protoPose.orientation().x();
+    waypoint.pose.orientation.y = protoPose.orientation().y();
+    waypoint.pose.orientation.z = protoPose.orientation().z();
+    waypoint.pose.position.x = protoPose.position().x();
+    waypoint.pose.position.y = protoPose.position().y();
+    waypoint.pose.position.z = protoPose.position().z();
+
+    switch (protoPose.mode()) {
+      case plan::Trajectory_PlanningMode_FLUID:
+        waypoint.mode = TrajectoryUtil::FLUID_PATH;
         break;
-    case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MAX_SENTINEL_DO_NOT_USE_:
-      ROS_ERROR_STREAM("Planning-mode: INT_MAX_SENTINEL_DO_NOT_USE_");
+      case plan::Trajectory_PlanningMode_CARTESIAN:
+        waypoint.mode = TrajectoryUtil::CARTESIAN_PATH;
         break;
-  }
+      default:
+        ROS_WARN("Received pose has invalid mode!");
     }
 
-void
-handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
-
-  geometry_msgs::PoseArray poseArray;
-
-  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);
+    poseArray.waypoints.push_back(waypoint);
   }
 
+  poseArray.loop = trajectory.loop();
+
   posePublisher.publish(poseArray);
 
 }
 
 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);
+
+  std::string links;
+  for (const auto &piece : dataConfig.links()) links = links + " " + "links";
+
+  ROS_INFO_STREAM("Retrieved new data-config. links = " << links << "; rate = " << dataConfig.publishrate());
 
   n.setParam("data_publish_rate", dataConfig.publishrate());
-  n.setParam("data_enable_position", dataConfig.enableposition());
-  n.setParam("data_enable_orientation", dataConfig.enableorientation());
-  n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
-  n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
+
+  // TODO add links to a set
 }
 
 void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
@@ -95,7 +89,7 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePub
         const std::string tc_payload = msg->get_payload_str();
         plan::Trajectory trajectoryConfig;
         trajectoryConfig.ParseFromString(tc_payload);
-        handleNewTrajectory(trajectoryConfig, n, posePublisher);
+        handleNewTrajectory(trajectoryConfig, posePublisher);
       }
     }
   } else {
@@ -107,7 +101,7 @@ 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);
+  ros::Publisher posePublisher = n.advertise<panda_mqtt_connector::Trajectory>("poses", 1000);
 
   std::string server;
   if (!n.getParam("server", server)) {

src/RobotStateProvider.cpp

@@ -9,7 +9,7 @@
 #include <gazebo_msgs/LinkStates.h>
 #include <mqtt/client.h>
 
-#include "linkstate.pb.h"
+#include "robot_state.pb.h"
 
 #include "util/MqttUtil.h"
 
@@ -35,26 +35,12 @@ namespace robot_state_provider {
  */
   void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
 
-    bool export_position = false;
-    bool export_orientation = false;
-    bool export_twist_linear = false;
-    bool export_twist_angular = false;
-
-    ros::NodeHandle n;
-
-    n.getParam("data_enable_position", export_position);
-    n.getParam("data_enable_orientation", export_orientation);
-    n.getParam("data_enable_twist_linear", export_twist_linear);
-    n.getParam("data_enable_twist_angular", export_twist_angular);
-
     ROS_INFO_STREAM("<<< LINK NAMES >>>");
 
     for (int i = 0; i < msg.name.size(); i++) {
       ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
     }
 
-    if (export_position) {
-
     ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
 
     for (int i = 0; i < msg.pose.size(); i++) {
@@ -62,9 +48,6 @@ namespace robot_state_provider {
       ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
       ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
     }
-    }
-
-    if (export_orientation) {
 
     ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
 
@@ -74,18 +57,14 @@ namespace robot_state_provider {
       ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
       ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
     }
-    }
 
-    if (export_twist_linear) {
     ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
     for (int i = 0; i < msg.twist.size(); i++) {
       ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
       ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
       ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
     }
-    }
 
-    if (export_twist_angular) {
     ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
     for (int i = 0; i < msg.twist.size(); i++) {
       ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
@@ -93,74 +72,45 @@ namespace robot_state_provider {
       ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
     }
   }
-  }
-
-  std::vector<panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
-
-    bool export_position = false;
-    bool export_orientation = false;
-    bool export_twist_linear = false;
-    bool export_twist_angular = false;
-
-    ros::NodeHandle n("panda_mqtt_connector");
 
-    if (!n.getParam("data_enable_position", export_position)) {
-      export_position = false;
-    }
-    if (!n.getParam("data_enable_orientation", export_orientation)) {
-      export_orientation = false;
-    }
-    if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
-      export_twist_linear = false;
-    }
-    if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
-      export_twist_angular = false;
-    }
+  std::vector<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
 
-    std::vector<panda::PandaLinkState> messages{};
+    std::vector<robot::RobotState> messages{};
     std::vector<std::string> names{msg.name};
 
     for (int link_number = 0; link_number < names.size(); ++link_number) {
       std::string name = names[link_number];
       if (topics.find(name) != topics.end()) {
 
-        panda::PandaLinkState pls_msg;
+        robot::RobotState pls_msg;
 
         // set the name to the MQTT topic provided by the topics map
         pls_msg.set_name(topics[name]);
 
-        if (export_position) {
-          auto *pos = new panda::PandaLinkState_Position();
-          pos->set_positionx(msg.pose.at(link_number).position.x);
-          pos->set_positiony(msg.pose.at(link_number).position.y);
-          pos->set_positionz(msg.pose.at(link_number).position.z);
-          pls_msg.set_allocated_pos(pos);
-        }
-
-        if (export_orientation) {
-          auto *orient = new panda::PandaLinkState_Orientation();
-          orient->set_orientationw(msg.pose.at(link_number).orientation.w);
-          orient->set_orientationx(msg.pose.at(link_number).orientation.x);
-          orient->set_orientationy(msg.pose.at(link_number).orientation.y);
-          orient->set_orientationz(msg.pose.at(link_number).orientation.z);
-          pls_msg.set_allocated_orient(orient);
-        }
-
-        if (export_twist_linear) {
-          auto *tl = new panda::PandaLinkState_TwistLinear();
-          tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
-          tl->set_twistlineary(msg.twist.at(link_number).linear.y);
-          tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
-          pls_msg.set_allocated_tl(tl);
-        }
-
-        if (export_twist_angular) {
-          auto *ta = new panda::PandaLinkState_TwistAngular();
-          ta->set_twistangularx(msg.twist.at(link_number).angular.x);
-          ta->set_twistangulary(msg.twist.at(link_number).angular.y);
-          ta->set_twistangularz(msg.twist.at(link_number).angular.z);
-          pls_msg.set_allocated_ta(ta);
-        }
+        auto *pos = new robot::RobotState_Position();
+        pos->set_x(msg.pose.at(link_number).position.x);
+        pos->set_y(msg.pose.at(link_number).position.y);
+        pos->set_z(msg.pose.at(link_number).position.z);
+        pls_msg.set_allocated_position(pos);
+
+        auto *orient = new robot::RobotState_Orientation();
+        orient->set_w(msg.pose.at(link_number).orientation.w);
+        orient->set_x(msg.pose.at(link_number).orientation.x);
+        orient->set_y(msg.pose.at(link_number).orientation.y);
+        orient->set_z(msg.pose.at(link_number).orientation.z);
+        pls_msg.set_allocated_orientation(orient);
+
+        auto *tl = new robot::RobotState_LinearTwist();
+        tl->set_x(msg.twist.at(link_number).linear.x);
+        tl->set_y(msg.twist.at(link_number).linear.y);
+        tl->set_z(msg.twist.at(link_number).linear.z);
+        pls_msg.set_allocated_linear_twist(tl);
+
+        auto *ta = new robot::RobotState_AngularTwist();
+        ta->set_x(msg.twist.at(link_number).angular.x);
+        ta->set_y(msg.twist.at(link_number).angular.y);
+        ta->set_z(msg.twist.at(link_number).angular.z);
+        pls_msg.set_allocated_angular_twist(ta);
 
         messages.push_back(pls_msg);
       } else {
@@ -175,7 +125,7 @@ namespace robot_state_provider {
 
     if (mqttUtil->ensureConnection()) {
       try {
-        for (const panda::PandaLinkState& pls_msg : buildLinkStateMessages(msg)) {
+        for (const robot::RobotState &pls_msg : buildLinkStateMessages(msg)) {
 
           std::string mqtt_msg;
           if (!pls_msg.SerializeToString(&mqtt_msg)) {

src/TimedPlanner.cpp

@@ -6,11 +6,13 @@
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <moveit_msgs/CollisionObject.h>
 
+#include "panda_mqtt_connector/Trajectory.h"
+#include "panda_mqtt_connector/Waypoint.h"
+
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #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>
 
@@ -48,7 +50,7 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou
 
 void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
 
-  nextPoses = geometry_msgs::PoseArray{};
+  nextPoses = panda_mqtt_connector::Trajectory{};
 
   double step_size_rad = 2 * M_PI / granularity;
 
@@ -61,13 +63,13 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
     tf2::Quaternion orientation; // the RPY constructor is deprecated
     orientation.setRPY(0, M_PI, next_rad);
 
-    geometry_msgs::Pose nextPose;
-    nextPose.orientation = tf2::toMsg(orientation);
-    nextPose.position.x = origin.x + radius * sin(next_rad);
-    nextPose.position.y = origin.y + radius * cos(next_rad);
-    nextPose.position.z = origin.z;
+    panda_mqtt_connector::Waypoint nextPose{};
+    nextPose.pose.orientation = tf2::toMsg(orientation);
+    nextPose.pose.position.x = origin.x + radius * sin(next_rad);
+    nextPose.pose.position.y = origin.y + radius * cos(next_rad);
+    nextPose.pose.position.z = origin.z;
 
-    nextPoses.get().poses.push_back(nextPose);
+    nextPoses->waypoints.push_back(nextPose);
   }
 }
 
@@ -111,21 +113,16 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
 
   loadWaypoints();
 
-  for (int i = 0; i < poses.poses.size(); i++) {
-
-    node_handle.getParam("loop_trajectory", isLooping);
-
-    std::string planning_mode;
-    if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
-      planning_mode = default_planning_mode;
-    }
+  while (poses.loop) {
+    for (auto &waypoint : poses.waypoints) {
 
-    ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode);
+      ROS_INFO_STREAM("Planning the next trajectory in " << waypoint.mode);
 
       moveit::planning_interface::MoveGroupInterface::Plan plan;
 
-    if (traj_util.computePathToPose(group, plan, poses.poses.at(i), planning_mode, 0.8,
-                                    0.8)) {
+      ROS_ERROR_STREAM(waypoint);
+
+      if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
 
         if (nextPoses.is_initialized()) {
           return;
@@ -150,18 +147,12 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
       } else {
         ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
       }
-
-    // make sure the robot moves in an infinite circle
-    if (i == (poses.poses.size() - 1)) {
-      if (isLooping) {
-        i = -1;
-      }
     }
   }
 }
 
-void TimedPlanner::newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg) {
-  ROS_INFO_STREAM("Received new pose list with " << msg->poses.size() << " poses.");
+void TimedPlanner::newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
+  ROS_INFO_STREAM("Received new pose list with " << msg->waypoints.size() << " poses.");
   nextPoses = *msg;
 }
 
@@ -171,8 +162,6 @@ TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group
 
   ros::NodeHandle node_handle("panda_mqtt_connector");
 
-  node_handle.getParam("loop_trajectory", isLooping);
-
   std::string defaultTrajectory = "<none>";
   node_handle.getParam("default_trajectory", defaultTrajectory);
 
@@ -200,22 +189,24 @@ void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) {
   base_pose.position.x = 0.4;
   base_pose.position.y = 0.4;
   base_pose.position.z = 0.3;
-  geometry_msgs::Pose target_pose_1 = base_pose;
-  geometry_msgs::Pose target_pose_2 = base_pose;
-  geometry_msgs::Pose target_pose_3 = base_pose;
-  geometry_msgs::Pose target_pose_4 = base_pose;
+  panda_mqtt_connector::Waypoint target_pose_1;
+  target_pose_1.pose = base_pose;
+  target_pose_1.mode = TrajectoryUtil::FLUID_PATH;
+  panda_mqtt_connector::Waypoint target_pose_2{target_pose_1};
+  panda_mqtt_connector::Waypoint target_pose_3{target_pose_1};
+  panda_mqtt_connector::Waypoint target_pose_4{target_pose_1};
 
-  poses.poses.push_back(target_pose_1);
+  poses.waypoints.push_back(target_pose_1);
 
-  target_pose_2.position.y *= -1;
-  poses.poses.push_back(target_pose_2);
+  target_pose_2.pose.position.y *= -1;
+  poses.waypoints.push_back(target_pose_2);
 
-  target_pose_3.position.x *= -1;
-  target_pose_3.position.y *= -1;
-  poses.poses.push_back(target_pose_3);
+  target_pose_3.pose.position.x *= -1;
+  target_pose_3.pose.position.y *= -1;
+  poses.waypoints.push_back(target_pose_3);
 
-  target_pose_4.position.x *= -1;
-  poses.poses.push_back(target_pose_4);
+  target_pose_4.pose.position.x *= -1;
+  poses.waypoints.push_back(target_pose_4);
 }
 
 

src/TimedPlanner.h

@@ -6,10 +6,11 @@
 #define SRC_TIMEDPLANNER_H
 
 
-#include <geometry_msgs/PoseArray.h>
 #include <boost/optional.hpp>
 #include "util/TrajectoryUtil.h"
 
+#include "panda_mqtt_connector/Trajectory.h"
+
 class TimedPlanner {
 
 public:
@@ -24,13 +25,11 @@ public:
   TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity,
                std::string defaultPlanningMode);
 
-  void newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg);
+  void newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
 
 private:
-  geometry_msgs::PoseArray poses;
-  boost::optional<geometry_msgs::PoseArray> nextPoses;
-  TrajectoryUtil traj_util;
-  bool isLooping = true;
+  panda_mqtt_connector::Trajectory poses;
+  boost::optional<panda_mqtt_connector::Trajectory> nextPoses;
 
   void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
 

src/util/TrajectoryUtil.cpp

@@ -4,9 +4,6 @@
 
 #include "TrajectoryUtil.h"
 
-const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
-const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
-
 bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                                        moveit::planning_interface::MoveGroupInterface::Plan &plan,
                                        geometry_msgs::Pose targetPose,

src/util/TrajectoryUtil.h

@@ -18,17 +18,16 @@ class TrajectoryUtil {
 
 public:
 
-  static const std::string CARTESIAN_PATH;
-  static const std::string FLUID_PATH;
+  constexpr static const char* const CARTESIAN_PATH = "cartesian_path";
+  constexpr static const char* const FLUID_PATH = "fluid_path";
 
-  bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
+  static bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                          moveit::planning_interface::MoveGroupInterface::Plan &plan,
                          geometry_msgs::Pose targetPose,
                          const std::string &pathType, double maxVelocityFactor,
                          double maxAccelerationFactor);
 
   static void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity);
-private:
 
 };