def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)

        # Get the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()

        # Initialize a Twist message
        self.cmd_vel = Twist()

        # Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout,
                                  self.motors_reversed)

        # Make the connection
        self.controller.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()
        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame,
                self.name + "_base_controller")

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            r.sleep()
    def __init__(self):
        rospy.init_node('ArbotixM', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyArb")
        self.baud = int(rospy.get_param("~baud", 38400))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        
        # Initialize a 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 the controlller
        self.controller = ArbotixM(self.port, self.baud, self.timeout)
        
        # Make the connection
        #self.controller.connect() <<<<<<<<<<<Debug
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()
              
        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller, self.base_frame)
    
        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            now = rospy.Time.now()
            
            r.sleep()
class ArduinoROS():
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)

        # Get the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensor_state_rate = int(rospy.get_param("~sensor_state_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensor_state_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=5)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout)

        # Make the connection
        self.controller.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'],
                              params['echopin'], params['rate'],
                              params['frame_id'])
            elif params['type'] == "PointCloudPing":
                sensor = PointCloudPing(self.controller, name, params['pin'],
                                        params['echopin'], params['rate'],
                                        params['frame_id'])
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'])
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       self.base_frame,
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      self.base_frame,
                                      direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'])
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'])
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'])

            try:
                self.mySensors.append(sensor)
                rospy.loginfo(name + " " + str(params) +
                              " published on topic " + rospy.get_name() +
                              "/sensor/" + name)
            except:
                rospy.logerr("Sensor type " + str(params['type']) +
                             " not recognized.")

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame,
                self.name + "_base_controller")

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = 'sensor_base'
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()

    # Service callback functions
    def ServoWriteHandler(self, req):
        self.controller.servo_write(req.id, req.value)
        return ServoWriteResponse()

    def ServoReadHandler(self, req):
        self.controller.servo_read(req.id)
        return ServoReadResponse()

    def DigitalSetDirectionHandler(self, req):
        self.controller.pin_mode(req.pin, req.direction)
        return DigitalSetDirectionResponse()

    def DigitalWriteHandler(self, req):
        self.controller.digital_write(req.pin, req.value)
        return DigitalWriteResponse()

    def AnalogWriteHandler(self, req):
        self.controller.analog_write(req.pin, req.value)
        return AnalogWriteResponse()

    def shutdown(self):
        rospy.loginfo("Shutting down Arduino Node...")
        # Stop the robot
        try:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.Publish(Twist())
            rospy.sleep(2)
        except:
            pass
        #Close the serial port
        try:
            self.controller.close()
        except:
            pass
        finally:
            rospy.loginfo("Serial port closed.")
            os._exit(0)
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.INFO)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Assume we don't have any joints by default
        self.have_joints = False
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        # Initialize a 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, queue_size=5)
        
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
        
        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
        
        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
        
        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
        
        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
        
        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # Initialize the device
        self.device = Arduino(self.port, self.baud, self.timeout)
        
        # Make the connection
        self.device.connect()
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()
        
        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))
        
        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
                
#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            
            if params['rate'] != None and params['rate'] != 0:
                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
            else:
                if sensor.direction == "input":
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
                else:
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")

            
        # Initialize any joints (servos)
        self.device.joints = dict()
        
        # Read in the joints (if any)    
        joint_params = rospy.get_param("~joints", dict())
        
        if len(joint_params) != 0:
            self.have_joints = True
            
            # Configure each servo
            for name, params in joint_params.iteritems():
                self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))
            
            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device, "ServoController")
            
            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()
            
#             # Initialize any trajectory action follow controllers
#             controllers = rospy.get_param("~controllers", dict())
#              
#             self.device.controllers = list()
#              
#             for name, params in controllers.items():
#                 try:
#                     controller = controller_types[params["type"]](self.device, name)
#                     self.device.controllers.append(controller)
#                 except Exception as e:
#                     if type(e) == KeyError:
#                         rospy.logerr("Unrecognized controller: " + params["type"])
#                     else:  
#                         rospy.logerr(str(type(e)) + str(e))
#      
#             for controller in self.device.controllers:
#                 controller.startup()

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.device, self.base_frame)
            
        print "\n==> ROS Arduino Bridge ready for action!"
    
        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                if sensor.rate != 0:
                    mutex.acquire()
                    sensor.poll()
                    mutex.release()                  
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()
                
            if self.have_joints:
                mutex.acquire()
                self.servo_controller.poll()
                self.joint_state_publisher.poll(self.device.joints.values())
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
Exemple #5
0
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.INFO)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Assume we don't have any joints by default
        self.have_joints = False
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        # Initialize a 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, queue_size=5)
        
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
        
        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
        
        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
        
        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
        
        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
        
        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # Initialize the device
        self.device = Arduino(self.port, self.baud, self.timeout)
        
        # Make the connection
        self.device.connect()
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()
        
        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))
        
        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
                
#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            
            if params['rate'] != None and params['rate'] != 0:
                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
            else:
                if sensor.direction == "input":
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
                else:
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")

            
        # Initialize any joints (servos)
        self.device.joints = dict()
        
        # Read in the joints (if any)    
        joint_params = rospy.get_param("~joints", dict())
        
        if len(joint_params) != 0:
            self.have_joints = True
            
            # Configure each servo
            for name, params in joint_params.iteritems():
                self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))
            
            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device, "ServoController")
            
            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()
            
