dwm1001_main.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 """ For more info on the documentation go to https://www.decawave.com/sites/default/files/dwm1001-api-guide.pdf
3 """
4 
5 from dwm1001_systemDefinitions import SYS_DEFS
6 
7 __author__ = SYS_DEFS.AUTHOR
8 __version__ = SYS_DEFS.VERSION
9 __maintainer__ = SYS_DEFS.MAINTAINER
10 __email__ = SYS_DEFS.EMAIL
11 __status__ = SYS_DEFS.STATUS
12 
13 
14 import rospy, time, serial, os
15 from dwm1001_apiCommands import DWM1001_API_COMMANDS
16 from dynamic_reconfigure.server import Server
17 from localizer_dwm1001.cfg import DWM1001_Tune_SerialConfig
18 from localizer_dwm1001.msg import Anchor
19 from localizer_dwm1001.msg import Tag
20 from localizer_dwm1001.srv import Anchor_0
21 
22 
23 
24 # initialize the node
25 rospy.init_node('Localizer_DWM1001', anonymous=False)
26 
27 # allow serial port to be detected by user
28 os.popen("sudo chmod 777 /dev/ttyACM0", "w")
29 
30 # initialize ros rate 10hz
31 rate = rospy.Rate(1)
32 
33 serialReadLine = ""
34 # For dynamic configuration
35 dynamicConfig_OPEN_PORT = {"open_port": False}
36 dynamicConfig_CLOSE_PORT = {"close_port": False}
37 dynamicConfig_SERIAL_PORT = {"serial_port": ""}
38 
39 # initialize serial port connections
40 serialPortDWM1001 = serial.Serial(
41 
42  port = str(rospy.get_param('~serial_port_name')),
43  baudrate = int(rospy.get_param('~serial_baud_rate')),
44  parity=SYS_DEFS.parity,
45  stopbits=SYS_DEFS.stopbits,
46  bytesize=SYS_DEFS.bytesize
47 )
48 
49 
50 class dwm1001_localizer:
51 
52 
53 
54  def main(self):
55  """
56  Initialize port and dwm1001 api
57 
58  :param:
59 
60  :returns: none
61 
62  """
63 
64  global serialReadLine
65 
66  #TODO implemnt functionality dynamic configuration
67  #updateDynamicConfiguration_SERIALPORT()
68  # close the serial port in case the previous run didn't closed it properly
69  serialPortDWM1001.close()
70  # sleep for one sec
71  time.sleep(1)
72  # open serial port
73  serialPortDWM1001.open()
74 
75  # check if the serial port is opened
76  if(serialPortDWM1001.isOpen()):
77  rospy.loginfo("Port opened: "+ str(serialPortDWM1001.name) )
78  # start sending commands to the board so we can initialize the board
79  self.initializeDWM1001API()
80  # give some time to DWM1001 to wake up
81  time.sleep(2)
82  # send command lec, so we can get positions is CSV format
83  serialPortDWM1001.write(DWM1001_API_COMMANDS.LEC)
84  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
85  rospy.loginfo("Reading DWM1001 coordinates")
86  else:
87  rospy.loginfo("Can't open port: "+ str(serialPortDWM1001.name))
88 
89  try:
90 
91  while not rospy.is_shutdown():
92  # just read everything from serial port
93  serialReadLine = serialPortDWM1001.read_until()
94 
95  try:
96  self.pubblishCoordinatesIntoTopics(self.splitByComma(serialReadLine))
97 
98  except IndexError:
99  rospy.loginfo("Found index error in the network array!DO SOMETHING!")
100 
101 
102 
103  except KeyboardInterrupt:
104  rospy.loginfo("Quitting DWM1001 Shell Mode and closing port, allow 1 second for UWB recovery")
105  serialPortDWM1001.write(DWM1001_API_COMMANDS.RESET)
106  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
107 
108  finally:
109  rospy.loginfo("Quitting, and sending reset command to dev board")
110  # serialPortDWM1001.reset_input_buffer()
111  serialPortDWM1001.write(DWM1001_API_COMMANDS.RESET)
112  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
113  rate.sleep()
114  if "reset" in serialReadLine:
115  rospy.loginfo("succesfully closed ")
116  serialPortDWM1001.close()
117 
118  def splitByComma(self, dataFromUSB):
119  """
120  Split network data such us coordinates of anchor and tag, by comma ','
121 
122  :param dataFromUSB: Array from serial port containing all informations, tag xyz and anchor xyz
123 
124  :returns: arrayFromUSBFormatted
125 
126  """
127 
128  arrayFromUSBFormatted = [x.strip() for x in dataFromUSB.strip().split(',')]
129 
130  return arrayFromUSBFormatted
131 
132  def pubblishCoordinatesIntoTopics(self, networkDataArray):
133  """
134  Publish anchors and tag in topics using Tag and Anchor Object
135 
136  :param networkDataArray: Array from serial port containing all informations, tag xyz and anchor xyz
137 
138  :returns: none
139 
140  """
141 
142  # loop trough the array given by the serial port
143  for network in networkDataArray:
144 
145  # check if there is any entry starting with AN, which means there is an anchor
146  if 'AN' in network:
147  # get the number after'AN' which we will use to pubblish topics, example /dwm1001/anchor1
148  temp_anchor_number = networkDataArray[networkDataArray.index(network)]
149  # construct the object for anchor(s)
150  anchor = Anchor( str(networkDataArray[networkDataArray.index(network) + 1]),
151  float(networkDataArray[networkDataArray.index(network) + 2]),
152  float(networkDataArray[networkDataArray.index(network) + 3]),
153  float(networkDataArray[networkDataArray.index(network) + 4]),
154  float(networkDataArray[networkDataArray.index(network) + 5]))
155 
156  # publish each anchor, add anchor number to the topic, so we can pubblish multiple anchors
157  # example /dwm1001/anchor0, the last digit is taken from AN0 and so on
158  pub_anchor = rospy.Publisher('/dwm1001/anchor'+str(temp_anchor_number[-1]), Anchor, queue_size=1)
159  pub_anchor.publish(anchor)
160  rospy.loginfo("Anchor: "
161  + str(anchor.id)
162  + " x: "
163  + str(anchor.x)
164  + " y: "
165  + str(anchor.y)
166  + " z: "
167  + str(anchor.z))
168 
169  elif 'POS' in network:
170 
171  # construct the object for the tag
172  tag = Tag(float(networkDataArray[networkDataArray.index(network) + 1]),
173  float(networkDataArray[networkDataArray.index(network) + 2]),
174  float(networkDataArray[networkDataArray.index(network) + 3]),)
175 
176  # publish tag
177  pub_anchor = rospy.Publisher('/dwm1001/tag', Tag, queue_size=1)
178  pub_anchor.publish(tag)
179 
180  rospy.loginfo("Tag: "
181  + " x: "
182  + str(tag.x)
183  + " y: "
184  + str(tag.y)
185  + " z: "
186  + str(tag.z))
187 
188 
189 
190 
191 
192  def updateDynamicConfiguration_SERIALPORT(self):
193 
194  """
195  Update dynamic configuration of ROS
196 
197  :param:
198 
199  :returns: none
200 
201  """
202 
203  global dynamicConfig_SERIAL_PORT
204 
205  # intialize dynamic configuration
206  dynamicConfigServer = Server(DWM1001_Tune_SerialConfig, self.callbackDynamicConfig)
207  # set close port to true
208  dynamicConfig_CLOSE_PORT.update({"close_port": True})
209  # set the open port to false
210  dynamicConfig_OPEN_PORT.update({"open_port" : False})
211  # update the server
212  dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
213  dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
214  # update the server with opened port
215  dynamicConfig_CLOSE_PORT.update({"close_port": False})
216  # update the server with close port
217  dynamicConfig_OPEN_PORT.update({"open_port": True})
218  # update name of serial port in dynamic configuration
219  dynamicConfig_SERIAL_PORT = {"serial_port": str(serialPortDWM1001.name)}
220  # now update server configuration
221  dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
222  dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
223  dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
224  dynamicConfigServer.update_configuration(dynamicConfig_SERIAL_PORT)
225 
226  def initializeDWM1001API(self):
227  """
228  Initialize dwm1001 api, by sending sending bytes
229 
230  :param:
231 
232  :returns: none
233 
234  """
235  # reset incase previuos run didn't close properly
236  serialPortDWM1001.write(DWM1001_API_COMMANDS.RESET)
237  # send ENTER two times in order to access api
238  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
239  # sleep for half a second
240  time.sleep(0.5)
241  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
242  # sleep for half second
243  time.sleep(0.5)
244  # send a third one - just in case
245  serialPortDWM1001.write(DWM1001_API_COMMANDS.SINGLE_ENTER)
246 
247 
248  def callbackDynamicConfig(self, config, level):
249  """
250  Map each button from dynamic configuration gui with specific action
251 
252  :param config: array contains value of the gui
253 
254  :returns: config
255 
256  """
257  global serialReadLine
258  #rospy.loginfo("""Reconfigure Request: {dwm1001_network_info}, {open_port},\
259  # {serial_port}, {close_port}""".format(**config))
260 
261  if config["quit_dwm1001_api"]:
262  rospy.loginfo("Not implement it yet")
263  config["quit_dwm1001_api"] = False
264 
265  if config["close_port"]:
266  rospy.loginfo("Close port not implement it yet")
267  config["close_port"] = False
268 
269  if config["exit"]:
270  rospy.loginfo("Not implement it yet")
271  config["exit"] = False
272 
273  return config
274 
275 def start():
276  dwm1001 = dwm1001_localizer()
277  dwm1001.main()
278 
279 
280 if __name__ == '__main__':
281  try:
282  start()
283  rospy.spin()
284  except rospy.ROSInterruptException:
285  pass
286 
287 
dwm1001_main.dwm1001_localizer.initializeDWM1001API
def initializeDWM1001API(self)
Definition: dwm1001_main.py:226
dwm1001_main.dwm1001_localizer.main
def main(self)
Definition: dwm1001_main.py:54
dwm1001_main.start
def start()
Definition: dwm1001_main.py:275
dwm1001_main.dwm1001_localizer
Definition: dwm1001_main.py:50
dwm1001_main.dwm1001_localizer.splitByComma
def splitByComma(self, dataFromUSB)
Definition: dwm1001_main.py:118
dwm1001_main.dwm1001_localizer.updateDynamicConfiguration_SERIALPORT
def updateDynamicConfiguration_SERIALPORT(self)
Definition: dwm1001_main.py:192
dwm1001_main.dwm1001_localizer.callbackDynamicConfig
def callbackDynamicConfig(self, config, level)
Definition: dwm1001_main.py:248
dwm1001_main.dwm1001_localizer.pubblishCoordinatesIntoTopics
def pubblishCoordinatesIntoTopics(self, networkDataArray)
Definition: dwm1001_main.py:132

dwm1001_ros
Author(s):
autogenerated on Mon Jun 10 2019 13:05:20