Merge pull request #15 from KKalem/noetic-devel

Joystick controller updates

sam_joystick/scripts/joystick_to_sam_setpoints.py

@@ -5,32 +5,29 @@
 import rospy
 import math
 
-from sam_msgs.msg import ThrusterAngles
-from smarc_msgs.msg import ThrusterRPM
+from sam_msgs.msg import ThrusterAngles, ThrusterRPMs
 from geometry_msgs.msg import Twist
 
 class JoystickTranslator(object):
     def __init__(self):
         self.rpm_sub = rospy.Subscriber("ctrl/rpm_joystick", Twist, self.rpm_cb, queue_size=1)
         self.vector_sub = rospy.Subscriber("ctrl/vector_deg_joystick", Twist, self.vector_cb, queue_size=1)
 
-        self.thruster1_pub = rospy.Publisher("core/thruster1_cmd", ThrusterRPM, queue_size=1)
-        self.thruster2_pub = rospy.Publisher("core/thruster2_cmd", ThrusterRPM, queue_size=1)
+        self.thruster_pub = rospy.Publisher("core/rpm_cmd", ThrusterRPMs, queue_size=1)
         self.vector_pub = rospy.Publisher("core/thrust_vector_cmd", ThrusterAngles, queue_size=1)
 
-        self.rpm_msg = ThrusterRPM()
+        self.rpm_msg = ThrusterRPMs()
         self.vec_msg = ThrusterAngles()
 
         self.published_zero_rpm_once = False
         self.published_zero_vec_once = False
 
     def rpm_cb(self, twist):
         # linear.x is forward, straight to RPMs
-        self.rpm_msg.rpm = int(twist.linear.x)
+        self.rpm_msg.thruster_1_rpm = int(twist.linear.x)
+        self.rpm_msg.thruster_2_rpm = int(twist.linear.x)
 
     def vector_cb(self, twist):
-        # angular.z is horizontal angle
-        # angular.y is vertical angle
         hori = math.radians(twist.angular.z)
         vert = math.radians(twist.angular.y)
 
@@ -39,24 +36,34 @@ def vector_cb(self, twist):
 
 
     def publish(self):
-        if self.rpm_msg.rpm != 0 or not self.published_zero_rpm_once:
-            self.thruster1_pub.publish(self.rpm_msg)
-            self.thruster2_pub.publish(self.rpm_msg)
-            rospy.loginfo("Rpm:{}".format(self.rpm_msg.rpm))
+        if self.rpm_msg.thruster_1_rpm != 0 or not self.published_zero_rpm_once:
+            self.thruster_pub.publish(self.rpm_msg)
+            zero = self.rpm_msg.thruster_1_rpm == 0
+            if zero:
+                rospy.loginfo(">>>>> Published 0 RPM")
+            else:
+                rospy.loginfo_throttle(1, "Rpm:{}".format(self.rpm_msg.thruster_1_rpm))
 
-            self.published_zero_rpm_once = self.rpm_msg.rpm == 0
+            self.published_zero_rpm_once = zero
 
         if self.vec_msg.thruster_horizontal_radians != 0 or \
            self.vec_msg.thruster_vertical_radians != 0 or \
            not self.published_zero_vec_once:
+
             self.vector_pub.publish(self.vec_msg)
-            rospy.loginfo("Vec:{},{}".format(self.vec_msg.thruster_horizontal_radians, self.vec_msg.thruster_vertical_radians))
 
-            self.published_zero_vec_once = self.vec_msg.thruster_horizontal_radians == 0 and\
-                                           self.vec_msg.thruster_vertical_radians == 0
+            zero = self.vec_msg.thruster_horizontal_radians == 0 and\
+                   self.vec_msg.thruster_vertical_radians == 0
+            if zero:
+                rospy.loginfo(">>>>> Published 0 thrust vector")
+            else:
+                rospy.loginfo_throttle(1, "Vec:{},{}".format(self.vec_msg.thruster_horizontal_radians, self.vec_msg.thruster_vertical_radians))
+
+            self.published_zero_vec_once = zero
 
         # reset the stuff
-        self.rpm_msg.rpm = 0
+        self.rpm_msg.thruster_1_rpm = 0
+        self.rpm_msg.thruster_2_rpm = 0
         self.vec_msg.thruster_horizontal_radians = 0
         self.vec_msg.thruster_vertical_radians = 0
 
@@ -66,13 +73,15 @@ def publish(self):
 if __name__ == "__main__":
     rospy.init_node("joystick_to_sam_setpoints")
     jt = JoystickTranslator()
-    rospy.loginfo("Joystick translator for SAM is running...")
+    rospy.loginfo("Digital joystick translator for SAM is running...")
 
-    rate = rospy.Rate(10)
+    rate = rospy.Rate(12)
     while not rospy.is_shutdown():
         jt.publish()
         rate.sleep()
 
+    rospy.loginfo("Joystick listener stopped")
+