#             # Initialize any trajectory action follow controllers
#             controllers = rospy.get_param("~controllers", dict())
#              
#             self.device.controllers = list()
#              
#             for name, params in controllers.items():
#                 try:
#                     controller = controller_types[params["type"]](self.device, name)
#                     self.device.controllers.append(controller)
#                 except Exception as e:
#                     if type(e) == KeyError:
#                         rospy.logerr("Unrecognized controller: " + params["type"])
#                     else:  
#                         rospy.logerr(str(type(e)) + str(e))
#      
#             for controller in self.device.controllers:
#                 controller.startup()

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.device, self.base_frame)
            
        print "\n==> ROS Arduino Bridge ready for action!"
    
        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                if sensor.rate != 0:
                    mutex.acquire()
                    sensor.poll()
                    mutex.release()                  
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()
                
            if self.have_joints:
                mutex.acquire()
                self.servo_controller.poll()
                self.joint_state_publisher.poll(self.device.joints.values())
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        self.l_scale = rospy.get_param("~linear_scale", 10)
        self.a_scale = rospy.get_param("~angular_scale", 90)

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        rospy.Subscriber("html_joy_stick", Joy, self.JoystickCallback)

        # Initialize a 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, queue_size=10)

        # A pan_tilt publisher so we can control the position of the pan/tilt gimble and center it on shutdown
        self.pan_tilt_pub = rospy.Publisher('pan_tilt', PanTilt, queue_size=10)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=10)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # Initialize the controller
        self.controller = None
        driver = rospy.get_param("~arduino_driver", "rab_driver")
        params = {'pid rate': rospy.get_param("~base_controller_rate", 10)}
        if driver == "rab_driver":
            from ros_arduino_python.rab_driver import RABDriver
            self.controller = RABDriver(params=params,
                                        port=self.port,
                                        baud=self.baud,
                                        timeout=self.timeout)
        elif driver == "pymata_driver":
            from ros_arduino_python.pymata_driver import PymataDriver
            self.controller = PymataDriver(params=params,
                                           port=self.port,
                                           baud=self.baud,
                                           timeout=self.timeout)

        else:
            rospy.loginfo("Unknown Arduino driver")
            return

        # Make the connection
        self.controller.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'],
                              params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       self.base_frame,
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      self.base_frame,
                                      direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'],
                                            self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'DigitalArray':
                sensor = LineSensor(self.controller,
                                    name,
                                    params['pins'],
                                    params['pullup'],
                                    self.base_frame,
                                    rate=params['rate'],
                                    direction=params['direction'])


#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            rospy.loginfo(name + " " + str(params))

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller,
                                                   self.base_frame)

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
Exemple #7
0
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout)

        # Make the connection
        self.controller.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'],
                              params['rate'], params['frame_id'])
            elif params['type'] == "PointCloudPing":
                sensor = PointCloudPing(self.controller, name, params['pin'],
                                        params['rate'], params['frame_id'])
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'])
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'])
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'])
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'])


#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            rospy.loginfo(name + " " + str(params))

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller)

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = 'base_link'
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
Exemple #8
0
class ArduinoROS():
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        self.voice = rospy.get_param("~voice", "voice_kal_diphone")
        self.wavepath = rospy.get_param("~wavepath", "")
        # Create the sound client object
        self.soundhandle = SoundClient()
        
        rospy.sleep(1)
        self.soundhandle.stopAll()
        
        # Announce that we are ready for input
        self.soundhandle.playWave(self.wavepath + "/R2D2a.wav")
        rospy.sleep(1)
        self.soundhandle.say("How can I help U Sir", self.voice)
        
        rospy.loginfo("Say one of the navigation commands...")

        # Subscribe to the recognizer output
        rospy.Subscriber('/recognizer/output', String, self.talkback)


        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        # Initialize a 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)
        
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
       
	# A service to set pwm values for the pins
	rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

	# Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout)
        
        # Make the connection
        self.controller.connect()
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()
        
        sensor_params = rospy.get_param("~sensors", dict({}))
        
        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'
                
            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
                
#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            rospy.loginfo(name + " " + str(params))
              
        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller, self.base_frame)
    
        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
    
    # Service callback functions
  
     
    def ServoWriteHandler(self, req):
        self.controller.servo_write(req.id, req.value)
        return ServoWriteResponse()
    
    def ServoReadHandler(self, req):
        self.controller.servo_read(req.id)
        return ServoReadResponse()
    
    def DigitalSetDirectionHandler(self, req):
        self.controller.pin_mode(req.pin, req.direction)
        return DigitalSetDirectionResponse()
    
    def DigitalWriteHandler(self, req):
        self.controller.digital_write(req.pin, req.value)
        return DigitalWriteResponse()
              
    def AnalogWriteHandler(self, req):
        self.controller.analog_write(req.pin, req.value)
        return AnalogWriteResponse()
 
    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 Arduino Node...")

    def talkback(self, msg):
        # Print the recognized words on the screen
        rospy.loginfo(msg.data)

        if msg.data == "go" :
         self.soundhandle.say("going now", self.voice)
        if msg.data == "lights on" :
         self.soundhandle.say("lights are on", self.voice) 
         self.controller.digital_write(13, 1)
         return DigitalWriteResponse()
        if msg.data == "lights off" :
         self.soundhandle.say("lights are off", self.voice) 
         self.controller.digital_write(13, 0)
         return DigitalWriteResponse()
        elif msg.data == "move" :
         self.soundhandle.say("moving now", self.voice)
        elif msg.data == "bring me the glass" :
         self.soundhandle.say("ther are no glasses here", self.voice)
        elif msg.data != "go" : 
          # Speak the recognized words in the selected voice
          self.soundhandle.say(msg.data, self.voice)
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)

        # Find the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 30))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        # Are we using the base_controller?
        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Default error threshold (percent) before getting a diagnostics warning
        self.diagnotics_error_threshold = rospy.get_param(
            "~diagnotics_error_threshold", 10)

        # Diagnostics update rate
        self.diagnotics_rate = rospy.get_param("~diagnotics_rate", 1.0)

        # Assume we don't have any joints by default
        self.have_joints = False

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=5)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)

        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode,
                      self.DigitalPinModeHandler)

        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode,
                      self.AnalogPinModeHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # Initialize the device
        self.device = Arduino(self.port,
                              self.baud,
                              self.timeout,
                              self.motors_reversed,
                              debug=True)

        # Make the connection
        if self.device.connect():
            rospy.loginfo("Connected to Arduino on port " + self.port +
                          " at " + str(self.baud) + " baud")
        else:
            rospy.logerr("No serial connection found.")
            os._exit(0)

        # Initialize the base controller if used
        if self.use_base_controller:
            self.base_controller = BaseController(
                self.device, self.base_frame, self.name + "_base_controller")

            # A service to reset the odometry values to 0
            rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)

            # A service to update the PID parameters Kp, Kd, Ki, and Ko
            rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)

            # Fire up the dynamic_reconfigure server
            dyn_server = dynamic_reconfigure.server.Server(
                ROSArduinoBridgeConfig,
                self.dynamic_reconfigure_server_callback,
                namespace=self.name)

            # Connect to the dynamic_reconfigure client
            dyn_client = dynamic_reconfigure.client.Client(self.name,
                                                           timeout=5)

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.device.sensors = list()

        # Keep a list of IMUs or gyros used for odometry so they can be reset
        self.imu_for_odom = list()

        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))

        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower(
            ) or params['type'].lower() == 'GP2Y0A21YK0F'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'AnalogFloat'.lower():
                sensor = AnalogFloatSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
            elif params['type'].lower() == 'IMU'.lower():
                sensor = IMU(self.device, name, **params)
                try:
                    if params['use_for_odom']:
                        self.imu_for_odom.append(sensor)
                except:
                    pass
            elif params['type'].lower() == 'Gyro'.lower():
                try:
                    sensor = Gyro(self.device,
                                  name,
                                  base_controller=self.base_controller,
                                  **params)
                except:
                    sensor = Gyro(self.device, name, **params)

                try:
                    if params['use_for_odom']:
                        self.imu_for_odom.append(sensor)
                except:
                    pass

