radar2pcd_pa_node.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # *****************************************************************************
5 # *
6 # radar2pcd_pa_node.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 node converts the radar_messages to PointCloud.msg *
53 # *
54 # Supported radars: *
55 # * Bosch GPR v1.0 *
56 # *
57 # *****************************************************************************
58 
59 # -- standard headers
60 import math
61 
62 # -- ros reahder
63 import rospy
64 import std_msgs.msg
65 from radar_pa_msgs.msg import radar_msg
66 from sensor_msgs.msg import PointCloud
67 from sensor_msgs.msg import ChannelFloat32
68 from geometry_msgs.msg import Point32
69 
70 class radar2pcdPaNode():
71 
72  def __init__(self):
73 
74  rospy.init_node('radar2pcd_pa_node.py', anonymous=True)
75  rospy.Subscriber('radar_messages', radar_msg, self.callback)
76 
77  self.pcpub = rospy.Publisher('radar_pcd', PointCloud, queue_size=10)
78 
79  # PointCloud
80  self.out_Pc = PointCloud()
81  self.out_Pc.header = std_msgs.msg.Header()
82  self.out_Pc.header.stamp = rospy.Time.now()
83  self.out_Pc.header.frame_id = "radar"
84 
85  num_targets = 48
86  # -- None here represent data type # this data type is point
87  self.out_Pc.points = [None] * num_targets
88 
89  # -- channels must be ChannelFloat32()
90  self.out_Pc.channels = [ChannelFloat32(), ChannelFloat32(), \
91  ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \
92  ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \
93  ChannelFloat32(), ChannelFloat32()]
94 
95  self.out_Pc.channels[0].values = [None] * num_targets
96  self.out_Pc.channels[1].values = [None] * num_targets
97  self.out_Pc.channels[2].values = [None] * num_targets
98  self.out_Pc.channels[3].values = [None] * num_targets
99  self.out_Pc.channels[4].values = [None] * num_targets
100  self.out_Pc.channels[5].values = [None] * num_targets
101  self.out_Pc.channels[6].values = [None] * num_targets
102  self.out_Pc.channels[7].values = [None] * num_targets
103  self.out_Pc.channels[8].values = [None] * num_targets
104  self.out_Pc.channels[9].values = [None] * num_targets
105 
106  self.out_Pc.channels[0].name = 'ID'
107  self.out_Pc.channels[1].name = 'distance'
108  self.out_Pc.channels[2].name = 'velocity'
109  self.out_Pc.channels[3].name = 'power'
110  self.out_Pc.channels[4].name = 'angle'
111  self.out_Pc.channels[5].name = 'distance_deviation'
112  self.out_Pc.channels[6].name = 'angle_deviation'
113  self.out_Pc.channels[7].name = 'velocity_deviation'
114  self.out_Pc.channels[8].name = 'proability_target'
115  self.out_Pc.channels[9].name = 'is_target'
116 
117  def callback(self, data):
118  self.out_Pc.header.stamp = data.header.stamp
119 
120  for i in range(len(self.out_Pc.points)):
121 
122 
123  # -- check if target is valid
124  if (data.data_A[i].is_target == 1):
125 
126  # -- compute each x,y for each point
127  x = data.data_A[i].distance * (math.cos(data.data_A[i].angle))
128  y = data.data_A[i].distance * (math.sin(data.data_A[i].angle))
129 
130  # -- fill the channels wit Id , distance,..
131  self.out_Pc.channels[0].values[i] = data.data_A[i].ID
132  self.out_Pc.channels[1].values[i] = data.data_A[i].distance
133  self.out_Pc.channels[2].values[i] = data.data_A[i].velocity
134  self.out_Pc.channels[3].values[i] = data.data_A[i].power
135  self.out_Pc.channels[4].values[i] = data.data_A[i].angle
136  self.out_Pc.channels[5].values[i] = data.data_B[i].distance_deviation
137  self.out_Pc.channels[6].values[i] = data.data_B[i].angle_deviation
138  self.out_Pc.channels[7].values[i] = data.data_B[i].velocity_deviation
139  self.out_Pc.channels[8].values[i] = data.data_B[i].proability_target
140  self.out_Pc.channels[9].values[i] = data.data_A[i].is_target
141 
142  else:
143  # -- assign NAN to not valid target
144  x = float('nan')
145  y = float('nan')
146  self.out_Pc.channels[0].values[i] = float('nan')
147  self.out_Pc.channels[1].values[i] = float('nan')
148  self.out_Pc.channels[2].values[i] = float('nan')
149  self.out_Pc.channels[3].values[i] = float('nan')
150  self.out_Pc.channels[4].values[i] = float('nan')
151  self.out_Pc.channels[5].values[i] = float('nan')
152  self.out_Pc.channels[6].values[i] = float('nan')
153  self.out_Pc.channels[7].values[i] = float('nan')
154  self.out_Pc.channels[8].values[i] = float('nan')
155  self.out_Pc.channels[9].values[i] = float('nan')
156 
157  self.out_Pc.points[i] = Point32(x, y, 0)
158 
159  self.pcpub.publish(self.out_Pc)
160 
161 # ****************************************************************************
162 
163 if __name__ == '__main__':
164 
165  try:
166  radar2pcdNode = radar2pcdPaNode()
167  rospy.spin()
168  except rospy.ROSInterruptException:
169  pass
170 
radar2pcd_pa_node.radar2pcdPaNode.callback
def callback(self, data)
Definition: radar2pcd_pa_node.py:117
radar2pcd_pa_node.radar2pcdPaNode.pcpub
pcpub
Definition: radar2pcd_pa_node.py:77
radar2pcd_pa_node.radar2pcdPaNode.__init__
def __init__(self)
Definition: radar2pcd_pa_node.py:72
radar2pcd_pa_node.radar2pcdPaNode
Definition: radar2pcd_pa_node.py:70
radar2pcd_pa_node.radar2pcdPaNode.out_Pc
out_Pc
Definition: radar2pcd_pa_node.py:80

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