upgrade to protobuff 3

proto/dataconfig.proto

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

proto/linkstate.proto

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

proto/robotconfig.proto

-syntax = "proto2";
+syntax = "proto3";
 
 package config;
 
 message RobotConfig {
 
-  required double speed = 1;
+  double speed = 1;
 }
\ No newline at end of file

src/MqttToRosNode.cpp

@@ -46,14 +46,14 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n)
                                         << " -- enablePosition: " << dataConfig.enableposition()
                                         << " -- enableOrientation: " << dataConfig.enableorientation()
                                         << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
-                                        << " -- enableTwistAngular: " << dataConfig.has_enabletwistangular()
+                                        << " -- enableTwistAngular: " << dataConfig.enabletwistangular()
                                         << std::endl;
 
     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.has_enabletwistangular());
+    n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
 }
 
 void receiveMqttMessages(ros::NodeHandle n)

src/TimedCartesianPlanner.cpp

@@ -16,6 +16,25 @@ const double default_velocity = 1.0;
 
 bool isInitialized = false;
 
+// adjust velocities, accelerations (= sqrt of velocity) and time_from_start
+void manipulateVelocity(double velocity,  moveit_msgs::RobotTrajectory &trajectory_msg) {
+
+    for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
+        double new_tfs = 1 / velocity;
+        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
+        for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
+            if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
+                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
+            }
+        }
+
+        for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
+            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
+                    sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
+        }
+    }
+}
+
 void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
     geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
     geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
@@ -84,7 +103,6 @@ void computeTrajectory(moveit::planning_interface::MoveGroupInterface &group, in
     bool success = iptp.computeTimeStamps(rt);
     ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
     rt.getRobotTrajectoryMsg(trajectory_msg);
-    plan.trajectory_ = trajectory_msg;
     ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)", fraction * 100.0);
 }
 
@@ -111,6 +129,7 @@ int main(int argc, char **argv) {
     text_pose.translation().z() = 1.75;
     visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
     visual_tools.trigger();
+    //planning_scene::PlanningScene;
 
     // Initialize start state of robot and target trajectory.
     moveit::planning_interface::MoveGroupInterface group("panda_arm");
@@ -131,24 +150,10 @@ int main(int argc, char **argv) {
         moveit::planning_interface::MoveGroupInterface::Plan plan;
         moveit_msgs::RobotTrajectory trajectory_msg;
         computeTrajectory(group, i, plan, trajectory_msg);
-
-        // adjust velocities, accelerations (= sqrt of velocity) and time_from_start
-        for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
-            double new_tfs = 1 / velocity;
-            trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
-            for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
-                if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
-                    trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
-                }
-            }
-
-            for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
-                trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
-                        sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
-            }
-        }
+        manipulateVelocity(velocity, trajectory_msg);
 
         // execute one step of the trajectory
+        plan.trajectory_ = trajectory_msg;
         group.execute(plan);
 
         // make sure the robot moves in an infinite circle