#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            try:
                self.device.sensors.append(sensor)

                if params['rate'] != None and params['rate'] != 0:
                    rospy.loginfo(name + " " + str(params) +
                                  " published on topic " + rospy.get_name() +
                                  "/sensor/" + name)
                else:
                    if sensor.direction == "input":
                        rospy.loginfo(name + " service ready at " +
                                      rospy.get_name() + "/" + name + "/read")
                    else:
                        rospy.loginfo(name + " service ready at " +
                                      rospy.get_name() + "/" + name + "/write")
            except:
                rospy.logerr("Sensor type " +
                             str(params['type'] + " not recognized."))

        # If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller
        if self.use_base_controller and len(self.imu_for_odom) > 0:
            self.base_controller.use_imu_heading = True

        # Read in the joints (if any)
        joint_params = rospy.get_param("~joints", dict())

        if len(joint_params) != 0:
            self.have_joints = True

            self.device.joints = dict()

            self.device.joint_update_rate = float(
                rospy.get_param("~joint_update_rate", 10))

            # Configure each servo
            for name, params in joint_params.iteritems():
                try:
                    if params['continuous'] == True:
                        self.device.joints[name] = ContinuousServo(
                            self.device, name)
                    else:
                        self.device.joints[name] = Servo(self.device, name)
                except:
                    self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))

            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device,
                                                    "ServoController")

            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()

        # Create the diagnostics updater for the Arduino device
        self.device.diagnostics = DiagnosticsUpdater(
            self,
            self.name,
            self.diagnotics_error_threshold,
            self.diagnotics_rate,
            create_watchdog=True)

        # Create the overall diagnostics publisher
        self.diagnostics_publisher = DiagnosticsPublisher(self)

        # Initialize any trajectory action follow controllers
        controllers = rospy.get_param("~controllers", dict())

        self.device.controllers = list()

        for name, params in controllers.items():
            try:
                controller = controller_types[params["type"]](self.device,
                                                              name)
                self.device.controllers.append(controller)
            except Exception as e:
                if type(e) == KeyError:
                    rospy.logerr("Unrecognized controller: " + params["type"])
                else:
                    rospy.logerr(str(type(e)) + str(e))

        for controller in self.device.controllers:
            controller.startup()

        print "\n==> ROS Arduino Bridge ready for action!"

        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            # Heartbeat/watchdog test for the serial connection
            try:
                # Update read counters
                self.device.diagnostics.reads += 1
                self.device.diagnostics.total_reads += 1
                self.device.serial_port.inWaiting()
                # Add this heartbeat to the frequency status diagnostic task
                self.device.diagnostics.freq_diag.tick()
                # Let the diagnostics updater know we're still alive
                self.device.diagnostics.watchdog = True
            except IOError:
                # Update error counter
                self.device.diagnostics.errors += 1
                rospy.loginfo(
                    "Lost serial connection. Waiting to reconnect...")
                # Let the diagnostics updater know that we're down
                self.device.diagnostics.watchdog = False
                self.device.close()
                with mutex:
                    while True:
                        try:
                            self.device.open()
                            while True:
                                self.device.serial_port.write('\r')
                                test = self.device.serial_port.readline(
                                ).strip('\n').strip('\r')
                                rospy.loginfo("Waking up serial port...")
                                if test == 'Invalid Command':
                                    self.device.serial_port.flushInput()
                                    self.device.serial_port.flushOutput()
                                    break
                            rospy.loginfo("Serial connection re-established.")
                            break
                        except:
                            r.sleep()
                            self.diagnostics_publisher.update()
                            continue

            # Poll any sensors
            for sensor in self.device.sensors:
                if sensor.rate != 0:
                    with mutex:
                        sensor.poll()

            # Poll the base controller
            if self.use_base_controller:
                with mutex:
                    self.base_controller.poll()

            # Poll any joints
            if self.have_joints:
                with mutex:
                    self.servo_controller.poll()
                    self.joint_state_publisher.poll(
                        self.device.joints.values())

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.device.sensors)):
                    if self.device.sensors[i].message_type != MessageType.IMU:
                        msg.name.append(self.device.sensors[i].name)
                        msg.value.append(self.device.sensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            # Update diagnostics and publish
            self.diagnostics_publisher.update()

            r.sleep()
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)

        # Find the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 30))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        # Are we using the base_controller?
        self.use_base_controller = rospy.get_param("~use_base_controller", False)

        # Default error threshold (percent) before getting a diagnostics warning
        self.diagnotics_error_threshold = rospy.get_param("~diagnotics_error_threshold", 10)

        # Diagnostics update rate
        self.diagnotics_rate = rospy.get_param("~diagnotics_rate", 1.0)

        # Assume we don't have any joints by default
        self.have_joints = False

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=5)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)

        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)

        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)

        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)

        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # Initialize the device
        self.device = Arduino(self.port, self.baud, self.timeout, self.motors_reversed, debug=True)

        # Make the connection
        if self.device.connect():
            rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
        else:
            rospy.logerr("No serial connection found.")
            os._exit(0)

        # Initialize the base controller if used
        if self.use_base_controller:
            self.base_controller = BaseController(self.device, self.base_frame, self.name + "_base_controller")

            # A service to reset the odometry values to 0
            rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)

            # A service to update the PID parameters Kp, Kd, Ki, and Ko
            rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)

            # Fire up the dynamic_reconfigure server
            dyn_server = dynamic_reconfigure.server.Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback, namespace=self.name)

            # Connect to the dynamic_reconfigure client
            dyn_client = dynamic_reconfigure.client.Client (self.name, timeout=5)

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.device.sensors = list()

        # Keep a list of IMUs or gyros used for odometry so they can be reset
        self.imu_for_odom = list()

        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))

        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower() or params['type'].lower() == 'GP2Y0A21YK0F'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'AnalogFloat'.lower():
                sensor = AnalogFloatSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
            elif params['type'].lower() == 'IMU'.lower():
                sensor = IMU(self.device, name, **params)
                try:
                    if params['use_for_odom']:
                        self.imu_for_odom.append(sensor)
                except:
                    pass
            elif params['type'].lower() == 'Gyro'.lower():
                try:
                    sensor = Gyro(self.device, name, base_controller=self.base_controller, **params)
                except:
                    sensor = Gyro(self.device, name, **params)

                try:
                    if params['use_for_odom']:
                        self.imu_for_odom.append(sensor)
                except:
                    pass

