remote_nav.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy, tf
4 import os, struct, re
5 from nav_msgs.msg import OccupancyGrid, Path
6 from nav_msgs.msg import Odometry
7 from geometry_msgs.msg import PoseWithCovarianceStamped
8 import oculusprimesocket
9 from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
10 from sensor_msgs.msg import LaserScan
11 import actionlib
12 from actionlib_msgs.msg import *
13 from std_srvs.srv import Empty
14 import dynamic_reconfigure.client
15 
16 """
17 sending info:
18 rosmapinfo (origin, size, resolution) on callback
19 rosamcl (x,y,th) on callback
20  -also sending global path, scan data at this time, if exists
21 roscurrentgoal (x,y,th) on callback
22 
23 change to:
24 send amcl on callback, + latest odom << change to amcloffset only???
25 roscurrentgoal, rosmapinfo unchanged
26 
27 send global path (without amcl offset), scan data, odom every 0.5 sec
28 
29 """
30 
31 
32 lockfilepath = "/run/shm/map.raw.lock"
33 lastodomupdate = 0
34 odomx = 0
35 odomy = 0
36 odomth = 0
37 globalpath = []
38 scannum = 0
39 scanpoints = []
40 sendinfodelay = 1.0
41 lastsendinfo = 0
42 move_base = None
43 goalseek = False
44 xoffst = 0
45 yoffst = 0
46 thoffst = 0
47 recoveryrotate = False
48 goal = None
49 turnspeed = 100
50 secondspertwopi = 4.2 # calibration, automate? (do in java, faster)
51 # docked = False
52 lidarclient = None
53 
54 
55 def mapcallBack(data):
56  global lockfilepath
57  lockfilepath = "/run/shm/map.raw.lock"
58  framefilepath ="/run/shm/map.raw"
59 
60  if os.path.exists(lockfilepath):
61  return
62 
63  open(lockfilepath, 'w') # creates lockfile
64 
65  framefile = open(framefilepath, 'w')
66  packeddata = struct.pack('%sb' %len(data.data), *data.data)
67  framefile.write(packeddata)
68  framefile.close()
69 
70  if os.path.exists(lockfilepath):
71  os.remove(lockfilepath)
72 
73  quaternion = ( data.info.origin.orientation.x, data.info.origin.orientation.y,
74  data.info.origin.orientation.z, data.info.origin.orientation.w )
75  th = tf.transformations.euler_from_quaternion(quaternion)[2]
76 
77  # width height res originx originy originth updatetime
78  s = "state rosmapinfo "+str(data.info.width)+","+str(data.info.height)+","
79  s += str(data.info.resolution)+","+str(data.info.origin.position.x)+","
80  s += str(data.info.origin.position.y)+","+str(th)+","+str(rospy.get_time())
81 
82  oculusprimesocket.sendString(s)
83  oculusprimesocket.sendString("state rosmapupdated true")
84 
85 def odomCallback(data):
86  global odomx, odomy, odomth
87  odomx = data.pose.pose.position.x
88  odomy = data.pose.pose.position.y
89  quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
90  data.pose.pose.orientation.z, data.pose.pose.orientation.w )
91  odomth = tf.transformations.euler_from_quaternion(quaternion)[2]
92 
93 def amclPoseCallback(data):
94  global odomx, odomy, odomth, goalseek, xoffst, yoffst, thoffst
95 
96  if not goalseek:
97  amclx = data.pose.pose.position.x
98  amcly = data.pose.pose.position.y
99  quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
100  data.pose.pose.orientation.z, data.pose.pose.orientation.w )
101  amclth = tf.transformations.euler_from_quaternion(quaternion)[2]
102 
103  xoffst = amclx - odomx
104  yoffst = amcly - odomy
105  thoffst = amclth - odomth
106 
107 
108 def feedbackCallback(d):
109  global odomx, odomy, odomth, xoffst, yoffst, thoffst
110  data = d.feedback.base_position.pose
111  amclx = data.position.x
112  amcly = data.position.y
113  quaternion = ( data.orientation.x, data.orientation.y,
114  data.orientation.z, data.orientation.w )
115  amclth = tf.transformations.euler_from_quaternion(quaternion)[2]
116 
117  xoffst = amclx - odomx
118  yoffst = amcly - odomy
119  thoffst = amclth - odomth
120 
121 def goalCallback(d):
122  data = d.goal.target_pose
123  x = data.pose.position.x
124  y = data.pose.position.y
125  quaternion = ( data.pose.orientation.x, data.pose.orientation.y,
126  data.pose.orientation.z, data.pose.orientation.w )
127  th = tf.transformations.euler_from_quaternion(quaternion)[2]
128  oculusprimesocket.sendString("state roscurrentgoal "+str(x)+","+str(y)+","+str(th))
129  oculusprimesocket.sendString("state delete rosgoalstatus")
130  oculusprimesocket.sendString("messageclients new navigation goal received");
131 
132 def globalPathCallback(data):
133  global globalpath
134  globalpath = data.poses
135  recoveryrotate = False
136 
137 def sendGlobalPath(path):
138  s = "state rosglobalpath "
139  step = 5
140  size = len(path)
141  i = 0
142 
143  while i < size - step:
144  s = s + str(round(path[i].pose.position.x,2))+","
145  s = s + str(round(path[i].pose.position.y,2))+","
146  i += step
147 
148  s = s + str(round(path[size-1].pose.position.x,2))
149  s = s + str(round(path[size-1].pose.position.y,2))
150 
151  oculusprimesocket.sendString(s)
152 
153 def publishinitialpose(str):
154  # global docked
155 
156  s = str.split("_")
157  x = float(s[0])
158  y = float(s[1])
159  th = float(s[2])
160 
161  pose = PoseWithCovarianceStamped()
162  pose.header.stamp = rospy.Time.now()
163  pose.header.frame_id = "map"
164 
165  pose.pose.pose.position.x = x
166  pose.pose.pose.position.y = y
167  pose.pose.pose.position.z = 0.0
168 
169  quat = tf.transformations.quaternion_from_euler(0, 0, th)
170  pose.pose.pose.orientation.x = quat[0]
171  pose.pose.pose.orientation.y = quat[1]
172  pose.pose.pose.orientation.z = quat[2]
173  pose.pose.pose.orientation.w = quat[3]
174 
175  # if not docked:
176  # pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
177  # docked = False # only skip covariance set on initial docked position
178 
179  # pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
180 
181  # pose.pose.covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
182 
183  initpose_pub.publish(pose)
184 
185 def publishgoal(str):
186  global move_base, goal
187 
188  s = str.split(",")
189  x = float(s[0])
190  y = float(s[1])
191  th = float(s[2])
192 
193  goal = MoveBaseGoal()
194  goal.target_pose.header.stamp = rospy.Time.now()
195  goal.target_pose.header.frame_id = "map"
196 
197  goal.target_pose.pose.position.x = x
198  goal.target_pose.pose.position.y = y
199  goal.target_pose.pose.position.z = 0.0
200 
201  quat = tf.transformations.quaternion_from_euler(0, 0, th)
202  goal.target_pose.pose.orientation.x = quat[0]
203  goal.target_pose.pose.orientation.y = quat[1]
204  goal.target_pose.pose.orientation.z = quat[2]
205  goal.target_pose.pose.orientation.w = quat[3]
206 
207  move_base.send_goal(goal)
208 
209 def scanCallback(data):
210  global scannum, scanpoints
211  scannum += 1
212  if scannum < 5:
213  return
214  scannum=0
215  scanpoints = data.ranges
216 
217 def sendScan():
218  global scanpoints
219 
220  s = "state rosscan "
221  size = len(scanpoints)
222 
223  step = 8 # vga depth cam, size typically 640
224  if (size < 600): # xaxxon lidar
225  # step = size/100
226  step = 2
227 
228  i = 0
229  while i < size-step:
230  s += str(round(scanpoints[i],3))+","
231  i += step
232 
233  try:
234  s += str(round(scanpoints[size-1],3))
235  except IndexError: # rare lidar scan size error
236  oculusprimesocket.sendString("messageclients remote_nav.py IndexError")
237 
238  oculusprimesocket.sendString(s)
239 
240 def cleanup():
241  oculusprimesocket.sendString("state delete roscurrentgoal")
242  oculusprimesocket.sendString("state delete rosamcl")
243  oculusprimesocket.sendString("state delete rosglobalpath")
244  oculusprimesocket.sendString("state delete rosscan")
245  oculusprimesocket.sendString("log remote_nav.py disconnecting")
246 
247 def goalcancel():
248  global goalseek, recoveryrotate
249 
250  move_base.cancel_goal()
251  goalseek = False
252  oculusprimesocket.sendString("messageclients cancel navigation goal")
253  oculusprimesocket.sendString("state delete roscurrentgoal")
254  oculusprimesocket.sendString("state delete rosgoalcancel")
255  globalpath = []
256  recoveryrotate = False
257 
258 def lidarSetParam(str):
259  global lidarclient
260 
261  if lidarclient == None:
262  lidarclient = dynamic_reconfigure.client.Client("lidarbroadcast")
263 
264  if (str == "disabled"):
265  # print("lidarSetParam " + str)
266  params = { 'lidar_enable' : 'false' }
267  lidarclient.update_configuration(params)
268  elif (str=="enabled"):
269  # print("lidarSetParam " + str)
270  params = { 'lidar_enable' : 'true' }
271  lidarclient.update_configuration(params)
272 
273 
274 # main
275 
276 oculusprimesocket.connect()
277 
278 rospy.init_node('remote_nav', anonymous=False)
279 
280 if os.path.exists(lockfilepath):
281  os.remove(lockfilepath)
282 
283 oculusprimesocket.sendString("log remote_nav.py connected")
284 oculusprimesocket.sendString("state delete roscurrentgoal")
285 oculusprimesocket.sendString("state delete rosamcl")
286 oculusprimesocket.sendString("state delete rosglobalpath")
287 oculusprimesocket.sendString("state delete rosmapinfo")
288 oculusprimesocket.sendString("state delete rosscan")
289 oculusprimesocket.sendString("state delete rosgoalstatus")
290 
291 # oculusprimesocket.sendString("state dockstatus")
292 # if oculusprimesocket.waitForReplySearch("<state> dockstatus").split()[3] == "docked":
293  # docked = True
294 
295 rospy.Subscriber("map", OccupancyGrid, mapcallBack)
296 rospy.Subscriber("odom", Odometry, odomCallback)
297 rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclPoseCallback)
298 rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
299 rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback)
300 rospy.Subscriber("scan", LaserScan, scanCallback)
301 rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback, feedbackCallback)
302 initpose_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
303 rospy.on_shutdown(cleanup)
304 
305 move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
306 move_base.wait_for_server()
307 rospy.sleep(0.5)
308 if not rospy.is_shutdown():
309  oculusprimesocket.sendString("messageclients navigation ready")
310  oculusprimesocket.sendString("state navsystemstatus running")
311 
312 lasttext = ""
313 
314 while not rospy.is_shutdown():
315 
316  s = oculusprimesocket.replyBufferSearch("<state> (rosinitialpose|rossetgoal|rosgoalcancel|lidar) ")
317  if re.search("rosinitialpose", s):
318  oculusprimesocket.sendString("state delete rosinitialpose")
319  publishinitialpose(s.split()[2])
320  recoveryrotate = False
321 
322  elif re.search("rossetgoal", s):
323  oculusprimesocket.sendString("state delete rossetgoal")
324  globalpath = []
325  publishgoal(s.split()[2])
326  goalseek = True
327  recoveryrotate = False
328 
329  elif re.search("rosgoalcancel true", s):
330  goalcancel()
331 
332  elif re.search("lidar", s):
333  lidarSetParam(s.split()[2])
334 
335 
336  t = rospy.get_time()
337  if t - lastsendinfo > sendinfodelay:
338  lastsendinfo = t
339 
340  if len(scanpoints) > 0:
341  sendScan()
342 
343  # if goalseek and not xoffst == None:
344  s = "state rosamcl "
345  s += str(round(xoffst, 3))+","+str(round(yoffst,3))+","+str(round(thoffst,3))+","
346  s += str(round(odomx,3))+","+str(round(odomy,3))+","+str(round(odomth,3))
347 
348  oculusprimesocket.sendString(s)
349 
350  if len(globalpath) > 0:
351  sendGlobalPath(globalpath)
352 
353  if goalseek:
354  state = move_base.get_state()
355  if state == GoalStatus.SUCCEEDED: # error if not seeking goal
356  oculusprimesocket.sendString("messageclients navigation goal reached")
357  oculusprimesocket.sendString("state rosgoalstatus succeeded") # having this below the one below may have caused null pointer
358  oculusprimesocket.sendString("state delete roscurrentgoal")
359  goalseek = False
360  elif state == GoalStatus.ABORTED:
361  if not recoveryrotate:
362  recoveryrotate = True
363 
364  #### recovery routine
365 
366  oculusprimesocket.sendString("messageclients navigation recovery")
367  oculusprimesocket.clearIncoming()
368 
369  # cancel goal, clear costmaps, generally reset as much as possible
370  move_base.cancel_goal()
371  rospy.wait_for_service('/move_base/clear_costmaps')
372  rospy.ServiceProxy('/move_base/clear_costmaps', Empty)()
373 
374  # wait for cpu
375  rospy.sleep(2)
376  oculusprimesocket.sendString("waitforcpu")
377  oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
378 
379  # check if cancelled by user while waiting
380  oculusprimesocket.sendString("state rosgoalcancel")
381  s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel")
382  if re.search("rosgoalcancel true", s):
383  oculusprimesocket.sendString("user cancelled nav goal")
384  goalcancel()
385  continue
386 
387  # resend pose, full rotate
388  # publishinitialpose(str(xoffst+odomx)+"_"+str(yoffst+odomy)+"_"+str(thoffst+odomth))
389  # oculusprimesocket.sendString("right 360")
390  # oculusprimesocket.waitForReplySearch("<state> direction stop")
391 
392  # back up a bit
393  oculusprimesocket.sendString("backward 0.25")
394  oculusprimesocket.waitForReplySearch("<state> direction stop")
395 
396  # wait for cpu
397  rospy.sleep(2)
398  oculusprimesocket.sendString("waitforcpu")
399  oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
400 
401  # check if cancelled by user while waiting
402  oculusprimesocket.sendString("state rosgoalcancel")
403  s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel")
404  if re.search("rosgoalcancel true", s):
405  oculusprimesocket.sendString("user cancelled nav goal")
406  goalcancel()
407  continue
408 
409  move_base.send_goal(goal) # try once more
410 
411  #### end of recovery routine
412 
413 
414  else:
415  oculusprimesocket.sendString("messageclients navigation goal ABORTED")
416  oculusprimesocket.sendString("state rosgoalstatus aborted")
417  oculusprimesocket.sendString("state delete roscurrentgoal")
418  goalseek = False
419 
420 
421  rospy.sleep(0.01) # non busy wait
422 
msg
remote_nav.amclPoseCallback
def amclPoseCallback(data)
Definition: remote_nav.py:93
oculusprimesocket.waitForReplySearch
def waitForReplySearch(pattern)
Definition: oculusprimesocket.py:36
oculusprimesocket.replyBufferSearch
def replyBufferSearch(pattern)
Definition: oculusprimesocket.py:79
remote_nav.publishgoal
def publishgoal(str)
Definition: remote_nav.py:185
remote_nav.sendGlobalPath
def sendGlobalPath(path)
Definition: remote_nav.py:137
remote_nav.cleanup
def cleanup()
Definition: remote_nav.py:240
remote_nav.lidarSetParam
def lidarSetParam(str)
Definition: remote_nav.py:258
remote_nav.sendScan
def sendScan()
Definition: remote_nav.py:217
remote_nav.mapcallBack
def mapcallBack(data)
Definition: remote_nav.py:55
remote_nav.goalCallback
def goalCallback(d)
Definition: remote_nav.py:121
remote_nav.goalcancel
def goalcancel()
Definition: remote_nav.py:247
remote_nav.feedbackCallback
def feedbackCallback(d)
Definition: remote_nav.py:108
remote_nav.scanCallback
def scanCallback(data)
Definition: remote_nav.py:209
remote_nav.globalPathCallback
def globalPathCallback(data)
Definition: remote_nav.py:132
remote_nav.publishinitialpose
def publishinitialpose(str)
Definition: remote_nav.py:153
remote_nav.odomCallback
def odomCallback(data)
Definition: remote_nav.py:85

oculusprime
Author(s): Colin Adamson
autogenerated on Wed Mar 10 2021 03:14:59