sample_node.py

Go to the documentation of this file.

#!/usr/bin/env python

"""
    sample_node.py - This sample program subscribes to the /element/SensorState
    topic and echos the results.  It also moves a servo on GPIO pin 8 (servo ID 1)
    if the value of the first sensor (index 0) is above or below a certain threshold.
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012
    
    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.
    
    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 roslib; roslib.load_manifest('element')
import rospy
from element.srv import *
from element.msg import *

class sample_node():
    def __init__(self):
        # Name this node
        rospy.init_node("sample_element_node")
        
        # Execute this function when shutting down
        rospy.on_shutdown(self.shutdown)
        
        # Set the update rate to 1 second by default
        self.rate = rospy.get_param("~rate", 1)
        r = rospy.Rate(self.rate)
        
        # Initialize the sensor value array
        self.sensor_state = SensorState()
        
        # Set the scanning servo limit
        self.servo_position = 60
        
        # Subscribe to the senors topic
        rospy.Subscriber('/element/sensors', SensorState, self.get_sensor_state)
        
        # Subscribe to the SetServo service
        rospy.wait_for_service('/element/SetServo')      
        self.servo_proxy = rospy.ServiceProxy('/element/SetServo', SetServo)

        # Begin the main loop
        while not rospy.is_shutdown():
            # Echo back the current sensor values
            rospy.loginfo(self.sensor_state)
            
            # Move one of the servos back and forth
            try:
                self.servo_position = -self.servo_position
                self.servo_proxy(1, self.servo_position)
            except:
                pass
                
            # Sleep for 1/rate seconds
            r.sleep()
            
    # Callback for the sensor state subscriber
    def get_sensor_state(self, data):
        self.sensor_state = data
    
    # Shutdown function just prints a message.
    def shutdown(self):
        rospy.loginfo("Shutting down Sample Node...")

if __name__ == '__main__':
    try:
        my_node = sample_node()
    except rospy.ROSInterruptException:
        pass