element_node.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 os, signal, time
import thread

class ElementROS():
    def __init__(self):
        rospy.init_node('element', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        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", 50))
        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()
  
        # A cmd_vel publisher so we can stop the robot when shutting down
        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")

        mutex = thread.allocate_lock()

        # Initialize any sensors
        if self.publish_sensors:
            mySensors = Sensors(self.controller)
                
        # Initialize the base controller if used
        if self.use_base_controller:
            myBaseController = BaseController(self.controller)
    
        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            if self.publish_sensors:
                for sensor in mySensors.sensors:
                    mutex.acquire()
                    sensor.poll()
                    mutex.release()
                    
            if self.use_base_controller:
                mutex.acquire()
                myBaseController.poll()
                mutex.release()
            
            # For backwards compatibility with the old Serializer ROS package...
            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
        rospy.loginfo("Shutting down Element Node...")
        
if __name__ == '__main__':
    myElement = ElementROS()