def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.addressBackMotors, 0)
roboclaw.ForwardM2(self.addressBackMotors, 0)
roboclaw.ForwardM1(self.addressFrontMotors, 0)
roboclaw.ForwardM2(self.addressFrontMotors, 0)
else:
roboclaw.SpeedM1M2(self.addressBackMotors, vr_ticks, vl_ticks)
roboclaw.SpeedM1M2(self.addressFrontMotors, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.addressBackMotors, 0)
roboclaw.ForwardM2(self.addressBackMotors, 0)
roboclaw.ForwardM1(self.addressFrontMotors, 0)
roboclaw.ForwardM2(self.addressFrontMotors, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, 1, None
status2, enc2, crc2 = None, 1, None
status3, enc3, crc3 = None, 1, None
status4, enc4, crc4 = None, 1, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.addressBackMotors)
status3, enc3, crc3 = roboclaw.ReadEncM1(self.addressFrontMotors)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.addressBackMotors)
status4, enc4, crc4 = roboclaw.ReadEncM2(self.addressFrontMotors)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars()):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
if ('enc3' in vars()) and ('enc4' in vars()):
rospy.logdebug(" Encoders %d %d" % (enc3, enc4))
self.encodm.update_publish(enc3, enc4)
self.updater.update()
r_time.sleep()
def shutdown(self):
rospy.loginfo("Shutting down")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
with self.lock:
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get comand for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
statusC, amp1, amp2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
try:
status1c, amp1, amp2 = roboclaw.ReadCurrents(self.address)
except ValueError:
pass
if (enc1 != None) & (enc2 != None):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
else:
rospy.logdebug("Error Reading enc")
if (amp1 != None) & (amp2 != None):
rospy.logdebug(" Currents %d %d" % (amp1, amp2))
amps=Motors_currents()
amps.motor1=float(amp1)/100
amps.motor2=float(amp2)/100
self.motors_currents_pub.publish(amps)
else:
rospy.logdebug("Error Reading Currents")
r_time.sleep()