#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            try:
                self.device.sensors.append(sensor)

                if params['rate'] != None and params['rate'] != 0:
                    rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
                else:
                    if sensor.direction == "input":
                        rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
                    else:
                        rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")
            except:
                rospy.logerr("Sensor type " + str(params['type'] + " not recognized."))

        # If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller
        if self.use_base_controller and len(self.imu_for_odom) > 0:
            self.base_controller.use_imu_heading = True

        # Read in the joints (if any)
        joint_params = rospy.get_param("~joints", dict())

        if len(joint_params) != 0:
            self.have_joints = True

            self.device.joints = dict()

            self.device.joint_update_rate = float(rospy.get_param("~joint_update_rate", 10))

            # Configure each servo
            for name, params in joint_params.iteritems():
                try:
                    if params['continuous'] == True:
                        self.device.joints[name] = ContinuousServo(self.device, name)
                    else:
                        self.device.joints[name] = Servo(self.device, name)
                except:
                    self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))

            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device, "ServoController")

            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()

        # Create the diagnostics updater for the Arduino device
        self.device.diagnostics = DiagnosticsUpdater(self, self.name, self.diagnotics_error_threshold, self.diagnotics_rate, create_watchdog=True)

        # Create the overall diagnostics publisher
        self.diagnostics_publisher = DiagnosticsPublisher(self)

        # Initialize any trajectory action follow controllers
        controllers = rospy.get_param("~controllers", dict())

        self.device.controllers = list()

        for name, params in controllers.items():
            try:
                controller = controller_types[params["type"]](self.device, name)
                self.device.controllers.append(controller)
            except Exception as e:
                if type(e) == KeyError:
                    rospy.logerr("Unrecognized controller: " + params["type"])
                else:
                    rospy.logerr(str(type(e)) + str(e))

        for controller in self.device.controllers:
            controller.startup()

        print "\n==> ROS Arduino Bridge ready for action!"

        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            # Heartbeat/watchdog test for the serial connection
            try:
                # Update read counters
                self.device.diagnostics.reads += 1
                self.device.diagnostics.total_reads += 1
                self.device.serial_port.inWaiting()
                # Add this heartbeat to the frequency status diagnostic task
                self.device.diagnostics.freq_diag.tick()
                # Let the diagnostics updater know we're still alive
                self.device.diagnostics.watchdog = True
            except IOError:
                # Update error counter
                self.device.diagnostics.errors += 1
                rospy.loginfo("Lost serial connection. Waiting to reconnect...")
                # Let the diagnostics updater know that we're down
                self.device.diagnostics.watchdog = False
                self.device.close()
                with mutex:
                    while True:
                        try:
                            self.device.open()
                            while True:
                                self.device.serial_port.write('\r')
                                test = self.device.serial_port.readline().strip('\n').strip('\r')
                                rospy.loginfo("Waking up serial port...")
                                if test == 'Invalid Command':
                                    self.device.serial_port.flushInput()
                                    self.device.serial_port.flushOutput()
                                    break
                            rospy.loginfo("Serial connection re-established.")
                            break
                        except:
                            r.sleep()
                            self.diagnostics_publisher.update()
                            continue

            # Poll any sensors
            for sensor in self.device.sensors:
                if sensor.rate != 0:
                    with mutex:
                        sensor.poll()

            # Poll the base controller
            if self.use_base_controller:
                with mutex:
                    self.base_controller.poll()

            # Poll any joints
            if self.have_joints:
                with mutex:
                    self.servo_controller.poll()
                    self.joint_state_publisher.poll(self.device.joints.values())

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.device.sensors)):
                    if self.device.sensors[i].message_type != MessageType.IMU:
                        msg.name.append(self.device.sensors[i].name)
                        msg.value.append(self.device.sensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            # Update diagnostics and publish
            self.diagnostics_publisher.update()

            r.sleep()
class ArduinoROS():
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        self.l_scale = rospy.get_param("~linear_scale", 10)
        self.a_scale = rospy.get_param("~angular_scale", 90)

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        rospy.Subscriber("html_joy_stick", Joy, self.JoystickCallback)
        
        # Initialize a 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, queue_size=10)
       
        # A pan_tilt publisher so we can control the position of the pan/tilt gimble and center it on shutdown
        self.pan_tilt_pub = rospy.Publisher('pan_tilt', PanTilt, queue_size=10)
 
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=10)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
       
        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # Initialize the controller
        self.controller = None
        driver = rospy.get_param("~arduino_driver", "rab_driver")
        params = { 'pid rate' : rospy.get_param("~base_controller_rate", 10) }
        if driver == "rab_driver":
            from ros_arduino_python.rab_driver import RABDriver
            self.controller = RABDriver(params=params, port=self.port, baud=self.baud, timeout=self.timeout)
        elif driver == "pymata_driver":
            from ros_arduino_python.pymata_driver import PymataDriver
            self.controller = PymataDriver(params=params, port=self.port, baud=self.baud, timeout=self.timeout)

        else:
            rospy.loginfo("Unknown Arduino driver")
            return
        
        # Make the connection
        self.controller.connect()
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()
        
        sensor_params = rospy.get_param("~sensors", dict({}))
        
        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'
                
            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'DigitalArray':
                sensor = LineSensor(self.controller, name, params['pins'], params['pullup'], self.base_frame, rate=params['rate'], direction=params['direction'])
                
