segmented_arc_base_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 listen to /move_base/TrajectoryPlannerROS/local_plan
5 use rotate and movedistance commands to get there, repeat
6 subscribe to odo so moves relative to current pos
7 assumes robot is stopped before running this
8 
9 rosmsg show nav_msgs/Path
10 rosmsg show nav_msgs/Odometry
11 rosmsg show geometry_msgs/PoseStamped << is in map frame!!!
12 
13 consider polling telnet and broadcasting odom from here, to only update odom between moves
14 /move_base/status 3 = goal reached, 1= accepted (read last in list)
15 rosmsg show actionlib_msgs/GoalStatusArray
16 
17 adding initial pose to goal pose:
18 1st test, initial poseth -90deg: solution would be just ot add initial pose to goal pose
19  gth = gth + ith
20  180 = 180 + (-90)
21  if dth > math.pi:
22  dth = -math.pi*2 + dth
23  elif dth < -math.pi:
24  dth = math.pi*2 + dth
25 ^^ works but not if odom gets whacked - need to use odom/map tf and ADD to goal pose diff
26 """
27 
28 import rospy, tf
29 import oculusprimesocket
30 from nav_msgs.msg import Odometry
31 import math
32 from nav_msgs.msg import Path
33 from geometry_msgs.msg import PoseStamped #, PoseWithCovarianceStamped
34 from actionlib_msgs.msg import GoalStatusArray
35 
36 listentime = 1.5 # constant, seconds
37 nextmove = 0
38 odomx = 0
39 odomy = 0
40 odomth = 0
41 targetx = 0
42 targety = 0
43 targetth = 0
44 followpath = False
45 goalth = 0
46 minturn = math.radians(6) # 0.21 minimum for pwm 255
47 lastpath = 0
48 goalpose = False
49 goalseek = False
50 linearspeed = 150
51 secondspermeter = 3.2 #float
52 turnspeed = 100
53 secondspertwopi = 3.8
54 initth = 0
55 #initgoalth = 0
56 tfth = 0
57 
58 def pathCallback(data):
59  global targetx, targety, targetth, followpath, lastpath, goalpose
60  lastpath = rospy.get_time()
61  goalpose = False
62  followpath = True
63  p = data.poses[len(data.poses)-1] # get latest pose
64  targetx = p.pose.position.x
65  targety = p.pose.position.y
66  quaternion = ( p.pose.orientation.x, p.pose.orientation.y,
67  p.pose.orientation.z, p.pose.orientation.w )
68  targetth = tf.transformations.euler_from_quaternion(quaternion)[2]
69 
70 def odomCallback(data):
71  global odomx, odomy, odomth
72  odomx = data.pose.pose.position.x
73  odomy = data.pose.pose.position.y
74  quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
75  data.pose.pose.orientation.z, data.pose.pose.orientation.w )
76  odomth = tf.transformations.euler_from_quaternion(quaternion)[2]
77 
78 def goalCallback(data):
79  global goalth, followpath, lastpath, goalpose, odomx, odomy, odomth, targetx, targety, targetth
80  goalpose = False
81  followpath = True
82  lastpath = 0
83  targetx = odomx
84  targety = odomy
85 
86  if goalpose:
87  targetth = goalth # - tfth
88  else:
89  targetth = odomth # + tfth
90 
91  # if targetth > math.pi:
92  # targetth = -math.pi*2 + targetth
93  # elif targetth < -math.pi:
94  # targetth = math.pi*2 + targetth
95 
96  quaternion = ( data.pose.orientation.x, data.pose.orientation.y,
97  data.pose.orientation.z, data.pose.orientation.w )
98  goalth = tf.transformations.euler_from_quaternion(quaternion)[2]
99  # goalth -= tfth
100  # if goalth > math.pi:
101  # goalth = -math.pi*2 + goalth
102  # elif goalth < -math.pi:
103  # goalth = math.pi*2 + goalth
104 
105 def goalStatusCallback(data):
106  global goalseek
107  goalseek = False
108  if len(data.status_list) == 0:
109  return
110  status = data.status_list[len(data.status_list)-1] # get latest status
111  if status.status == 1:
112  goalseek = True
113 
114 # def initialposeCallback(data):
115  # global initth
116  # quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
117  # data.pose.pose.orientation.z, data.pose.pose.orientation.w )
118  # initth = tf.transformations.euler_from_quaternion(quaternion)[2]
119 
120 # def amclposeCallback(data):
121  # global goalth, initgoalth
122  # quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
123  # data.pose.pose.orientation.z, data.pose.pose.orientation.w )
124  # amclth = tf.transformations.euler_from_quaternion(quaternion)[2]
125  # goalth = initgoalth - amclth
126  # if goalth > math.pi:
127  # goalth = -math.pi*2 + goalth
128  # elif goalth < -math.pi:
129  # goalth = math.pi*2 + goalth
130 
131 def move(ox, oy, oth, tx, ty, tth, gth):
132  global followpath, goalpose, tfth
133 
134  # print "odom: "+str(ox)+", "+str(oy)+", "+str(oth)
135  # print "target: "+str(tx)+", "+str(ty)+", "+str(tth)
136 
137  distance = 0
138  if followpath:
139  dx = tx - ox
140  dy = ty - oy
141  distance = math.sqrt( pow(dx,2) + pow(dy,2) )
142 
143  if distance > 0:
144  th = math.acos(dx/distance)
145  if dy <0:
146  th = -th
147  elif goalpose:
148  th = gth - tfth
149  else:
150  th = tth
151 
152  #if goalpose:
153  #th = gth
154  #distance = 0
155 
156  dth = th - oth
157  if dth > math.pi:
158  dth = -math.pi*2 + dth
159  elif dth < -math.pi:
160  dth = math.pi*2 + dth
161 
162  # force minimums
163  if distance > 0 and distance < 0.05:
164  distance = 0.05
165 
166  # supposed to reduce zig zagging
167  if dth < minturn*0.3 and dth > -minturn*0.3:
168  dth = 0
169  elif dth >= minturn*0.3 and dth < minturn:
170  dth = minturn
171  elif dth <= -minturn*0.3 and dth > -minturn:
172  dth = -minturn
173 
174 
175  if dth > 0:
176  oculusprimesocket.sendString("speed "+str(turnspeed) )
177  oculusprimesocket.sendString("move left")
178  rospy.sleep(dth/(2.0*math.pi) * secondspertwopi)
179  oculusprimesocket.sendString("move stop")
180  oculusprimesocket.waitForReplySearch("<state> direction stop")
181  elif dth < 0:
182  oculusprimesocket.sendString("speed "+str(turnspeed) )
183  oculusprimesocket.sendString("move right")
184  rospy.sleep(-dth/(2.0*math.pi) * secondspertwopi)
185  oculusprimesocket.sendString("move stop")
186  oculusprimesocket.waitForReplySearch("<state> direction stop")
187 
188  if distance > 0:
189  oculusprimesocket.sendString("speed "+str(linearspeed) )
190  oculusprimesocket.sendString("move forward")
191  rospy.sleep(distance*secondspermeter)
192  oculusprimesocket.sendString("move stop")
193  oculusprimesocket.waitForReplySearch("<state> direction stop")
194 
195 def cleanup():
196  oculusprimesocket.sendString("odometrystop")
197  oculusprimesocket.sendString("state stopbetweenmoves false")
198  oculusprimesocket.sendString("move stop")
199 
200 
201 # MAIN
202 
203 rospy.init_node('base_controller', anonymous=False)
204 rospy.Subscriber("move_base/TrajectoryPlannerROS/local_plan", Path, pathCallback)
205 rospy.Subscriber("odom", Odometry, odomCallback)
206 rospy.Subscriber("move_base_simple/goal", PoseStamped, goalCallback)
207 rospy.Subscriber("move_base/status", GoalStatusArray, goalStatusCallback)
208 # rospy.Subscriber("initialpose", PoseWithCovarianceStamped, initialposeCallback)
209 # rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclposeCallback)
210 rospy.on_shutdown(cleanup)
211 listener = tf.TransformListener()
212 
213 while not rospy.is_shutdown():
214  t = rospy.get_time()
215 
216  if t >= nextmove and goalseek:
217  move(odomx, odomy, odomth, targetx, targety, targetth, goalth)
218  nextmove = t + listentime
219  # nextmove = rospy.get_time()+listentime
220  followpath = False
221 
222  if t - lastpath > 3:
223  goalpose = True
224 
225  try:
226  (trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
227  except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
228  continue
229  quaternion = (rot[0], rot[1], rot[2], rot[3])
230  tfth = tf.transformations.euler_from_quaternion(quaternion)[2]
231 
232 cleanup()
msg
oculusprimesocket.waitForReplySearch
def waitForReplySearch(pattern)
Definition: oculusprimesocket.py:36
segmented_arc_base_controller.move
def move(ox, oy, oth, tx, ty, tth, gth)
Definition: segmented_arc_base_controller.py:131
segmented_arc_base_controller.goalStatusCallback
def goalStatusCallback(data)
Definition: segmented_arc_base_controller.py:105
segmented_arc_base_controller.pathCallback
def pathCallback(data)
Definition: segmented_arc_base_controller.py:58
segmented_arc_base_controller.goalCallback
def goalCallback(data)
Definition: segmented_arc_base_controller.py:78
segmented_arc_base_controller.cleanup
def cleanup()
Definition: segmented_arc_base_controller.py:195
segmented_arc_base_controller.odomCallback
def odomCallback(data)
Definition: segmented_arc_base_controller.py:70

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