global_path_follower.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 on any new /initialpose, do full rotation, then delay (to hone in amcl)
5 
6 follow something ~15th pose in global path for all moves (about 0.3m away?)
7  -maximum path length seems to be about 35*5 (45*5 max) for 2-3 meter path
8  -(longer if more turns -- go for 15th or 20th pose, or max if less, should be OK)
9 
10 ignore local path, except for determining if at goal or not
11  if no recent local path, must be at goal: followpath = False, goalpose = true
12 
13 requires dwa_base_controller, global path updated continuously as bot moves
14 
15 """
16 
17 
18 import rospy, tf
19 import oculusprimesocket
20 from nav_msgs.msg import Odometry
21 import math
22 from nav_msgs.msg import Path
23 from geometry_msgs.msg import PoseWithCovarianceStamped
24 from actionlib_msgs.msg import GoalStatusArray
25 from move_base_msgs.msg import MoveBaseActionGoal
26 
27 listentime = 0.6 # 0.6 # allows odom + amcl to catch up
28 nextmove = 0
29 odomx = 0
30 odomy = 0
31 odomth = 0
32 targetx = 0
33 targety = 0
34 targetth = 0
35 followpath = False
36 pathid = None
37 goalth = 0
38 initialturn = False
39 waitonaboutface = 0
40 minturn = math.radians(8) # (was 6) -- 0.21 minimum for pwm 255
41 minlinear = 0.08 # was 0.05
42 maxlinear = 0.5
43 lastpath = 0 # refers to localpath
44 goalpose = False
45 goalseek = False
46 meterspersec = 0.33 # linear speed TODO: get from java
47 degperms = 0.0857 # turnspeed TODO: get from java
48 tfth = 0
49 globalpathposenum = 20 # just right
50 listener = None
51 
52 
53 def pathCallback(data): # local path
54  global goalpose, lastpath
55 
56  lastpath = rospy.get_time()
57  goalpose = False
58 
59 def globalPathCallback(data):
60  global targetx, targety, targetth , followpath, pathid
61 
62  n = len(data.poses)
63  if n < 5:
64  return
65 
66  if n-1 < globalpathposenum:
67  p = data.poses[n-1]
68  else:
69  p = data.poses[globalpathposenum]
70 
71  targetx = p.pose.position.x
72  targety = p.pose.position.y
73  quaternion = ( p.pose.orientation.x, p.pose.orientation.y,
74  p.pose.orientation.z, p.pose.orientation.w )
75  targetth = tf.transformations.euler_from_quaternion(quaternion)[2]
76 
77  followpath = True
78  pathid = data.header.seq
79 
80 def odomCallback(data):
81  global odomx, odomy, odomth
82  odomx = data.pose.pose.position.x
83  odomy = data.pose.pose.position.y
84  quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
85  data.pose.pose.orientation.z, data.pose.pose.orientation.w )
86  odomth = tf.transformations.euler_from_quaternion(quaternion)[2]
87 
88  # determine direction (angle) on map
89  global tfth, listener
90  try:
91  (trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
92  quaternion = (rot[0], rot[1], rot[2], rot[3])
93  tfth = tf.transformations.euler_from_quaternion(quaternion)[2]
94  except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
95  pass
96 
97 def intialPoseCallback(data):
98  if data.pose.pose.position.x == 0 and data.pose.pose.position.y == 0:
99  return
100  # do full rotation on pose estimate, to hone-in amcl (if not docked)
101  rospy.sleep(0.5) # let amcl settle
102  oculusprimesocket.clearIncoming() # why?
103  oculusprimesocket.sendString("right 360")
104  oculusprimesocket.waitForReplySearch("<state> direction stop")
105 
106 
107 def goalCallback(d):
108  global goalth, goalpose, lastpath, initialturn, followpath, nextmove
109 
110  # to prevent immediately rotating wrongly towards new goal direction
111  lastpath = rospy.get_time()
112  goalpose = False
113 
114  # set goal angle
115  data = d.goal.target_pose
116  quaternion = ( data.pose.orientation.x, data.pose.orientation.y,
117  data.pose.orientation.z, data.pose.orientation.w )
118  goalth = tf.transformations.euler_from_quaternion(quaternion)[2]
119  initialturn = True
120  followpath = False
121  nextmove = lastpath + 2 # sometimes globalpath still points at previoius goal
122 
123 def goalStatusCallback(data):
124  global goalseek
125  goalseek = False
126  if len(data.status_list) == 0:
127  return
128  status = data.status_list[len(data.status_list)-1] # get latest status
129  if status.status == 1:
130  goalseek = True
131 
132 def move(ox, oy, oth, tx, ty, tth, gth):
133  global followpath, goalpose, tfth, pathid, initialturn, waitonaboutface
134  global odomx, odomy, odomth
135 
136  currentpathid = pathid
137 
138  # determine xy deltas for move
139  distance = 0
140  if followpath:
141  dx = tx - ox
142  dy = ty - oy
143  distance = math.sqrt( pow(dx,2) + pow(dy,2) )
144 
145  goalrotate = False
146  if distance > 0:
147  th = math.acos(dx/distance)
148  if dy <0:
149  th = -th
150  elif goalpose:
151  th = gth - tfth
152  goalrotate = True
153  else:
154  th = tth
155 
156  # determine angle delta for move
157  dth = th - oth
158  if dth > math.pi:
159  dth = -math.pi*2 + dth
160  elif dth < -math.pi:
161  dth = math.pi*2 + dth
162 
163  # force minimums
164  if distance > 0 and distance < minlinear:
165  distance = minlinear
166 
167  if distance > maxlinear:
168  distance = maxlinear
169 
170  # supposed to reduce zig zagging (was 0.3)
171  if dth < minturn*0.5 and dth > -minturn*0.5:
172  dth = 0
173  elif dth >= minturn*0.5 and dth < minturn:
174  dth = minturn
175  elif dth <= -minturn*0.5 and dth > -minturn:
176  dth = -minturn
177 
178  oculusprimesocket.clearIncoming()
179 
180  # if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
181  if abs(dth) > 2.0944 and not goalrotate and not initialturn and waitonaboutface < 1:
182  oculusprimesocket.sendString("forward 0.25")
183  oculusprimesocket.waitForReplySearch("<state> direction stop")
184  waitonaboutface += 1 # only do this once
185  rospy.sleep(1)
186  return
187 
188  waitonaboutface = 0
189 
190  if not pathid == currentpathid:
191  return
192 
193  if dth > 0:
194  oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
195  oculusprimesocket.waitForReplySearch("<state> direction stop")
196  elif dth < 0:
197  oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
198  oculusprimesocket.waitForReplySearch("<state> direction stop")
199 
200  if distance > 0:
201  oculusprimesocket.sendString("forward "+str(distance))
202  rospy.sleep(distance/meterspersec)
203  initialturn = False
204 
205  # if goalrotate:
206  # rospy.sleep(1)
207 
208 
209 def cleanup():
210  # oculusprimesocket.sendString("move stop")
211  # oculusprimesocket.sendString("state delete navigationenabled")
212  oculusprimesocket.sendString("log global_path_follower.py disconnecting")
213 
214 
215 # MAIN
216 
217 # rospy.init_node('dwa_base_controller', anonymous=False)
218 rospy.init_node('global_path_follower', anonymous=False)
219 listener = tf.TransformListener()
220 oculusprimesocket.connect()
221 rospy.on_shutdown(cleanup)
222 
223 rospy.Subscriber("odom", Odometry, odomCallback)
224 rospy.Subscriber("move_base/DWAPlannerROS/local_plan", Path, pathCallback)
225 rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
226 rospy.Subscriber("move_base/status", GoalStatusArray, goalStatusCallback)
227 rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback)
228 rospy.Subscriber("initialpose", PoseWithCovarianceStamped, intialPoseCallback)
229 
230 oculusprimesocket.sendString("log global_path_follower.py connected")
231 # oculusprimesocket.sendString("state odomturndpms "+str(degperms)) # degrees per ms
232 # oculusprimesocket.sendString("state odomturnpwm 100") # approx starting point smooth floor
233 # oculusprimesocket.sendString("state odomlinearmpms "+str(meterspersec/1000))
234 # oculusprimesocket.sendString("state odomlinearpwm 150") # approx starting point
235 
236 # oculusprimesocket.sendString("speed "+str(linearspeed) )
237 
238 while not rospy.is_shutdown():
239  t = rospy.get_time()
240 
241  if t >= nextmove:
242  # nextmove = t + listentime
243  if goalseek and (followpath or goalpose):
244  move(odomx, odomy, odomth, targetx, targety, targetth, goalth) # blocking
245  nextmove = rospy.get_time() + listentime
246  followpath = False
247 
248  if t - lastpath > 3:
249  goalpose = True
250 
251  rospy.sleep(0.01)
252 
msg
oculusprimesocket.waitForReplySearch
def waitForReplySearch(pattern)
Definition: oculusprimesocket.py:36
global_path_follower.goalStatusCallback
def goalStatusCallback(data)
Definition: global_path_follower.py:123
global_path_follower.cleanup
def cleanup()
Definition: global_path_follower.py:209
global_path_follower.intialPoseCallback
def intialPoseCallback(data)
Definition: global_path_follower.py:97
global_path_follower.pathCallback
def pathCallback(data)
Definition: global_path_follower.py:53
global_path_follower.odomCallback
def odomCallback(data)
Definition: global_path_follower.py:80
global_path_follower.globalPathCallback
def globalPathCallback(data)
Definition: global_path_follower.py:59
global_path_follower.goalCallback
def goalCallback(d)
Definition: global_path_follower.py:107
global_path_follower.move
def move(ox, oy, oth, tx, ty, tth, gth)
Definition: global_path_follower.py:132

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