#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            rospy.loginfo(name + " " + str(params))
              
        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller, self.base_frame)
    
        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
    
    # Service callback functions
    def ServoWriteHandler(self, req):
        self.controller.servo_write(req.id, req.value)
        return ServoWriteResponse()
    
    def ServoReadHandler(self, req):
        self.controller.servo_read(req.id)
        return ServoReadResponse()
    
    def DigitalSetDirectionHandler(self, req):
        self.controller.pin_mode(req.pin, req.direction)
        return DigitalSetDirectionResponse()
    
    def DigitalWriteHandler(self, req):
        self.controller.digital_write(req.pin, req.value)
        return DigitalWriteResponse()
              
    def AnalogWriteHandler(self, req):
        self.controller.analog_write(req.pin, req.value)
        return AnalogWriteResponse()
 
    def JoystickCallback(self, data):
        #rospy.loginfo("Received joystick msg: " + str(data))
        
        # axes 0 and 1 are assigned to the drive train
        # axes 2 and 3 are assigned to the pan/tilt
        
        # Handle motor drive/turning
        twist = Twist()
        twist.linear.x = self.l_scale*data.axes[0] # forward/backward speed
        twist.linear.y = 0
        twist.linear.z = 0        
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = self.a_scale*data.axes[1] # turning speed
        self.cmd_vel_pub.publish(twist)

        # Handle pan/tilt
        # The value received on axes 3 and 4 ranges from -1.0 to 1.0
        pan_tilt = PanTilt()
        pan_tilt.pan = data.axes[2]
        pan_tilt.tilt = data.axes[3]
        #rospy.loginfo("Publishing: pan %f tilt %f" % (pan_tilt.pan, pan_tilt.tilt))
        self.pan_tilt_pub.publish(pan_tilt) 
        
    
    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 Arduino Node...")
class ArduinoROS():
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Assume we don't have any joints by default
        self.have_joints = False
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        # Initialize a 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, queue_size=5)
        
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
        
        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
        
        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
        
        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
        
        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
        
        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # A service to reset the odometry values to 0
        rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)

        # A service to update the PID parameters Kp, Kd, Ki, and Ko
        rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)

        # Fire up the dynamic_reconfigure server
        dyn_server = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback)

        # Connect to the dynamic_reconfigure client
        dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5)

        # Initialize the device
        self.device = Arduino(self.port, self.baud, self.timeout)

        # Make the connection
        self.device.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.sensors = list()

        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))

        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
            elif params['type'].lower() == 'IMU'.lower():
                sensor = IMU(self.device, name, **params)
                
#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.sensors.append(sensor)
            
            if params['rate'] != None and params['rate'] != 0:
                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
            else:
                if sensor.direction == "input":
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
                else:
                    rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")

            
        # Initialize any joints (servos)
        self.device.joints = dict()
        
        # Read in the joints (if any)    
        joint_params = rospy.get_param("~joints", dict())
        
        if len(joint_params) != 0:
            self.have_joints = True
            
            # Configure each servo
            for name, params in joint_params.iteritems():
                try:
                    if params['continuous'] == True:
                        self.device.joints[name] = ContinuousServo(self.device, name)
                    else:
                        self.device.joints[name] = Servo(self.device, name)        
                except:
                    self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))
            
            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device, "ServoController")
            
            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()
            
#             # Initialize any trajectory action follow controllers
#             controllers = rospy.get_param("~controllers", dict())
#              
#             self.device.controllers = list()
#              
#             for name, params in controllers.items():
#                 try:
#                     controller = controller_types[params["type"]](self.device, name)
#                     self.device.controllers.append(controller)
#                 except Exception as e:
#                     if type(e) == KeyError:
#                         rospy.logerr("Unrecognized controller: " + params["type"])
#                     else:  
#                         rospy.logerr(str(type(e)) + str(e))
#      
#             for controller in self.device.controllers:
#                 controller.startup()

        # Initialize the base controller if used
        if self.use_base_controller:
            self.base_controller = BaseController(self.device, self.base_frame)
            
        print "\n==> ROS Arduino Bridge ready for action!"
    
        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            # Heartbeat test for the serial connection
            try:
                self.device.serial_port.inWaiting()
            except IOError:
                rospy.loginfo("Lost serial connection. Waiting to reconnect...")
                self.device.close()
                while True:
                    try:
                        self.device.open()
                        rospy.loginfo("Serial connection re-established.")
                        break
                    except:
                        r.sleep()
                        continue

            for sensor in self.sensors:
                if sensor.rate != 0:
                    mutex.acquire()
                    sensor.poll()
                    mutex.release()                  
                    
            if self.use_base_controller:
                mutex.acquire()
                self.base_controller.poll()
                mutex.release()
                
            if self.have_joints:
                mutex.acquire()
                self.servo_controller.poll()
                self.joint_state_publisher.poll(self.device.joints.values())
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.sensors)):
                    if self.sensors[i].message_type != MessageType.IMU:
                        msg.name.append(self.sensors[i].name)
                        msg.value.append(self.sensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
    
    # Service callback functions
    def ServoAttachHandler(self, req):
        self.device.attach_servo(req.id)
        return ServoAttachResponse()
    
    def ServoDetachHandler(self, req):
        self.device.detach_servo(req.id)
        return ServoDetachResponse()
    
    def ServoWriteHandler(self, req):
        self.device.servo_write(req.id, req.position)
        return ServoWriteResponse()
    
#     def ServoSpeedWriteHandler(self, req):
#         delay = 
#         self.device.servo_delay(req.id, delay)
#         return ServoSpeedResponse()
# 
#         # Convert servo speed in deg/s to a step delay in milliseconds
#         step_delay = self.device.joints[name].get_step_delay(req.value)
# 
#         # Update the servo speed
#         self.device.config_servo(pin, step_delay)
#         
#         return SetServoSpeedResponse()
    
    def ServoReadHandler(self, req):
        position = self.device.servo_read(req.id)
        return ServoReadResponse(position)
    
    def DigitalPinModeHandler(self, req):
        self.device.digital_pin_mode(req.pin, req.direction)
        return DigitalPinModeResponse()
    
    # Obsolete: use DigitalPinMode instead
    def DigitalSetDirectionHandler(self, req):
        self.device.digital_pin_mode(req.pin, req.direction)
        return DigitalSetDirectionResponse()
    
    def DigitalWriteHandler(self, req):
        self.device.digital_write(req.pin, req.value)
        return DigitalWriteResponse()
    
    def DigitalReadHandler(self, req):
        value = self.device.digital_read(req.pin)
        return DigitalReadResponse(value)
    
    def AnalogPinModeHandler(self, req):
        self.device.analog_pin_mode(req.pin, req.direction)
        return AnalogPinModeResponse()
              
    def AnalogWriteHandler(self, req):
        self.device.analog_write(req.pin, req.value)
        return AnalogWriteResponse()
    
    def AnalogReadHandler(self, req):
        value = self.device.analog_read(req.pin)
        return AnalogReadResponse(value)

    def ResetOdometryHandler(self, req):
        if self.use_base_controller:
            self.base_controller.reset_odometry()
        else:
            rospy.logwarn("Not using base controller!")
        return EmptyResponse()

    def UpdatePIDHandler(self, req):
        if self.use_base_controller:
           self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko)
           rospy.loginfo("Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f" %(req.Kp, req.Kd, req.Ki, req.Ko))
        else:
            rospy.logwarn("Not using base controller!")
        return UpdatePIDResponse()

    def dynamic_reconfigure_server_callback(self, config, level):
        if self.use_base_controller:
            try:
                if self.base_controller.Kp != config['Kp']:
                    self.base_controller.Kp = config['Kp']

                if self.base_controller.Kd != config['Kd']:
                    self.base_controller.Kd = config['Kd']

                if self.base_controller.Ki != config['Ki']:
                    self.base_controller.Ki = config['Ki']

                if self.base_controller.Ko != config['Ko']:
                    self.base_controller.Ko = config['Ko']

                if self.base_controller.accel_limit != config['accel_limit']:
                    self.base_controller.accel_limit = config['accel_limit']
                    self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate

                self.device.update_pid(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko)

                rospy.loginfo("Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f" %(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko, self.base_controller.accel_limit))
            except Exception as e:
                print e

        return config
        
    def shutdown(self):
        rospy.loginfo("Shutting down Arduino node...")

        # Stop the robot
        if self.use_base_controller:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.publish(Twist())
            rospy.sleep(2)

        # Detach any servos
        if self.have_joints:
            rospy.loginfo("Detaching servos...")
            for joint in self.device.joints.values():
                self.device.detach_servo(joint.pin)
                rospy.sleep(0.2)
