Source Code for Module sick_ldmrs.utils

  1  #* Software License Agreement (BSD License) 
  2  #* 
  3  #*  Copyright (c) 2010, CSIRO Autonomous Systems Laboratory 
  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 CSIRO 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   
 34  # Author: Fred Pauling 
 35  #$Id$ 
 36   
 37  from __future__ import division 
 38   
 39  import roslib 
 40  roslib.load_manifest('sick_ldmrs') 
 41  import rospy 
 42   
 43  """ Some useful routines 
 44  """ 
 45   
 46  #-------------------------------------------------------- 
 47   
 48 -def NTP64_to_ROStime(NTPtime): 
 49      """ Convert a 64bit NTP timestamp to rostime.Time 
 50   
 51      @param NTPtime: 64bit NTP timestamp. high 32bits are seconds, 
 52      low 32bits are fractional seconds (*2^-31) 
 53      @type NTPtime: int 
 54      @return: rospy.Time with seconds and nanoseconds 
 55      fields initialised based on input 
 56      @rtype: rospy.Time 
 57      """ 
 58      seconds = int(NTPtime >> 32) 
 59      frac_seconds = NTPtime & 0xFFFFFFFF 
 60      # nanoseconds =  frac_seconds * 1x10^9      / 2^31 
 61      nanoseconds   = int(round((frac_seconds * 0x3b9aca00) / 0x100000000)) 
 62      return rospy.Time(seconds,  nanoseconds) 
 63   
 64 -def NTP64split_to_ROStime(seconds_part, fractional_part): 
 65      """ Convert seconds and fractional seconds to a rospy.Time object 
 66   
 67      @param seconds_part: high 32 bits of NTP64 timestamp 
 68      @type seconds_part: int 
 69      @param fractional_part: low 32 bits of NTP64 timestamp 
 70      @type fractional_part: int 
 71      @return: rospy.Time representation of the input 
 72      @rtype: rospy.Time 
 73      """ 
 74      NTPtime = seconds_part << 32 | fractional_part 
 75      return NTP64_to_ROStime(NTPtime) 
 76   
 77 -def ROStime_to_NTP64(ROStime): 
 78      """ Convert rospy.Time object to a 64bit NTP timestamp 
 79   
 80      @param ROStime: timestamp 
 81      @type ROStime: rospy.Time 
 82      @return: 64bit NTP representation of the input 
 83      @rtype: int 
 84      """ 
 85      NTPtime = ROStime.secs << 32 
 86      # NTPtime |= nanoseconds * 2^31 / 1x10^9 
 87      NTPtime |= int((ROStime.nsecs * 0x100000000) / 0x3b9aca00) 
 88      return NTPtime 
 89   
 90 -def Now_to_NTP64(): 
 91      """ Get 64bit NTP timestamp of current system time 
 92   
 93      @return: NTP representation of current system time 
 94      @rtype: int 
 95      """ 
 96      return ROStime_to_NTP64(rospy.Time.now()) 
 97   
 98 -def bitString(integral_value, min_bits=8): 
 99      """ Return a string representation of a binary value 
100   
101      Return a string representation of a binary value, padded with zeros in the 
102      higher order bits if necessary 
103      @param integral_value: integer or long integer to convert to bit string 
104      @type integral_value: int 
105      @param min_bits: minimum number of bits to display in the result 
106      (pad zeros to the left if necessary) 
107      @type min_bits: int 
108      @return: ascii string containing the binary representation of the 
109      of the input value 
110      @rtype: str 
111      """ 
112      bitstring = bin(integral_value).split('b')[1] 
113      pad_zeros = min_bits - len(bitstring) 
114      if pad_zeros > 0: 
115          bitstring = '0'*pad_zeros + bitstring 
116      return bitstring 
117