Source Code for Module create_node.create_sensor_handler

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2011, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of the Willow Garage nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32   
 33  import roslib.message 
 34  import struct 
 35  import logging 
 36  import time 
 37  import rospy 
 38   
 39  from math import radians 
 40  from create_driver import SENSOR_GROUP_PACKET_LENGTHS 
 41   
 42  #_struct_I = roslib.message.struct_I 
 43  _struct_BI = struct.Struct(">BI") 
 44  _struct_12B2hBHhb7HBH5B4h = struct.Struct(">12B2hBHhb7HBH5B4h") 
 45   
 46 -def deserialize(msg, buff, timestamp): 
 47      """ 
 48      unpack serialized message in str into this message instance 
 49      @param buff: byte array of serialized message 
 50      @type  buff: str 
 51      """ 
 52      try: 
 53          _x = msg 
 54          (_x.bumps_wheeldrops, _x.wall, _x.cliff_left, _x.cliff_front_left, _x.cliff_front_right, _x.cliff_right, _x.virtual_wall, _x.motor_overcurrents, _x.dirt_detector_left, _x.dirt_detector_right, _x.remote_opcode, _x.buttons, _x.distance, _x.angle, _x.charging_state, _x.voltage, _x.current, _x.temperature, _x.charge, _x.capacity, _x.wall_signal, _x.cliff_left_signal, _x.cliff_front_left_signal, _x.cliff_front_right_signal, _x.cliff_right_signal, _x.user_digital_inputs, _x.user_analog_input, _x.charging_sources_available, _x.oi_mode, _x.song_number, _x.song_playing, _x.number_of_stream_packets, _x.requested_velocity, _x.requested_radius, _x.requested_right_velocity, _x.requested_left_velocity,) = _struct_12B2hBHhb7HBH5B4h.unpack(buff[0:52]) 
 55   
 56          msg.wall = bool(msg.wall) 
 57          msg.cliff_left = bool(msg.cliff_left) 
 58          msg.cliff_front_left = bool(msg.cliff_front_left) 
 59          msg.cliff_front_right = bool(msg.cliff_front_right) 
 60          msg.cliff_right = bool(msg.cliff_right) 
 61          msg.virtual_wall = bool(msg.virtual_wall) 
 62          msg.song_playing = bool(msg.song_playing) 
 63   
 64          # do unit conversions 
 65          msg.angle = radians(msg.angle) 
 66          msg.header.stamp = rospy.Time.from_seconds(timestamp)         
 67          msg.distance     = float(msg.distance) / 1000. 
 68           
 69          msg.requested_velocity       = float(msg.requested_velocity) / 1000. 
 70          msg.requested_radius         = float(msg.requested_radius) / 1000. 
 71          msg.requested_right_velocity = float(msg.requested_right_velocity) / 1000. 
 72          msg.requested_left_velocity  = float(msg.requested_left_velocity) / 1000. 
 73   
 74          return msg 
 75      except struct.error, e: 
 76        raise roslib.message.DeserializationError(e) 
 77   
 78 -class CreateSensorHandler(object): 
 79       
 80 -    def __init__(self, robot): 
 81          self.robot = robot     
 82 -    def request_packet(self, packet_id): 
 83          """Reqeust a sensor packet.""" 
 84          with self.robot.sci.lock: 
 85              self.robot.sci.flush_input() 
 86              self.robot.sci.sensors(packet_id) 
 87              #kwc: there appears to be a 10-20ms latency between sending the 
 88              #sensor request and fully reading the packet.  Based on 
 89              #observation, we are preferring the 'before' stamp rather than 
 90              #after. 
 91              stamp = time.time() 
 92              length = SENSOR_GROUP_PACKET_LENGTHS[packet_id] 
 93              return self.robot.sci.read(length), stamp 
 94   
 95 -    def get_all(self, sensor_state): 
 96          buff, timestamp = self.request_packet(6) 
 97          if buff is not None: 
 98              deserialize(sensor_state, buff, timestamp) 
 99