Added servo example to ros_arduino_python

ros_arduino_python/config/servo_example_params.yaml

@@ -0,0 +1,31 @@
+# A UDEV rule maps the Arduino serial number to /dev/arduino
+
+port: /dev/arduino
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: False
+
+joint_update_rate: 10
+
+# === Sensor definitions.  Examples only - edit for your robot.
+#     Sensor type can be one of the follow (case sensitive!):
+#	  * Analog
+#     * Digital
+#     * Ping
+#	  * GP2D12
+#	  * PololuMotorCurrent
+#	  * PhidgetsVoltage
+#	  * PhidgetsCurrent (20 Amp, DC)
+
+sensors: {
+  onboard_led: {pin: 13, type: Digital, direction: output, rate: 1}
+}
+
+# Joint name and pin assignment is an example only
+joints: {
+    head_pan_joint: {pin: 3, init_position: 30, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False}
+}

ros_arduino_python/launch/servo_example.launch

@@ -0,0 +1,5 @@
+<launch>
+   <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen" clear_params="true">
+      <rosparam file="$(find ros_arduino_python)/config/servo_example_params.yaml" command="load" />
+   </node>
+</launch>

ros_arduino_python/nodes/sweep_servo.py

@@ -0,0 +1,128 @@
+#!/usr/bin/env python
+
+''' sweep_servo.py - Version 1.0 2015-12-04
+
+    Move a servo back and forth between two positions
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2015 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.5
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+      
+'''
+
+import rospy
+import sys
+import os
+from std_msgs.msg import Float64
+from ros_arduino_msgs.srv import SetSpeed
+from sensor_msgs.msg import JointState
+from math import radians
+
+class SweepServo():
+    def __init__(self):
+        # Set a name for the node
+        self.node_name = "sweep_servo"
+        
+        # Initialize the node
+        rospy.init_node(self.node_name)
+                
+        # Set a shutdown function to clean up when termniating the node
+        rospy.on_shutdown(self.shutdown)
+        
+        # Name of the joint we want to control
+        joint_name = rospy.get_param('~joint', 'head_pan_joint')
+            
+        if joint_name == '':
+            rospy.logino("Joint name for servo must be specified in launch 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
+                
+        # How fast should we sweep the servo
+        speed = rospy.get_param('~speed', 60) # degrees per second
+        
+        # Time between between sweeps
+        delay = rospy.get_param('~delay', 2)  # seconds
+        
+        # Create a publisher for setting the joint position
+        joint_pub = rospy.Publisher('/' + joint_name + '/command', Float64, queue_size=5)
+        
+        self.joint_state = JointState()
+        
+        # Subscribe to the /joint_states topic so we know where the servo is positioned
+        rospy.Subscriber('/joint_states', JointState, self.update_joint_state)
+        
+        # Wait for the /joint_state message to come online
+        rospy.wait_for_message('/joint_states', JointState, 60)
+        
+        # Wait until we actually have a message
+        while self.joint_state == JointState():
+            rospy.sleep(0.1)
+        
+        # Wait for the set_speed service
+        rospy.loginfo('Waiting for set_speed services...')
+        try:
+            rospy.wait_for_service('/' + joint_name + '/set_speed', 60)
+            rospy.loginfo('Ready.')
+        except:
+            rospy.loginfo('Could not connect to service!')
+            os._exit(1)
+
+        # Create a proxy for the set speed service
+        set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed)
+        
+        # Set the initial servo speed
+        set_speed(speed)
+
+        rospy.loginfo('Sweeping servo...')
+        
+        while not rospy.is_shutdown():
+            joint_pub.publish(target_max)
+            
+            while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
+                rospy.sleep(0.1)
+                
+            rospy.sleep(delay)
+            
+            joint_pub.publish(target_min)
+            
+            while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
+                rospy.sleep(0.1)
+            
+            rospy.sleep(delay)
+    
+    def update_joint_state(self, msg):
+        self.joint_state = msg
+        
+    def get_joint_position(self, joint_name):
+        index = self.joint_state.name.index(joint_name)
+        return self.joint_state.position[index]
+    
+    def shutdown(self):
+        rospy.loginfo('Shutting down ' + str(self.node_name))
+        os._exit(0)
+        
+if __name__ == '__main__':
+    try:
+        SweepServo()
+    except: 
+        rospy.loginfo('Unexpected error: ' +  str(sys.exc_info()[0]))
+        raise
+