roboclaw_pyscript.py 文件源码

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()