element_node_threads.py

Go to the documentation of this file.

#!/usr/bin/env python

"""
    ROS Node for the cmRobot Element microcontroller
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 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.
    
    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_driver import Element
from base_controller import BaseController
from sensors import Sensors
from element.srv import *
from element.msg import *
from geometry_msgs.msg import Twist
import Queue
import threading
import os, signal, time

class ElementROS():
    def __init__(self):
        rospy.init_node('element', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        # The thread queue to use for sensor and base control
        self.thread_queue = Queue.Queue()
        
        #self.thread_queue.maxsize = 5
        
        # A lock for the queue
        self.queue_lock = threading.Lock()
        
        # Thread pool for running publishers
        self.thread_pool = list()
    
        # An empty object to cleanly stop the queue on exit
        self.STOP = object()
        
        self.port = rospy.get_param("~port", "/dev/ttyUSB0")
        self.baud = int(rospy.get_param("~baud", 19200))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.sensor_rate = int(rospy.get_param("~sensor_rate", 10))
        self.rate = int(rospy.get_param("~rate", 20))
        r = rospy.Rate(self.rate)
        
        self.units = rospy.get_param("~units", 0)
        
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensor_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        self.publish_sensors = rospy.get_param("~publish_sensors", True)
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        self.pid_params = rospy.get_param("~pid_params", {})
        
        # ROS services for sensors and base control.
        rospy.Service('~SetServo', SetServo, self.SetServoHandler)
        rospy.Service('~GetAnalog', GetAnalog, self.GetAnalogHandler)
        rospy.Service('~Voltage', Voltage, self.VoltageHandler)
        rospy.Service('~Ping', Ping, self.PingHandler)
        rospy.Service('~MaxEZ1', MaxEZ1, self.MaxEZ1Handler)
        rospy.Service('~GP2D12', GP2D12, self.GP2D12Handler)
        rospy.Service('~PhidgetsTemperature', PhidgetsTemperature, self.PhidgetsTemperatureHandler)
        rospy.Service('~PhidgetsVoltage', PhidgetsVoltage, self.PhidgetsVoltageHandler)
        rospy.Service('~PhidgetsCurrent', PhidgetsCurrent, self.PhidgetsCurrentHandler)
        rospy.Service('~Rotate', Rotate, self.RotateHandler)
        rospy.Service('~TravelDistance', TravelDistance, self.TravelDistanceHandler)
        rospy.Service('~Speak', SP03, self.SP03Handler)
        
        # The SensorState publisher for backward compatibility
        self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
        
        # Initialize the Twist message
        self.cmd_vel = Twist()
  
        # Publish the Twist message to the cmd_vel topic
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
           
        # Initialize and connect to the controller
        self.controller = Element(self.port, self.baud, self.timeout, self.units)
        self.controller.connect(self.use_base_controller)
        
        rospy.loginfo("Connected to Element on port " + self.port + " at " + str(self.baud) + " baud")
        
        self.heartbeat = 0

        # Initialize any sensors
        if self.publish_sensors:
            mySensors = Sensors(self.controller, self.thread_queue)
        
            for i in range(len(mySensors.sensors)):
                self.thread_pool.append(threading.Timer(mySensors.sensors[i].interval, mySensors.poll_sensors, [self.thread_queue, mySensors.sensors[i]]))
                self.thread_pool[i].daemon = True
                self.thread_pool[i].start()
                
        # Initialize the base controller if used
        if self.use_base_controller:
            myBaseController = BaseController(self.controller, self.thread_queue)
            base_thread = threading.Timer(myBaseController.interval, myBaseController.poll_odom, [self.thread_queue])
            self.thread_pool.append(base_thread)
            self.thread_pool[len(self.thread_pool) - 1].daemon = True
            self.thread_pool[len(self.thread_pool) - 1].start()
            #base_thread.start()
    
        # Set the thread queue in motion...
        while True:
         
            #if not self.thread_queue.empty() and not self.thread_queue.full():
            print "tick: ", self.heartbeat, self.thread_queue.qsize()

            #self.queue_lock.acquire()
            # Get the next poller
            poller = self.thread_queue.get()
            #self.queue_lock.release()
                        
            if poller is self.STOP:
                self.thread_queue.task_done()
                break
            
            # Run the poller
            poller()
            
            # Signal that we are finished
            #self.thread_queue.task_done()
            
            self.heartbeat += 1
            
            # For backwards compatibility...
            now = rospy.Time.now()
            
            if self.publish_sensors and now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = 'base_link'
                msg.header.stamp = now
                for i in range(len(mySensors.sensors)):
                    msg.name.append(mySensors.sensors[i].name)
                    msg.value.append(mySensors.sensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
                
    def SetServoHandler(self, req):
        self.controller.servo(req.id, req.value)
        return SetServoResponse()
    
    def RotateHandler(self, req):
        self.controller.rotate(req.angle, req.velocity)
        return RotateResponse()
    
    def TravelDistanceHandler(self, req):
        self.controller.travel_distance(req.distance, req.velocity)
        return TravelDistanceResponse()
    
    def GetAnalogHandler(self, req):
        return GetAnalogResponse(self.controller.get_analog(req.pin))
    
    def PingHandler(self, req):
        try:
            range = self.controller.get_Ping(req.pin, req.cached)
            if self.units == 0:
                range /= 100.
        except:
            rospy.loginfo("Bad sonar value on Ping " + str(req.pin) + ". Value was: " + str(range))
        return PingResponse(range)
    
    def MaxEZ1Handler(self, req):
        try:
            range = self.controller.get_MaxEZ1(req.trigger_pin, req.output_pin, req.cached)
            if self.units == 0:
                range /= 100.
        except:
            rospy.loginfo("Bad sonar value on MaxEZ1 " + str(req.trigger_pin) + ". Value was: " + str(range))
        return MaxEZ1Response(range)

    def GP2D12Handler(self, req):
        try:
            range = self.controller.get_GP2D12(req.pin, req.cached)
            if self.units == 0:
                range /= 100.
        except:
            rospy.loginfo("Bad IR value on GP2D12 " + str(req.pin) + ". Value was: " + str(range))
        return GP2D12Response(range)
                
    def VoltageHandler(self, req):
        return VoltageResponse(self.controller.voltage(req.cached))

    def PhidgetsTemperatureHandler(self, req):
        return PhidgetsTemperatureResponse(self.controller.get_PhidgetsTemperature(req.pin, req.cached))
    
    def PhidgetsVoltageHandler(self, req):
        return PhidgetsVoltageResponse(self.controller.get_PhidgetsVoltage(req.pin, req.cached))
    
    def PhidgetsCurrentHandler(self, req):
        return PhidgetsCurrentResponse(self.controller.get_PhidgetsCurrent(req.pin, req.cached))
    
    def SP03Handler(self, req):
        return SP03Response(self.controller.sp03(req.msg, req.i2c_addr))
    
    def signal_handler(self, signum, frame):
        pass
        
    def shutdown(self):
        # Stop the robot
        try:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.publish(Twist())
            rospy.sleep(2)
        except:
            pass
        self.controller.shutdown = True
        rospy.loginfo("Shutting down Element Node...")
        rospy.loginfo("Closing threads, please wait...")
        for i in range(len(self.thread_pool)):
            self.thread_queue.put(self.STOP)
        #self.thread_queue.join()
        
if __name__ == '__main__':
    myElement = ElementROS()