sample_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     sample_node.py - This sample program subscribes to the /element/SensorState
00005     topic and echos the results.  It also moves a servo on GPIO pin 8 (servo ID 1)
00006     if the value of the first sensor (index 0) is above or below a certain threshold.
00007     
00008     Created for the Pi Robot Project: http://www.pirobot.org
00009     Copyright (c) 2012
00010     
00011     This program is free software; you can redistribute it and/or modify
00012     it under the terms of the GNU General Public License as published by
00013     the Free Software Foundation; either version 2 of the License, or
00014     (at your option) any later version.
00015     
00016     This program is distributed in the hope that it will be useful,
00017     but WITHOUT ANY WARRANTY; without even the implied warranty of
00018     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019     GNU General Public License for more details at:
00020     
00021     http://www.gnu.org/licenses/gpl.html
00022 """
00023 
00024 import roslib; roslib.load_manifest('element')
00025 import rospy
00026 from element.srv import *
00027 from element.msg import *
00028 
00029 class sample_node():
00030     def __init__(self):
00031         # Name this node
00032         rospy.init_node("sample_element_node")
00033         
00034         # Execute this function when shutting down
00035         rospy.on_shutdown(self.shutdown)
00036         
00037         # Set the update rate to 1 second by default
00038         self.rate = rospy.get_param("~rate", 1)
00039         r = rospy.Rate(self.rate)
00040         
00041         # Initialize the sensor value array
00042         self.sensor_state = SensorState()
00043         
00044         # Set the scanning servo limit
00045         self.servo_position = 60
00046         
00047         # Subscribe to the senors topic
00048         rospy.Subscriber('/element/sensors', SensorState, self.get_sensor_state)
00049         
00050         # Subscribe to the SetServo service
00051         rospy.wait_for_service('/element/SetServo')      
00052         self.servo_proxy = rospy.ServiceProxy('/element/SetServo', SetServo)
00053 
00054         # Begin the main loop
00055         while not rospy.is_shutdown():
00056             # Echo back the current sensor values
00057             rospy.loginfo(self.sensor_state)
00058             
00059             # Move one of the servos back and forth
00060             try:
00061                 self.servo_position = -self.servo_position
00062                 self.servo_proxy(1, self.servo_position)
00063             except:
00064                 pass
00065                 
00066             # Sleep for 1/rate seconds
00067             r.sleep()
00068             
00069     # Callback for the sensor state subscriber
00070     def get_sensor_state(self, data):
00071         self.sensor_state = data
00072     
00073     # Shutdown function just prints a message.
00074     def shutdown(self):
00075         rospy.loginfo("Shutting down Sample Node...")
00076 
00077 if __name__ == '__main__':
00078     try:
00079         my_node = sample_node()
00080     except rospy.ROSInterruptException:
00081         pass


element
Author(s): Patrick Goebel
autogenerated on Sun Oct 5 2014 23:44:54