diff_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4  diff_controller.py - controller for a differential drive
5  Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
6 
7  Redistribution and use in source and binary forms, with or without
8  modification, are permitted provided that the following conditions are met:
9  * Redistributions of source code must retain the above copyright
10  notice, this list of conditions and the following disclaimer.
11  * Redistributions in binary form must reproduce the above copyright
12  notice, this list of conditions and the following disclaimer in the
13  documentation and/or other materials provided with the distribution.
14  * Neither the name of Vanadium Labs LLC nor the names of its
15  contributors may be used to endorse or promote products derived
16  from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21  DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
24  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
25  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
27  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 """
29 
30 import rospy
31 
32 from math import sin,cos,pi
33 
34 from geometry_msgs.msg import Quaternion
35 from geometry_msgs.msg import Twist
36 from nav_msgs.msg import Odometry
37 from diagnostic_msgs.msg import *
38 from tf.broadcaster import TransformBroadcaster
39 
40 from ax12 import *
41 from controllers import *
42 from struct import unpack
43 
45  """ Controller to handle movement & odometry feedback for a differential
46  drive mobile base. """
47  def __init__(self, device, name):
48  Controller.__init__(self, device, name)
49  self.pause = True
50  self.last_cmd = rospy.Time.now()
51 
52  # parameters: rates and geometry
53  self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
54  self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
55  self.t_delta = rospy.Duration(1.0/self.rate)
56  self.t_next = rospy.Time.now() + self.t_delta
57  self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
58  self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
59 
60  self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
61  self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
62 
63  # parameters: PID
64  self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
65  self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
66  self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
67  self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
68 
69  # parameters: acceleration
70  self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
71  self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
72 
73  # output for joint states publisher
74  self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"]
75  self.joint_positions = [0,0]
76  self.joint_velocities = [0,0]
77 
78  # internal data
79  self.v_left = 0 # current setpoint velocity
80  self.v_right = 0
81  self.v_des_left = 0 # cmd_vel setpoint
82  self.v_des_right = 0
83  self.enc_left = None # encoder readings
84  self.enc_right = None
85  self.x = 0 # position in xy plane
86  self.y = 0
87  self.th = 0
88  self.dx = 0 # speeds in x/rotation
89  self.dr = 0
90  self.then = rospy.Time.now() # time for determining dx/dy
91 
92  # subscriptions
93  rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
94  self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
95  self.odomBroadcaster = TransformBroadcaster()
96 
97  rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
98 
99  def startup(self):
100  if not self.fake:
101  self.setup(self.Kp,self.Kd,self.Ki,self.Ko)
102 
103  def update(self):
104  now = rospy.Time.now()
105  if now > self.t_next:
106  elapsed = now - self.then
107  self.then = now
108  elapsed = elapsed.to_sec()
109 
110  if self.fake:
111  x = cos(self.th)*self.dx*elapsed
112  y = -sin(self.th)*self.dx*elapsed
113  self.x += cos(self.th)*self.dx*elapsed
114  self.y += sin(self.th)*self.dx*elapsed
115  self.th += self.dr*elapsed
116  else:
117  # read encoders
118  try:
119  left, right = self.status()
120  except Exception as e:
121  rospy.logerr("Could not update encoders: " + str(e))
122  return
123  rospy.logdebug("Encoders: " + str(left) +","+ str(right))
124 
125  # calculate odometry
126  if self.enc_left == None:
127  d_left = 0
128  d_right = 0
129  else:
130  d_left = (left - self.enc_left)/self.ticks_meter
131  d_right = (right - self.enc_right)/self.ticks_meter
132  self.enc_left = left
133  self.enc_right = right
134 
135  d = (d_left+d_right)/2
136  th = (d_right-d_left)/self.base_width
137  self.dx = d / elapsed
138  self.dr = th / elapsed
139 
140  if (d != 0):
141  x = cos(th)*d
142  y = -sin(th)*d
143  self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
144  self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
145  if (th != 0):
146  self.th = self.th + th
147 
148  # publish or perish
149  quaternion = Quaternion()
150  quaternion.x = 0.0
151  quaternion.y = 0.0
152  quaternion.z = sin(self.th/2)
153  quaternion.w = cos(self.th/2)
154  self.odomBroadcaster.sendTransform(
155  (self.x, self.y, 0),
156  (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
157  rospy.Time.now(),
158  self.base_frame_id,
159  self.odom_frame_id
160  )
161 
162  odom = Odometry()
163  odom.header.stamp = now
164  odom.header.frame_id = self.odom_frame_id
165  odom.pose.pose.position.x = self.x
166  odom.pose.pose.position.y = self.y
167  odom.pose.pose.position.z = 0
168  odom.pose.pose.orientation = quaternion
169  odom.child_frame_id = self.base_frame_id
170  odom.twist.twist.linear.x = self.dx
171  odom.twist.twist.linear.y = 0
172  odom.twist.twist.angular.z = self.dr
173  self.odomPub.publish(odom)
174 
175  if now > (self.last_cmd + rospy.Duration(self.timeout)):
176  self.v_des_left = 0
177  self.v_des_right = 0
178 
179  # update motors
180  if not self.fake:
181  if self.v_left < self.v_des_left:
182  self.v_left += self.max_accel
183  if self.v_left > self.v_des_left:
184  self.v_left = self.v_des_left
185  else:
186  self.v_left -= self.max_accel
187  if self.v_left < self.v_des_left:
188  self.v_left = self.v_des_left
189 
190  if self.v_right < self.v_des_right:
191  self.v_right += self.max_accel
192  if self.v_right > self.v_des_right:
193  self.v_right = self.v_des_right
194  else:
195  self.v_right -= self.max_accel
196  if self.v_right < self.v_des_right:
197  self.v_right = self.v_des_right
198  self.write(self.v_left, self.v_right)
199 
200  self.t_next = now + self.t_delta
201 
202  def shutdown(self):
203  if not self.fake:
204  self.write(0,0)
205 
206  def cmdVelCb(self,req):
207  """ Handle movement requests. """
208  self.last_cmd = rospy.Time.now()
209  if self.fake:
210  self.dx = req.linear.x # m/s
211  self.dr = req.angular.z # rad/s
212  else:
213  # set motor speeds in ticks per 1/30s
214  self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
215  self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
216 
217  def getDiagnostics(self):
218  """ Get a diagnostics status. """
219  msg = DiagnosticStatus()
220  msg.name = self.name
221  msg.level = DiagnosticStatus.OK
222  msg.message = "OK"
223  if not self.fake:
224  msg.values.append(KeyValue("Left", str(self.enc_left)))
225  msg.values.append(KeyValue("Right", str(self.enc_right)))
226  msg.values.append(KeyValue("dX", str(self.dx)))
227  msg.values.append(KeyValue("dR", str(self.dr)))
228  return msg
229 
230 
239 
240  def setup(self, kp, kd, ki, ko):
241  success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
242 
243  def write(self, left, right):
244  """ Send a closed-loop speed. Base PID loop runs at 30Hz, these values
245  are therefore in ticks per 1/30 second. """
246  left = left&0xffff
247  right = right&0xffff
248  success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
249 
250  def status(self):
251  """ read 32-bit (signed) encoder values. """
252  values = self.device.execute(253, AX_CONTROL_STAT, [10])
253  left_values = "".join([chr(k) for k in values[0:4] ])
254  right_values = "".join([chr(k) for k in values[4:] ])
255  try:
256  left = unpack('=l',left_values)[0]
257  right = unpack('=l',right_values)[0]
258  return [left, right]
259  except:
260  return None
261 
Controllers interact with ArbotiX hardware.
Definition: controllers.py:32
def setup(self, kp, kd, ki, ko)
Controller Specification:


arbotix_python
Author(s): Michael Ferguson
autogenerated on Mon Feb 28 2022 21:37:42