Exemple #13
0
    def __init__(self):
        # 初始化节点 设置 日志 等级
        rospy.init_node('arduino', log_level=rospy.INFO)

        # 得到实际的节点名称 (参数设置文件可能会更改名字)
        self.name = rospy.get_name()

        # 关闭时清理节点
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")  # 端口
        self.baud = int(rospy.get_param("~baud", 57600))  # 波特率
        self.timeout = rospy.get_param("~timeout", 0.5)  # 等待时间
        self.base_frame = rospy.get_param("~base_frame",
                                          'base_link')  # 设置参考坐标系(里程记)

        # 主循环 的频率 要高于 传感器的 检测频率
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # 传感器信息发布频率
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        # 是否使用 ros内置的 基础控制器
        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # 设置下一次发布传感器信息的时间
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # 初始化 姿态信息(线速度、角速度)
        self.cmd_vel = Twist()

        # 发布速度指令消息到 cmd_vel话题
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # 发布传感器信息(值)到sensor_state话题
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # 读取的一些服务
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # 初始化ARDUINO控制器
        self.controller = Arduino(self.port, self.baud, self.timeout)
        # 连接
        self.controller.connect()
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # 创建一个锁对象LockType,使多个线程同步访问共享资源。  多线程
        mutex = thread.allocate_lock()

        # 初始化传感器列表
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":  # ping传感器?
                sensor = Ping(self.controller, name, params['pin'],
                              params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       self.base_frame,
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      self.base_frame,
                                      direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'],
                                            self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)


# 超声波传感器
#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
            try:
                self.mySensors.append(sensor)
                rospy.loginfo(name + " " + str(params) +
                              " published on topic " + rospy.get_name() +
                              "/sensor/" + name)
            except:
                rospy.logerr("Sensor type " + str(params['type']) +
                             " not recognized.")

        # 初始化基本控制器
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame,
                self.name + "_base_controller")

        # 发布传感器信息 以及 下发速度等控制命令
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()  #开启多线程
                sensor.poll()  #
                mutex.release()  # 关闭多线程

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll(
                )  # 得到轮子当前的速度,发布里程记信息到 话题上,获取cmd_cel话题上的速度命令解析后得到轮子的速度,简单单限值处理后下发到下位机
                mutex.release()

            # 在 ~sensor_state 话题上发布所有使用的传感器的数据信息
            now = rospy.Time.now()
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)  #列表
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)  # 发布传感器信息
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
Exemple #14
0
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)

        # Get the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        if rospy.get_param("~override_arduino_port") not in ['false', False]:
            self.port = rospy.get_param("~override_arduino_port")
        else:
            self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.robot_id = rospy.get_namespace().replace('/', '')
        self.base_frame = rospy.get_param("~base_frame", 'base_link')
        self.base_frame = '{}_{}'.format(self.robot_id, self.base_frame)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published.
        # Individual sensors publish at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param(
            "~use_base_controller", False
        )

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=5)

        # The SensorState publisher periodically publishes the values
        # of all sensors on a single topic.
        self.sensorStatePub = rospy.Publisher(
            '~sensor_state', SensorState, queue_size=5
        )

        # A service to position a PWM servo
        rospy.Service(
            '~servo_write', ServoWrite, self.ServoWriteHandler
        )

        # A service to read the position of a PWM servo
        rospy.Service(
            '~servo_read', ServoRead, self.ServoReadHandler
        )

        # A service to turn set the direction of a digital pin
        # (0 = input, 1 = output)
        rospy.Service(
            '~digital_set_direction', DigitalSetDirection,
            self.DigitalSetDirectionHandler
        )

        # A service to turn a digital sensor on or off
        rospy.Service(
            '~digital_write', DigitalWrite, self.DigitalWriteHandler
        )

        # A service to read the value of a digital sensor
        rospy.Service(
            '~digital_read', DigitalRead, self.DigitalReadHandler
        )

        # A service to set pwm values for the pins
        rospy.Service(
            '~analog_write', AnalogWrite, self.AnalogWriteHandler
        )

        # A service to read the value of an analog sensor
        rospy.Service(
            '~analog_read', AnalogRead, self.AnalogReadHandler
        )

        # Initialize the controlller
        self.controller = Arduino(
            self.port, self.baud, self.timeout, self.motors_reversed
        )

        # Make the connection
        self.controller.connect()

        rospy.loginfo(
            "Connected to Arduino on port {} at {} baud".format(
                self.port, self.baud
            )
        )

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))
        sensor_funcs = {
            'Ping': Ping,
            'GP2D12': GP2D12,
            'Digital': DigitalSensor,
            'Analog': AnalogSensor,
            'PololuMotorCurrent': PololuMotorCurrent,
            'PhidgetsVoltage': PhidgetsVoltage,
            'PhidgetsCurrent': PhidgetsCurrent,
        }

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'
            sensor = sensor_funcs[params['type']](
                self.controller, name, params['pin'], params['rate'],
                self.base_frame, direction=params['direction']
            )

            try:
                self.mySensors.append(sensor)
                rospy.loginfo(
                    "Sensor {}[{}] publishing on topic {}/sensor/{}".format(
                        name, params["type"], rospy.get_name(), name
                    )
                )
            except:
                rospy.logerr(
                    "Sensor type {} not recognized.".format(params['type'])
                )

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame, 
                "{}_base_controller".format(self.name)
            )

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
    def __init__(self):
        rospy.init_node('Arduino', log_level=rospy.DEBUG)
                
        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)
        
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.        
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)
        
        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors
        
        # Initialize a 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)
        
        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
        
        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
        
        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
        
        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
        
        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
       
	# A service to set pwm values for the pins
	rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

	# Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout)
        
        # Make the connection
        self.controller.connect()
        
        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
     
        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()
        
        sensor_params = rospy.get_param("~sensors", dict({}))
        
        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'
                
            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
                
