From e23f357d33a0da99fa6026a8f25c6be78b126291 Mon Sep 17 00:00:00 2001
From: Mikhail Khrenov <mkhrenov34@gmail.com>
Date: Fri, 5 Jun 2020 15:15:22 -0400
Subject: [PATCH] Add velocity scaling and collision feedback

---
 CMakeLists.txt   |  2 +-
 package.xml      |  4 ----
 rvc_ros_node.cpp | 18 ++++++++++++++++--
 3 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 694fe95..7cc3ff3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ project(rvc_ros)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS serial roscpp sensor_msgs std_srvs message_generation)
+find_package(catkin REQUIRED COMPONENTS serial roscpp sensor_msgs std_srvs std_msgs)
 find_package(fmt)
 
 ## System dependencies are found with CMake's conventions
diff --git a/package.xml b/package.xml
index 8b1d500..845241b 100644
--- a/package.xml
+++ b/package.xml
@@ -57,10 +57,6 @@
   <exec_depend>std_msgs</exec_depend>
   <build_depend>std_srvs</build_depend>
   <exec_depend>std_srvs</exec_depend>
-  <build_depend>message_generation</build_depend>
-  <exec_depend>message_generation</exec_depend>
-  <build_depend>message_runtime</build_depend>
-  <exec_depend>message_runtime</exec_depend>
   <build_depend>serial</build_depend>
   <exec_depend>serial</exec_depend>
 
diff --git a/rvc_ros_node.cpp b/rvc_ros_node.cpp
index 0e451ae..c9077e7 100644
--- a/rvc_ros_node.cpp
+++ b/rvc_ros_node.cpp
@@ -1,6 +1,7 @@
 // C/C++ Headers
 #include <stdlib.h>
 #include <string>
+#include <algorithm>
 
 // Utility library headers
 #include <boost/algorithm/string.hpp>
@@ -10,6 +11,7 @@
 // ROS Headers
 #include <ros/ros.h>
 #include <serial/serial.h>
+#include <std_msgs/Float32.h>
 #include <sensor_msgs/JointState.h>
 #include <std_srvs/Empty.h>
 #include <std_srvs/SetBool.h>
@@ -24,13 +26,20 @@ ros::Publisher pub;
 
 // Relative movement subscriber callback
 void move(const sensor_msgs::JointState &msg) {
-  ser->write(fmt::format("m {} {};", msg.position[0], msg.position[1]));
+  ser->write(fmt::format("m {0:.2f} {1:.2f};", msg.position[0], msg.position[1]));
 }
 
 
 // Absolute movement subscriber callback
 void move_to(const sensor_msgs::JointState &msg) {
-  ser->write(fmt::format("M {} {};", msg.position[0], msg.position[1]));
+  ser->write(fmt::format("M {0:.2f} {1:.2f};", msg.position[0], msg.position[1]));
+}
+
+
+// Scale maximum velocity of device callback
+void velocity_scale(const std_msgs::Float32 &msg) {
+  float scale = std::min(std::max(msg.data, 0.0f), 1.0f);
+  ser->write(fmt::format("V {0:.3f};", scale));
 }
 
 
@@ -132,6 +141,10 @@ void publishPosition(const ros::TimerEvent &e) {
     msg.position[0] = std::stod(results[0]);
     msg.position[1] = std::stod(results[1]);
 
+    msg.effort.resize(2);
+    msg.effort[0] = std::stod(results[2]);
+    msg.effort[1] = std::stod(results[3]);
+    
     // Publish and increment sequence counter
     pub.publish(msg);
     s++;
@@ -178,6 +191,7 @@ int main(int argc, char **argv) {
   
   ros::Subscriber move_sub = nh.subscribe("move_jr", 1000, &move);
   ros::Subscriber move_to_sub = nh.subscribe("move_jp", 1000, &move_to);
+  ros::Subscriber velocity_scale_sub = nh.subscribe("velocity_scale", 1000, &velocity_scale);
   
   ros::ServiceServer halt_serv = nh.advertiseService("halt", &halt);
   ros::ServiceServer tap_serv = nh.advertiseService("tap", &tap);