arduino_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     A ROS Node for the Arduino microcontroller
00005     
00006     Created for the Pi Robot Project: http://www.pirobot.org
00007     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
00008 
00009     This program is free software; you can redistribute it and/or modify
00010     it under the terms of the GNU General Public License as published by
00011     the Free Software Foundation; either version 2 of the License, or
00012     (at your option) any later version.
00013     
00014     This program is distributed in the hope that it will be useful,
00015     but WITHOUT ANY WARRANTY; without even the implied warranty of
00016     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017     GNU General Public License for more details at:
00018     
00019     http://www.gnu.org/licenses/gpl.html
00020 """
00021 
00022 import rospy
00023 from ros_arduino_python.arduino_driver import Arduino
00024 from ros_arduino_python.arduino_sensors import *
00025 from ros_arduino_msgs.srv import *
00026 from ros_arduino_python.base_controller import BaseController
00027 from geometry_msgs.msg import Twist
00028 import os, time
00029 import thread
00030 
00031 class ArduinoROS():
00032     def __init__(self):
00033         rospy.init_node('Arduino', log_level=rospy.DEBUG)
00034                 
00035         # Cleanup when termniating the node
00036         rospy.on_shutdown(self.shutdown)
00037         
00038         self.port = rospy.get_param("~port", "/dev/ttyACM0")
00039         self.baud = int(rospy.get_param("~baud", 57600))
00040         self.timeout = rospy.get_param("~timeout", 0.5)
00041         self.base_frame = rospy.get_param("~base_frame", 'base_link')
00042 
00043         # Overall loop rate: should be faster than fastest sensor rate
00044         self.rate = int(rospy.get_param("~rate", 50))
00045         r = rospy.Rate(self.rate)
00046 
00047         # Rate at which summary SensorState message is published. Individual sensors publish
00048         # at their own rates.        
00049         self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
00050         
00051         self.use_base_controller = rospy.get_param("~use_base_controller", False)
00052         
00053         # Set up the time for publishing the next SensorState message
00054         now = rospy.Time.now()
00055         self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
00056         self.t_next_sensors = now + self.t_delta_sensors
00057         
00058         # Initialize a Twist message
00059         self.cmd_vel = Twist()
00060   
00061         # A cmd_vel publisher so we can stop the robot when shutting down
00062         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
00063         
00064         # The SensorState publisher periodically publishes the values of all sensors on
00065         # a single topic.
00066         self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
00067         
00068         # A service to position a PWM servo
00069         rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
00070         
00071         # A service to read the position of a PWM servo
00072         rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
00073         
00074         # A service to turn set the direction of a digital pin (0 = input, 1 = output)
00075         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
00076         
00077         # A service to turn a digital sensor on or off
00078         rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
00079        
00080         # A service to set pwm values for the pins
00081         rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
00082 
00083         # Initialize the controlller
00084         self.controller = Arduino(self.port, self.baud, self.timeout)
00085         
00086         # Make the connection
00087         self.controller.connect()
00088         
00089         rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
00090      
00091         # Reserve a thread lock
00092         mutex = thread.allocate_lock()
00093 
00094         # Initialize any sensors
00095         self.mySensors = list()
00096         
00097         sensor_params = rospy.get_param("~sensors", dict({}))
00098         
00099         for name, params in sensor_params.iteritems():
00100             # Set the direction to input if not specified
00101             try:
00102                 params['direction']
00103             except:
00104                 params['direction'] = 'input'
00105                 
00106             if params['type'] == "Ping":
00107                 sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
00108             elif params['type'] == "GP2D12":
00109                 sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
00110             elif params['type'] == 'Digital':
00111                 sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
00112             elif params['type'] == 'Analog':
00113                 sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
00114             elif params['type'] == 'PololuMotorCurrent':
00115                 sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
00116             elif params['type'] == 'PhidgetsVoltage':
00117                 sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
00118             elif params['type'] == 'PhidgetsCurrent':
00119                 sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
00120                 
00121 #                if params['type'] == "MaxEZ1":
00122 #                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
00123 #                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
00124 
00125             self.mySensors.append(sensor)
00126             rospy.loginfo(name + " " + str(params))
00127               
00128         # Initialize the base controller if used
00129         if self.use_base_controller:
00130             self.myBaseController = BaseController(self.controller, self.base_frame)
00131     
00132         # Start polling the sensors and base controller
00133         while not rospy.is_shutdown():
00134             for sensor in self.mySensors:
00135                 mutex.acquire()
00136                 sensor.poll()
00137                 mutex.release()
00138                     
00139             if self.use_base_controller:
00140                 mutex.acquire()
00141                 self.myBaseController.poll()
00142                 mutex.release()
00143             
00144             # Publish all sensor values on a single topic for convenience
00145             now = rospy.Time.now()
00146             
00147             if now > self.t_next_sensors:
00148                 msg = SensorState()
00149                 msg.header.frame_id = self.base_frame
00150                 msg.header.stamp = now
00151                 for i in range(len(self.mySensors)):
00152                     msg.name.append(self.mySensors[i].name)
00153                     msg.value.append(self.mySensors[i].value)
00154                 try:
00155                     self.sensorStatePub.publish(msg)
00156                 except:
00157                     pass
00158                 
00159                 self.t_next_sensors = now + self.t_delta_sensors
00160             
00161             r.sleep()
00162     
00163     # Service callback functions
00164     def ServoWriteHandler(self, req):
00165         self.controller.servo_write(req.id, req.value)
00166         return ServoWriteResponse()
00167     
00168     def ServoReadHandler(self, req):
00169         self.controller.servo_read(req.id)
00170         return ServoReadResponse()
00171     
00172     def DigitalSetDirectionHandler(self, req):
00173         self.controller.pin_mode(req.pin, req.direction)
00174         return DigitalSetDirectionResponse()
00175     
00176     def DigitalWriteHandler(self, req):
00177         self.controller.digital_write(req.pin, req.value)
00178         return DigitalWriteResponse()
00179               
00180     def AnalogWriteHandler(self, req):
00181         self.controller.analog_write(req.pin, req.value)
00182         return AnalogWriteResponse()
00183  
00184     def shutdown(self):
00185         # Stop the robot
00186         try:
00187             rospy.loginfo("Stopping the robot...")
00188             self.cmd_vel_pub.Publish(Twist())
00189             rospy.sleep(2)
00190         except:
00191             pass
00192         rospy.loginfo("Shutting down Arduino Node...")
00193         
00194 if __name__ == '__main__':
00195     myArduino = ArduinoROS()


ros_arduino_python
Author(s): Patrick Goebel
autogenerated on Thu Jun 6 2019 21:43:59