From 3559a4461ae78a58bd6c2937e3efb59390fb2904 Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Tue, 15 Dec 2015 06:46:24 -0800
Subject: [PATCH] Fixed sweep_demo.py loop to prevent stalling and added
 sweep_demo.launch file

---
 ros_arduino_python/launch/sweep_servo.launch |  7 ++++++
 ros_arduino_python/nodes/sweep_servo.py      | 23 +++++++++-----------
 2 files changed, 17 insertions(+), 13 deletions(-)
 create mode 100755 ros_arduino_python/launch/sweep_servo.launch

diff --git a/ros_arduino_python/launch/sweep_servo.launch b/ros_arduino_python/launch/sweep_servo.launch
new file mode 100755
index 0000000..3ba78d0
--- /dev/null
+++ b/ros_arduino_python/launch/sweep_servo.launch
@@ -0,0 +1,7 @@
+<launch>
+   <arg name="joint" default="head_pan_joint" />
+   
+   <node pkg="ros_arduino_python" name="$(anon sweep_servo)" type="sweep_servo.py" output="screen" clear_params="true">
+      <param name="joint" value="$(arg joint)" />
+   </node>
+</launch>
diff --git a/ros_arduino_python/nodes/sweep_servo.py b/ros_arduino_python/nodes/sweep_servo.py
index f061695..64992f6 100755
--- a/ros_arduino_python/nodes/sweep_servo.py
+++ b/ros_arduino_python/nodes/sweep_servo.py
@@ -35,7 +35,7 @@ class SweepServo():
         self.node_name = "sweep_servo"
         
         # Initialize the node
-        rospy.init_node(self.node_name)
+        rospy.init_node(self.node_name, anonymous=True)
                 
         # Set a shutdown function to clean up when termniating the node
         rospy.on_shutdown(self.shutdown)
@@ -44,19 +44,18 @@ class SweepServo():
         joint_name = rospy.get_param('~joint', 'head_pan_joint')
             
         if joint_name == '':
-            rospy.logino("Joint name for servo must be specified in launch file.")
+            rospy.logino("Joint name for servo must be specified in parameter file.")
             os._exit(1)
             
         servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin')
         max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle'))
         min_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_angle'))
         
-        # Set the sweep limits to 90% of the max limits
-        target_max = 0.9 * max_angle
-        target_min = 0.9 * min_angle
+        target_max = max_angle
+        target_min = min_angle
                 
         # How fast should we sweep the servo
-        speed = rospy.get_param('~speed', 60) # degrees per second
+        speed = rospy.get_param('~speed', 1.0) # rad/s
         
         # Time between between sweeps
         delay = rospy.get_param('~delay', 2)  # seconds
@@ -93,17 +92,15 @@ class SweepServo():
 
         rospy.loginfo('Sweeping servo...')
         
-        while not rospy.is_shutdown():
-            joint_pub.publish(target_max)
-            
+        while not rospy.is_shutdown():            
             while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
+                joint_pub.publish(target_max)
                 rospy.sleep(0.1)
                 
             rospy.sleep(delay)
             
-            joint_pub.publish(target_min)
-            
-            while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
+            while abs(self.get_joint_position(joint_name) - target_min) > 0.1:  
+                joint_pub.publish(target_min)
                 rospy.sleep(0.1)
             
             rospy.sleep(delay)
@@ -125,4 +122,4 @@ if __name__ == '__main__':
     except: 
         rospy.loginfo('Unexpected error: ' +  str(sys.exc_info()[0]))
         raise
-              
\ No newline at end of file
+    
\ No newline at end of file