From 816f1215c840262ef8aabe20ff4f32a130100bfd Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Wed, 9 Sep 2015 13:34:21 +0200
Subject: [PATCH] Add speed interface

---
 src/ur_ros_wrapper.cpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index b689b31..490da76 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -17,6 +17,7 @@
 #include <mutex>
 #include <condition_variable>
 #include <thread>
+#include <algorithm>
 
 #include "sensor_msgs/JointState.h"
 #include "geometry_msgs/WrenchStamped.h"
@@ -24,8 +25,6 @@
 #include "actionlib/server/simple_action_server.h"
 #include "trajectory_msgs/JointTrajectoryPoint.h"
 
-
-
 class RosWrapper {
 protected:
 	ros::NodeHandle nh_;
@@ -161,7 +160,14 @@ private:
 		traj.points = new_traj;
 	}
 
-	void speedInterface(const trajectory_msgs::JointTrajectory::ConstPtr& msg) {
+	void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) {
+		reorder_traj_joints(*msg);
+		double acc = *std::max_element(msg->points[0].accelerations.begin(),
+				msg->points[0].accelerations.end());
+		robot_.setSpeed(msg->points[0].velocities[0],
+				msg->points[0].velocities[1], msg->points[0].velocities[2],
+				msg->points[0].velocities[3], msg->points[0].velocities[4],
+				msg->points[0].velocities[5], acc);
 
 	}