#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.mySensors.append(sensor)
            rospy.loginfo(name + " " + str(params))
              
        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(self.controller, self.base_frame)
    
        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()
                    
            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()
            
            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()
            
            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass
                
                self.t_next_sensors = now + self.t_delta_sensors
            
            r.sleep()
Exemple #16
0
    def __init__(self):
        rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)

        # Get the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.7)
        self.base_frame = rospy.get_param("~base_frame", 'base_footprint')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 20))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   True)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=20)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # A service to set alarm sensor
        rospy.Service('~alarm_write', AlarmWrite, self.AlarmWriteHandler)

        # A service to set light show
        rospy.Service('~light_show', LightShow, self.LightShowHandler)

        # Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout)

        # Make the connection
        self.controller.connect()
        self.controller.alarm_write(1)
        self.controller.light_show(1)

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.iteritems():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'],
                              params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'], self.base_frame)
            elif params['type'] == "IR2Y0A02":
                sensor = IR2Y0A02(self.controller, name, params['pin'],
                                  params['rate'], self.base_frame)
            elif params['type'] == "GP2Y0A41":
                sensor = GP2Y0A41(self.controller, name, params['pin'],
                                  params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       self.base_frame,
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      self.base_frame,
                                      direction=params['direction'])
            elif params['type'] == 'Voltage':
                sensor = VoltageSensor(self.controller, name, params['pin'],
                                       params['rate'], self.base_frame)
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'],
                                            self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'MotorTotalCurrent':
                sensor = MotorTotalCurrent(self.controller, name,
                                           params['pin'], params['rate'],
                                           self.base_frame)

            try:
                self.mySensors.append(sensor)
                rospy.loginfo(name + " " + str(params) +
                              " published on topic " + rospy.get_name() +
                              "/sensor/" + name)
            except:
                rospy.logerr("Sensor type " + str(params['type']) +
                             " not recognized.")

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame,
                self.name + "_base_controller")

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
Exemple #17
0
class ArduinoROS():
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.DEBUG)

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')

        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Assume we don't have any joints by default
        self.have_joints = False

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a 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, queue_size=5)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # A service to attach a PWM servo to a specified pin
        rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)

        # A service to detach a PWM servo to a specified pin
        rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_pin_mode', DigitalPinMode,
                      self.DigitalPinModeHandler)

        # Obsolete: Use DigitalPinMode instead
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn set the direction of an analog pin (0 = input, 1 = output)
        rospy.Service('~analog_pin_mode', AnalogPinMode,
                      self.AnalogPinModeHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # A service to reset the odometry values to 0
        rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)

        # A service to update the PID parameters Kp, Kd, Ki, and Ko
        rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)

        # Fire up the dynamic_reconfigure server
        dyn_server = Server(ROSArduinoBridgeConfig,
                            self.dynamic_reconfigure_server_callback)

        # Connect to the dynamic_reconfigure client
        dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5)

        # Initialize the device
        self.device = Arduino(self.port, self.baud, self.timeout)

        # Make the connection
        self.device.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = thread.allocate_lock()

        # Initialize any sensors
        self.sensors = list()

        # Read in the sensors parameter dictionary
        sensor_params = rospy.get_param("~sensors", dict({}))

        # Initialize individual sensors appropriately
        for name, params in sensor_params.iteritems():
            if params['type'].lower() == 'Ping'.lower():
                sensor = Ping(self.device, name, **params)
            elif params['type'].lower() == 'GP2D12'.lower():
                sensor = GP2D12(self.device, name, **params)
            elif params['type'].lower() == 'Digital'.lower():
                sensor = DigitalSensor(self.device, name, **params)
            elif params['type'].lower() == 'Analog'.lower():
                sensor = AnalogSensor(self.device, name, **params)
            elif params['type'].lower() == 'PololuMotorCurrent'.lower():
                sensor = PololuMotorCurrent(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsVoltage'.lower():
                sensor = PhidgetsVoltage(self.device, name, **params)
            elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                sensor = PhidgetsCurrent(self.device, name, **params)
            elif params['type'].lower() == 'IMU'.lower():
                sensor = IMU(self.device, name, **params)

#                if params['type'].lower() == 'MaxEZ1'.lower():
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']

            self.sensors.append(sensor)

            if params['rate'] != None and params['rate'] != 0:
                rospy.loginfo(name + " " + str(params) +
                              " published on topic " + rospy.get_name() +
                              "/sensor/" + name)
            else:
                if sensor.direction == "input":
                    rospy.loginfo(name + " service ready at " +
                                  rospy.get_name() + "/" + name + "/read")
                else:
                    rospy.loginfo(name + " service ready at " +
                                  rospy.get_name() + "/" + name + "/write")

        # Initialize any joints (servos)
        self.device.joints = dict()

        # Read in the joints (if any)
        joint_params = rospy.get_param("~joints", dict())

        if len(joint_params) != 0:
            self.have_joints = True

            # Configure each servo
            for name, params in joint_params.iteritems():
                try:
                    if params['continuous'] == True:
                        self.device.joints[name] = ContinuousServo(
                            self.device, name)
                    else:
                        self.device.joints[name] = Servo(self.device, name)
                except:
                    self.device.joints[name] = Servo(self.device, name)

                # Display the joint setup on the terminal
                rospy.loginfo(name + " " + str(params))

            # The servo controller determines when to read and write position values to the servos
            self.servo_controller = ServoController(self.device,
                                                    "ServoController")

            # The joint state publisher publishes the latest joint values on the /joint_states topic
            self.joint_state_publisher = JointStatePublisher()

#             # Initialize any trajectory action follow controllers
#             controllers = rospy.get_param("~controllers", dict())
#
#             self.device.controllers = list()
#
#             for name, params in controllers.items():
#                 try:
#                     controller = controller_types[params["type"]](self.device, name)
#                     self.device.controllers.append(controller)
#                 except Exception as e:
#                     if type(e) == KeyError:
#                         rospy.logerr("Unrecognized controller: " + params["type"])
#                     else:
#                         rospy.logerr(str(type(e)) + str(e))
#
#             for controller in self.device.controllers:
#                 controller.startup()

# Initialize the base controller if used
        if self.use_base_controller:
            self.base_controller = BaseController(self.device, self.base_frame)

        print "\n==> ROS Arduino Bridge ready for action!"

        # Start polling the sensors, base controller, and servo controller
        while not rospy.is_shutdown():
            # Heartbeat test for the serial connection
            try:
                self.device.serial_port.inWaiting()
            except IOError:
                rospy.loginfo(
                    "Lost serial connection. Waiting to reconnect...")
                self.device.close()
                while True:
                    try:
                        self.device.open()
                        rospy.loginfo("Serial connection re-established.")
                        break
                    except:
                        r.sleep()
                        continue

            for sensor in self.sensors:
                if sensor.rate != 0:
                    mutex.acquire()
                    sensor.poll()
                    mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.base_controller.poll()
                mutex.release()

            if self.have_joints:
                mutex.acquire()
                self.servo_controller.poll()
                self.joint_state_publisher.poll(self.device.joints.values())
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.sensors)):
                    if self.sensors[i].message_type != MessageType.IMU:
                        msg.name.append(self.sensors[i].name)
                        msg.value.append(self.sensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()

    # Service callback functions
    def ServoAttachHandler(self, req):
        self.device.attach_servo(req.id)
        return ServoAttachResponse()

    def ServoDetachHandler(self, req):
        self.device.detach_servo(req.id)
        return ServoDetachResponse()

    def ServoWriteHandler(self, req):
        self.device.servo_write(req.id, req.position)
        return ServoWriteResponse()


#     def ServoSpeedWriteHandler(self, req):
#         delay =
#         self.device.servo_delay(req.id, delay)
#         return ServoSpeedResponse()
#
#         # Convert servo speed in deg/s to a step delay in milliseconds
#         step_delay = self.device.joints[name].get_step_delay(req.value)
#
#         # Update the servo speed
#         self.device.config_servo(pin, step_delay)
#
#         return SetServoSpeedResponse()

    def ServoReadHandler(self, req):
        position = self.device.servo_read(req.id)
        return ServoReadResponse(position)

    def DigitalPinModeHandler(self, req):
        self.device.digital_pin_mode(req.pin, req.direction)
        return DigitalPinModeResponse()

    # Obsolete: use DigitalPinMode instead
    def DigitalSetDirectionHandler(self, req):
        self.device.digital_pin_mode(req.pin, req.direction)
        return DigitalSetDirectionResponse()

    def DigitalWriteHandler(self, req):
        self.device.digital_write(req.pin, req.value)
        return DigitalWriteResponse()

    def DigitalReadHandler(self, req):
        value = self.device.digital_read(req.pin)
        return DigitalReadResponse(value)

    def AnalogPinModeHandler(self, req):
        self.device.analog_pin_mode(req.pin, req.direction)
        return AnalogPinModeResponse()

    def AnalogWriteHandler(self, req):
        self.device.analog_write(req.pin, req.value)
        return AnalogWriteResponse()

    def AnalogReadHandler(self, req):
        value = self.device.analog_read(req.pin)
        return AnalogReadResponse(value)

    def ResetOdometryHandler(self, req):
        if self.use_base_controller:
            self.base_controller.reset_odometry()
        else:
            rospy.logwarn("Not using base controller!")
        return EmptyResponse()

    def UpdatePIDHandler(self, req):
        if self.use_base_controller:
            self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko)
            rospy.loginfo(
                "Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f"
                % (req.Kp, req.Kd, req.Ki, req.Ko))
        else:
            rospy.logwarn("Not using base controller!")
        return UpdatePIDResponse()

    def dynamic_reconfigure_server_callback(self, config, level):
        if self.use_base_controller:
            try:
                if self.base_controller.Kp != config['Kp']:
                    self.base_controller.Kp = config['Kp']

                if self.base_controller.Kd != config['Kd']:
                    self.base_controller.Kd = config['Kd']

                if self.base_controller.Ki != config['Ki']:
                    self.base_controller.Ki = config['Ki']

                if self.base_controller.Ko != config['Ko']:
                    self.base_controller.Ko = config['Ko']

                if self.base_controller.accel_limit != config['accel_limit']:
                    self.base_controller.accel_limit = config['accel_limit']
                    self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate

                self.device.update_pid(self.base_controller.Kp,
                                       self.base_controller.Kd,
                                       self.base_controller.Ki,
                                       self.base_controller.Ko)

                rospy.loginfo(
                    "Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f"
                    % (self.base_controller.Kp, self.base_controller.Kd,
                       self.base_controller.Ki, self.base_controller.Ko,
                       self.base_controller.accel_limit))
            except Exception as e:
                print e

        return config

    def shutdown(self):
        rospy.loginfo("Shutting down Arduino node...")

        # Stop the robot
        if self.use_base_controller:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.publish(Twist())
            rospy.sleep(2)

        # Detach any servos
        if self.have_joints:
            rospy.loginfo("Detaching servos...")
            for joint in self.device.joints.values():
                self.device.detach_servo(joint.pin)
                rospy.sleep(0.2)