extract_message.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # *****************************************************************************
5 # *
6 # extract_message.py *
7 # *
8 # *****************************************************************************
9 # *
10 # github repository *
11 # https://github.com/TUC-ProAut/radar_pa *
12 # *
13 # Chair of Automation Technology, Technische Universität Chemnitz *
14 # https://www.tu-chemnitz.de/etit/proaut *
15 # *
16 # *****************************************************************************
17 # *
18 # BSD 3-Clause License *
19 # *
20 # Copyright (c) 2018-2019, Karim Haggag, Technische Universität Chemnitz *
21 # All rights reserved. *
22 # *
23 # Redistribution and use in source and binary forms, with or without *
24 # modification, are permitted provided that the following conditions are met: *
25 # * Redistributions of source code must retain the above copyright *
26 # notice, this list of conditions and the following disclaimer. *
27 # * Redistributions in binary form must reproduce the above copyright *
28 # notice, this list of conditions and the following disclaimer in the *
29 # documentation and/or other materials provided with the distribution. *
30 # * Neither the names of the copyright holders nor the names of its *
31 # contributors may be used to endorse or promote products *
32 # derived from this software without specific prior written permission. *
33 # *
34 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
35 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
36 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
37 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE *
38 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR *
39 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF *
40 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS *
41 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN *
42 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) *
43 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE *
44 # POSSIBILITY OF SUCH DAMAGE. *
45 # *
46 # *****************************************************************************
47 # *
48 # Revision 1 *
49 # *
50 # *****************************************************************************
51 # *
52 # This file converts the given data from the can messages to radar_msgs. *
53 # *
54 # Supported radars: *
55 # * Bosch GPR v1.0 *
56 # *
57 # *****************************************************************************
58 
59 # -- standard modules
60 import sys
61 
62 # -- ros modules
63 import rospy
64 from radar_pa_msgs.msg import radar_msg_A
65 from radar_pa_msgs.msg import radar_msg_B
66 
67 def extract_GPR_v10(received_data): # (): # -- to test the function
68 
69  # -- use for test the function , function argument must removed
70  #received_data = [128, 255,7, 128, 85, 0, 192, 244] # only for test
71  # -- received_data have 8-bit string so using ord return integer
72  data = data_ord(received_data)
73 
74  # -- depend on the Amessage to know if it A or B message S
75  if ((data[0] & 0b00000001) == 0b00000001):
76  # -- message A
77  msg = radar_msg_A()
78  msg.message = extract_bit (0, 1, data)
79  msg.ID = extract_bit (1, 6, data)
80  msg.distance = (extract_bit(7, 12, data)) * 0.0625 # unit m
81  msg.velocity = (twos_complement( extract_bit(19, 12, data), 12)) * 0.0625 # unit m/s
82  msg.power = (twos_complement( extract_bit(31, 8, data), 8)) * 0.5 # unit dBm2
83  msg.angle = (twos_complement( extract_bit(39, 14, data), 14)) * 0.0002 # rad
84  msg.is_target = extract_bit(53, 1, data)
85  msg.counter = extract_bit(54, 2, data)
86 
87  else:
88  # -- message B
89  msg = radar_msg_B()
90  msg.message = extract_bit( 0, 1, data)
91  msg.ID = extract_bit( 1, 6, data)
92  msg.angle_deviation = extract_bit( 7, 6, data) * 0.001 # rad
93  msg.velocity_deviation = extract_bit(13, 6, data) * 0.0625 # m/s
94  msg.distance_deviation = extract_bit(19, 6, data) * 0.0625 # m
95  msg.proability_target = extract_bit(25, 5, data) * 0.03125
96  #msg.time = extract_bit(30, 13, data) * 0.0001 # s
97  msg.counter = extract_bit(54, 2, data)
98 
99  return msg
100 
101 # ***************** extract_bit *****************
102 
103 def extract_bit(start, length, data):
104 
105  # -- get the byte number
106  byteNum = start / 8
107  #-- get the bit number
108  bitNum = start % 8
109 
110  if ((bitNum + length) <= 8):
111  fisrt_byte = data[byteNum] >>(bitNum) & mask_value(length)
112  new_data = fisrt_byte
113 
114  elif ((bitNum + length) <= 16):
115  fisrt_byte = data[byteNum] >>(bitNum) & mask_value(8-bitNum)
116  second_byte = data[byteNum+1] <<(8-bitNum) & mask_value(length)
117  new_data = fisrt_byte | second_byte
118 
119  elif ((bitNum + length) <= 24):
120  fisrt_byte = data[byteNum] >>(bitNum) & mask_value(8-bitNum)
121  second_byte = data[byteNum+1] <<(8-bitNum) & mask_value(8+bitNum)
122  third_byte = data[byteNum+2] <<(8+(8-bitNum)) & mask_value(length)
123  new_data = fisrt_byte | second_byte |third_byte
124 
125  else:
126  print ('out of range')
127 
128  return(new_data)
129 
130 # ***************** generate the maske value *****************
131 
132 def mask_value(length):
133 
134  # -- we can do this by using two methods
135  # -- method 1 with multipliaction or shift
136  # -- we can get the data by make it 2 ** (length+1 ) - 1
137  #print ('the value of mask is ',bin(((1<<length))-1))
138  # -- (2**(length) == (1<<length)
139  #return ((2**(length))-1)
140 
141  return ((1 << length)-1)
142 
143  # method 2 with for loop
144  #sum_1 = 0
145  #for i in range (0 ,x):
146  #sum_1 = sum_1 + 2**i
147  #print ('totl sum', sum_1 , bin(sum_1))
148  #return (sum_1)
149 
150 # ***************** convert the message from str to int *****************
151 
152 def data_ord(data):
153 
154  #result = [0] * len(data)
155  result = []
156  for i in range(len(data)):
157  if ((type(data[i]) is str)):
158  #result[i] = ord (data[i])
159  result.append(ord(data[i]))
160  else:
161  #result[i] = data[i]
162  result.append(data[i])
163 
164  return (result)
165 
166 #***************** two's complement function *****************
167 
168 def twos_complement(input_data, length):
169 
170  if (input_data & (1<<(length-1))):
171  input_data -= (1<<length)
172 
173  return (input_data)
174 
175 #***************** count number of bit *****************
176 
177 def bit_len(input_data):
178 
179  data_length = 0
180  while (input_data):
181  input_data >>= 1
182  data_length += 1
183 
184  return (data_length)
extract_message.twos_complement
def twos_complement(input_data, length)
Definition: extract_message.py:168
extract_message.bit_len
def bit_len(input_data)
Definition: extract_message.py:177
extract_message.extract_GPR_v10
def extract_GPR_v10(received_data)
Definition: extract_message.py:67
extract_message.data_ord
def data_ord(data)
Definition: extract_message.py:152
extract_message.extract_bit
def extract_bit(start, length, data)
Definition: extract_message.py:103
extract_message.mask_value
def mask_value(length)
Definition: extract_message.py:132

radar_pa
Author(s):
autogenerated on Fri Mar 19 2021 02:46:18