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 StopRover(): AccessRoboclaw() addresses = [128, 129, 130] for a in addresses: roboclaw.SpeedM1M2(a, 0, 0) roboclaw.ResetEncoders(a) addresses = [131, 132, 133] for a in addresses: roboclaw.SpeedM1M2(a, 0, 0)
def __init__(self):
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("tome_roboclaws")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
self.addressBackMotors = int(rospy.get_param("~addressBackMotors"))
self.addressFrontMotors = int(rospy.get_param("~addressFrontMotors"))
if self.addressBackMotors > 0x87 or self.addressBackMotors < 0x80:
rospy.logfatal("addressBackMotors out of range")
rospy.signal_shutdown("addressBackMotors out of range")
if self.addressFrontMotors > 0x87 or self.addressFrontMotors < 0x80:
rospy.logfatal("addressBackMotors out of range")
rospy.signal_shutdown("addressBackMotors out of range")
# TODO need someway to check if addressBackMotors is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals_back_motors))
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals_front_motors))
try:
versionBackMotor = roboclaw.ReadVersion(self.addressBackMotors)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
# rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not versionBackMotor[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(versionBackMotor[1]))
try:
versionFrontMotor = roboclaw.ReadVersion(self.addressFrontMotors)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
# rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not versionFrontMotor[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(versionFrontMotor[1]))
roboclaw.SpeedM1M2(self.addressBackMotors, 0, 0)
roboclaw.SpeedM1M2(self.addressFrontMotors, 0, 0)
roboclaw.ResetEncoders(self.addressBackMotors)
roboclaw.ResetEncoders(self.addressFrontMotors)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
self.TICKS_PER_METER = float(rospy.get_param("~tick_per_meter", "4342.2"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("addressBackMotors %d", self.addressBackMotors)
rospy.logdebug("addressFrontMotors %d", self.addressFrontMotors)
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
def CalibrateAbsoluteEncoders(): #better use the roboclaw software for that AccessRoboclaw() roboclaw.ResetEncoders(131) cal_speed = 100 #drive to positive limit pos roboclaw.SpeedM1M2(131, cal_speed, cal_speed) time.sleep(2) roboclaw.SpeedM1M2(132, cal_speed, cal_speed) time.sleep(2) # wait till position is reached roboclaw.SpeedM1M2(131, 0, 0) time.sleep(2) roboclaw.SpeedM1M2(132, 0, 0) time.sleep(2) #get limit positions while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_LEFT) if enc[1] != 0: break print "upper limit w1: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_RIGHT) if enc[1] != 0: break print "upper limit w4: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_LEFT) if enc[1] != 0: break print "upper limit w3: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_RIGHT) if enc[1] != 0: break print "upper limit w6: " + str(enc); #drive to positive limit pos roboclaw.SpeedM1M2(131, -cal_speed, -cal_speed) time.sleep(2) roboclaw.SpeedM1M2(132, -cal_speed, -cal_speed) time.sleep(2) # wait till position is reached roboclaw.SpeedM1M2(131, 0, 0) time.sleep(2) roboclaw.SpeedM1M2(132, 0, 0) time.sleep(2) #get limit positions while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_LEFT) if enc[1] != 0: break print "lower limit w1: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_RIGHT) if enc[1] != 0: break print "lower limit w4: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_LEFT) if enc[1] != 0: break print "lower limit w3: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_RIGHT) if enc[1] != 0: break print "lower limit w6: " + str(enc);
def __init__(self):
self.lock = threading.Lock()
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("roboclaw_node",log_level=rospy.DEBUG)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/roboclaw")
baud_rate = int(rospy.get_param("~baud", "460800"))
self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1)
self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1)
self.address = int(rospy.get_param("~address", "128"))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.
FunctionDiagnosticTask("Vitals", self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.ResetEncoders(self.address)
self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0"))
self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0"))
self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED)
rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
