radar_pa_node.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 """ Radar_Pa package, this is radar_pa_node """
5 
6 # *****************************************************************************
7 # *
8 # radar_pa_node.py *
9 # *
10 # *****************************************************************************
11 # *
12 # github repository *
13 # https://github.com/TUC-ProAut/radar_pa *
14 # *
15 # Chair of Automation Technology, Technische Universität Chemnitz *
16 # https://www.tu-chemnitz.de/etit/proaut *
17 # *
18 # *****************************************************************************
19 # *
20 # BSD 3-Clause License *
21 # *
22 # Copyright (c) 2018-2019, Karim Haggag, Technische Universität Chemnitz *
23 # All rights reserved. *
24 # *
25 # Redistribution and use in source and binary forms, with or without *
26 # modification, are permitted provided that the following conditions are met: *
27 # * Redistributions of source code must retain the above copyright *
28 # notice, this list of conditions and the following disclaimer. *
29 # * Redistributions in binary form must reproduce the above copyright *
30 # notice, this list of conditions and the following disclaimer in the *
31 # documentation and/or other materials provided with the distribution. *
32 # * Neither the names of the copyright holders nor the names of its *
33 # contributors may be used to endorse or promote products *
34 # derived from this software without specific prior written permission. *
35 # *
36 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
37 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
38 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
39 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE *
40 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR *
41 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF *
42 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS *
43 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN *
44 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) *
45 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE *
46 # POSSIBILITY OF SUCH DAMAGE. *
47 # *
48 # *****************************************************************************
49 # *
50 # Revision 1 *
51 # *
52 # *****************************************************************************
53 # *
54 # This node converts the given data from the can messages to radar_msgs. *
55 # *
56 # Supported radars: *
57 # * Bosch GPR v1.0 *
58 # *
59 # *****************************************************************************
60 
61 # -- ros modules
62 import rospy
63 import std_msgs.msg
64 from can_msgs.msg import Frame
65 from radar_pa_msgs.msg import radar_msg_A
66 from radar_pa_msgs.msg import radar_msg_B
67 from radar_pa_msgs.msg import radar_msg
68 from extract_message import extract_GPR_v10
69 
70 # -- this is a class now :-)
71 class RadarPaNode:
72 
73  """This is a Radar Node class, which instansiate a node and
74  subscribig to received_messages topice to be able to read can messages.
75  Then, publish the extracted message A on "can_messages_A", extracted
76  message A on "can_messages_A" and both A,B on "radar_messages"."""
77 
78  def __init__(self):
79 
80  # -- instantiat instance a node
81  rospy.init_node('radar_pa_node.py', anonymous=True)
82 
83  # -- use socketcan_bridge package / then subscribe to the topic 'received_messages'
84  # -- published by socketcan_bridge_node
85  # -- we install 'socketcan_bridge package' so it becomes system package
86  # /opt/ros/kinetic/share/socketcan_bridge
87  rospy.Subscriber('received_messages', Frame, self.callback)
88 
89  # -- publish radar_msg_A type
90  self.pub_a = rospy.Publisher('can_messages_A', radar_msg_A, queue_size=10)
91  # -- radar_msg_B type
92  self.pub_b = rospy.Publisher('can_messages_B', radar_msg_B, queue_size=10)
93  # -- both radar_msg A,B
94  self.pub = rospy.Publisher('radar_messages', radar_msg , queue_size=10)
95 
96  # -- instantiate radar_messages
97  self.global_msg = radar_msg()
98 
99  # -- instantiate radar_message
100  self.global_msg.header = std_msgs.msg.Header()
101 
102  # -- assign frame id
103  self.global_msg.header.frame_id = "radar"
104 
105  # -- set time stamp
106  self.global_msg.header.stamp = rospy.Time.now()
107 
108  def callback(self, data):
109 
110  """This method calling the extection function and fill the msg with
111  the result from the extraction function."""
112  self.global_msg.header.stamp = data.header.stamp
113 
114  # -- check the bounderies of id
115  if (data.id >= 512 and data.id <= 607):
116 
117  # -- call the extract function from extract_message file
118  msg = extract_GPR_v10(data.data)
119 
120  # -- call fill_message
121  self.fill_message(data, msg)
122  # -- check message type the publish to the corresponding topic
123  if (isinstance(msg, radar_msg_A)):
124  #if (msg.message == 1) :
125  self.pub_a.publish(msg)
126 
127  else:
128  self.pub_b.publish(msg)
129 
130  # -- check if the message have the same counter then publish it
131  if (data.id == 607):
132  if (self.check_message_counter() is True):#== True):
133  rospy.logdebug('the whole packet is completed')
134  self.pub.publish(self.global_msg)
135 
136  else:
137  rospy.logdebug('can id out of range %d (%s)', data.id, hex(data.id))
138 
139  # ***************** fill the message with msg A , B *****************
140 
141  def fill_message(self, data, msg):
142 
143  """ Ths function to fill the global message with the extracted result"""
144  if (data.id % 2 == 0):
145  # -- can_messages_A start from 0x200 = 512
146  i = (data.id - 512)/2
147  self.global_msg.data_A[i] = msg
148  else:
149  # -- can_messages_A start from 0x201 = 513
150  j = (data.id - 513) / 2
151  self.global_msg.data_B[j] = msg
152 
153 
154  # ***************** check the counter for all target *****************
155 
156  def check_message_counter(self):
157 
158  """ This function used to cehck all counters are the same before
159  publishing the message to the topic"""
160  # -- check for that all message have the same counter
161  # -- which mean all message are from the current cycle
162  for i in range(len(self.global_msg.data_A)):
163  if ((self.global_msg.data_A[0].counter != self.global_msg.data_A[i].counter) or \
164  (self.global_msg.data_A[0].counter != self.global_msg.data_B[i].counter)):
165  rospy.loginfo('counter is not equal')
166  return False
167 
168  return True
169 
170 # ****************************************************************************
171 
172 if __name__ == '__main__':
173  try:
174  RADAR_NODE = RadarPaNode()
175 
176  # -- spin simply keeps python from exiting until this node is stopped
177  rospy.spin()
178 
179  except rospy.ROSInterruptException:
180  pass
radar_pa_node.RadarPaNode.pub_a
pub_a
Definition: radar_pa_node.py:90
radar_pa_node.RadarPaNode.pub_b
pub_b
Definition: radar_pa_node.py:92
radar_pa_node.RadarPaNode.global_msg
global_msg
Definition: radar_pa_node.py:97
radar_pa_node.RadarPaNode.callback
def callback(self, data)
Definition: radar_pa_node.py:108
radar_pa_node.RadarPaNode
Definition: radar_pa_node.py:71
extract_message.extract_GPR_v10
def extract_GPR_v10(received_data)
Definition: extract_message.py:67
radar_pa_node.RadarPaNode.fill_message
def fill_message(self, data, msg)
Definition: radar_pa_node.py:141
radar_pa_node.RadarPaNode.pub
pub
Definition: radar_pa_node.py:94
radar_pa_node.RadarPaNode.check_message_counter
def check_message_counter(self)
Definition: radar_pa_node.py:156
radar_pa_node.RadarPaNode.__init__
def __init__(self)
Definition: radar_pa_node.py:78

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