Added thrust testing Arduino code and python node

scripts/thrust_test.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python2
+
+import rospy
+from std_msgs.msg import Float64
+from iarc7_msgs.msg import OrientationThrottleStamped
+from std_srvs.srv import SetBool
+
+import numpy as np
+
+last_scale_msg = None
+have_new_scale_msg = False
+def scale_callback(msg):
+    global last_scale_msg, have_new_scale_msg
+    last_scale_msg = msg
+    have_new_scale_msg = True
+
+rospy.init_node('thrust_test')
+rospy.Subscriber('/scale', Float64, scale_callback)
+pub = rospy.Publisher('/uav_direction_command', OrientationThrottleStamped, queue_size=5)
+rospy.logerr('Publisher created')
+rospy.wait_for_service('/uav_arm')
+arm_service = rospy.ServiceProxy('/uav_arm', SetBool)
+rospy.logerr('Service ready')
+assert arm_service(True)
+rospy.logerr('Motors armed')
+
+command = OrientationThrottleStamped()
+samples = np.linspace(0.0, 1.0, 100)
+forces = np.ndarray(samples.shape)
+for i, sample in enumerate(samples):
+    if rospy.is_shutdown():
+        break
+    rospy.logerr('Testing %f'%sample)
+    command.throttle = sample
+    pub.publish(command)
+    rospy.logerr('Set throttle to %f'%sample)
+    rospy.sleep(0.5)
+    rospy.logerr('Waiting for scale...')
+    have_new_scale_msg = False
+    while not have_new_scale_msg:
+        if rospy.is_shutdown():
+            break
+    rospy.logerr('Received thrust of %f'%last_scale_msg.data)
+    forces[i] = last_scale_msg.data
+np.savetxt('thrust_data.csv',
+           np.hstack(np.vstack(samples), np.vstack(forces)),
+           delimiter=',')
+rospy.logerr('Setting throttle to 0')
+command.throttle = 0
+pub.publish(command)
+rospy.logerr('Disarming')
+arm_service(False)
+rospy.logerr('Disarmed')

thrust_testing/thrust_testing.ino

@@ -0,0 +1,44 @@
+#include <ros.h>
+#include <std_msgs/Float64.h>
+#include <HX711.h>
+
+HX711 scale;
+ros::NodeHandle nh;
+std_msgs::Float64 msg;
+ros::Publisher pub ("scale", &msg);
+
+float units;
+void setup() {
+  pinMode(13, OUTPUT);
+  digitalWrite(13, LOW);
+
+  scale.begin(A4, A5);
+  nh.initNode();
+  nh.advertise(pub);
+  scale.set_scale();
+  scale.tare(20);
+  //scale.set_scale(397756.0);
+
+  digitalWrite(13, HIGH);
+  delay(10000);
+  digitalWrite(13, LOW);
+  units = scale.get_units(10);
+  scale.set_scale(units/0.5);
+  digitalWrite(13, HIGH);
+}
+
+boolean p = false;
+void loop() {
+  if (p) {
+    msg.data = units/0.5;
+    pub.publish(&msg);
+    nh.spinOnce();
+  }
+
+//  msg.data = scale.get_units(5);
+//  pub.publish(&msg);
+//  nh.spinOnce();
+  msg.data = scale.read();
+  pub.publish(&msg);
+  nh.spinOnce();
+}