def connect(self, dev_name, baud_rate, address, test_mode=False):
"""Connects the node to the Roboclaw controller, or the test stub
Parameters:
:param str dev_name: Serial device name (e.g. /dev/ttyACM0)
:param int baud_rate: Serial baud rate (e.g. 115200)
:param int address: Serial address (default 0x80)
:param bool test_mode: if connecting to the controller test stub
"""
rospy.loginfo("Connecting to roboclaw")
if not test_mode:
roboclaw = Roboclaw(dev_name, baud_rate)
else:
roboclaw = RoboclawStub(dev_name, baud_rate)
self._rbc_ctls.append(RoboclawControl(roboclaw, address))
def __init__(self):
"""init variables and ros stuff"""
self._have_shown_message = False # flag to not spam logging
# ints in [-127,127] that get sent to roboclaw to drive forward or backward
self.curr_drive1_cmd = 0
self.curr_drive2_cmd = 0
#rospy.init_node("roboclaw_node")
rospy.init_node("roboclaw_node", log_level=rospy.INFO)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev = rospy.get_param("~dev")
self.baudrate = int(rospy.get_param("~baudrate"))
self.frontaddr = int(rospy.get_param("~frontaddr"))
self.backaddr = int(rospy.get_param("~backaddr"))
self.diggeraddr = int(rospy.get_param("~diggeraddr"))
# open roboclaw serial device connection
self.roboclaw = Roboclaw(self.dev, self.baudrate)
# diagnostics
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.pub_vitals))
# TODO (p1): we probably just want to crash here or something if we don't have a connection
try:
version = self.roboclaw.ReadVersion(self.frontaddr)
rospy.logdebug("Front Version " + str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.backaddr)
rospy.logdebug("Back Version "+ str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting back roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.diggeraddr)
rospy.logdebug("Digger Version "+ str(repr(version[1])))
self.roboclaw.SetM1EncoderMode(self.diggeraddr, 1)
self.roboclaw.SetM2EncoderMode(self.diggeraddr, 1)
rospy.logdebug("Digger Encoder Mode "+ str(self.roboclaw.ReadEncoderModes(self.diggeraddr)))
except Exception as e:
rospy.logwarn("Problem getting digger roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
self.roboclaw.SpeedM1M2(self.frontaddr, 0, 0)
self.roboclaw.ResetEncoders(self.frontaddr)
self.roboclaw.SpeedM1M2(self.backaddr, 0, 0)
self.roboclaw.ResetEncoders(self.backaddr)
# TODO (p2): test resetting
#self.roboclaw.SpeedM1M2(self.diggeraddr, 0, 0)
#self.roboclaw.ResetEncoders(self.diggeraddr)
self.LINEAR_MAX_SPEED = float(rospy.get_param("~linear/x/max_velocity"))
self.ANGULAR_MAX_SPEED = float(rospy.get_param("~angular/z/max_velocity"))
self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter"))
self.BASE_WIDTH = float(rospy.get_param("~base_width"))
self.TIMEOUT = float(rospy.get_param("~timeout"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_vel_cmd_time = rospy.Time.now()
self.last_digger_cmd_time = rospy.Time.now()
self.last_digger_extended_time = rospy.Time.now()
self.digger_extended = False
self.cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
self.digger_sub = rospy.Subscriber("/digger_spin/cmd", Float32, self.digger_spin_callback, queue_size=1)
self.digger_extended_sub = rospy.Subscriber("/digger_extended", Bool, self.digger_extended_callback, queue_size=1)
rospy.sleep(1) # wait for things to initialize
rospy.logdebug("dev %s", self.dev)
rospy.logdebug("baudrate %d", self.baudrate)
rospy.logdebug("front address %d", self.frontaddr)
rospy.logdebug("back address %d", self.backaddr)
rospy.logdebug("digger address %d", self.diggeraddr)
rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
rospy.logdebug("timeout %f", self.TIMEOUT)
#!/usr/bin/env python
from __future__ import print_function
import sys
from roboclaw_driver import Roboclaw
if len(sys.argv) == 1:
print()
print("Example Usage: {} /dev/ttyACM0".format(sys.argv[0]))
print()
exit()
dev_name = sys.argv[1]
baud_rate = 115200
address = 0x80
roboclaw = Roboclaw(dev_name, baud_rate)
roboclaw.Open()
temp_response = roboclaw.ReadTemp(address)[1] / 10.0
volts_response = roboclaw.ReadMainBatteryVoltage(address)[1] / 10.0
print("Reading Roboclaw at: {}".format(dev_name))
print("Temp: {}C, Battery: {}V".format(temp_response, volts_response))
class Node(object):
""" Class for running roboclaw ros node for 2 motors in a diff drive setup"""
def __init__(self):
"""init variables and ros stuff"""
self._have_shown_message = False # flag to not spam logging
# ints in [-127,127] that get sent to roboclaw to drive forward or backward
self.curr_drive1_cmd = 0
self.curr_drive2_cmd = 0
#rospy.init_node("roboclaw_node")
rospy.init_node("roboclaw_node", log_level=rospy.INFO)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev = rospy.get_param("~dev")
self.baudrate = int(rospy.get_param("~baudrate"))
self.frontaddr = int(rospy.get_param("~frontaddr"))
self.backaddr = int(rospy.get_param("~backaddr"))
self.diggeraddr = int(rospy.get_param("~diggeraddr"))
# open roboclaw serial device connection
self.roboclaw = Roboclaw(self.dev, self.baudrate)
# diagnostics
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.pub_vitals))
# TODO (p1): we probably just want to crash here or something if we don't have a connection
try:
version = self.roboclaw.ReadVersion(self.frontaddr)
rospy.logdebug("Front Version " + str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.backaddr)
rospy.logdebug("Back Version "+ str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting back roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.diggeraddr)
rospy.logdebug("Digger Version "+ str(repr(version[1])))
self.roboclaw.SetM1EncoderMode(self.diggeraddr, 1)
self.roboclaw.SetM2EncoderMode(self.diggeraddr, 1)
rospy.logdebug("Digger Encoder Mode "+ str(self.roboclaw.ReadEncoderModes(self.diggeraddr)))
except Exception as e:
rospy.logwarn("Problem getting digger roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
self.roboclaw.SpeedM1M2(self.frontaddr, 0, 0)
self.roboclaw.ResetEncoders(self.frontaddr)
self.roboclaw.SpeedM1M2(self.backaddr, 0, 0)
self.roboclaw.ResetEncoders(self.backaddr)
# TODO (p2): test resetting
#self.roboclaw.SpeedM1M2(self.diggeraddr, 0, 0)
#self.roboclaw.ResetEncoders(self.diggeraddr)
self.LINEAR_MAX_SPEED = float(rospy.get_param("~linear/x/max_velocity"))
self.ANGULAR_MAX_SPEED = float(rospy.get_param("~angular/z/max_velocity"))
self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter"))
self.BASE_WIDTH = float(rospy.get_param("~base_width"))
self.TIMEOUT = float(rospy.get_param("~timeout"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_vel_cmd_time = rospy.Time.now()
self.last_digger_cmd_time = rospy.Time.now()
self.last_digger_extended_time = rospy.Time.now()
self.digger_extended = False
self.cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
self.digger_sub = rospy.Subscriber("/digger_spin/cmd", Float32, self.digger_spin_callback, queue_size=1)
self.digger_extended_sub = rospy.Subscriber("/digger_extended", Bool, self.digger_extended_callback, queue_size=1)
rospy.sleep(1) # wait for things to initialize
rospy.logdebug("dev %s", self.dev)
rospy.logdebug("baudrate %d", self.baudrate)
rospy.logdebug("front address %d", self.frontaddr)
rospy.logdebug("back address %d", self.backaddr)
rospy.logdebug("digger address %d", self.diggeraddr)
rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
rospy.logdebug("timeout %f", self.TIMEOUT)
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
# do watchdog checks to shut down motors if we haven't heard anything in awhile
if (rospy.Time.now() - self.last_vel_cmd_time).to_sec() > self.TIMEOUT:
self.curr_drive1_cmd = 0
self.curr_drive2_cmd = 0
if (not self._have_shown_message):
rospy.loginfo("Did not get drive command for %d second, stopping", self.TIMEOUT)
self._have_shown_message = True
else:
self._have_shown_message = False
if (rospy.Time.now() - self.last_digger_cmd_time).to_sec() > self.TIMEOUT:
self.curr_digger_cmd = 0
if (not self._have_shown_message):
rospy.loginfo("Did not get digger command for %d second, stopping", self.TIMEOUT)
self._have_shown_message = True
else:
self._have_shown_message = False
# send actual commands to the devices (the values to send are generally set in sub callbacks)
self._send_drive_cmd()
self._send_digger_cmd()
# read encoder data and publish odom
self.front_enc1, self.front_enc2 = None, None
self.back_enc1, self.back_enc2 = None, None
self.digger_current1, self.digger_current2 = None, None
try:
pass
_, self.digger_current1, _ = self.roboclaw.ReadEncM1(self.diggeraddr)
_, self.digger_current2, _ = self.roboclaw.ReadEncM2(self.diggeraddr)
_, self.front_enc1, _ = self.roboclaw.ReadEncM1(self.frontaddr) # returns (status, ENCODER, crc) -> (_, enc, _)
_, self.front_enc2, _ = self.roboclaw.ReadEncM2(self.frontaddr)
_, self.back_enc1, _ = self.roboclaw.ReadEncM1(self.backaddr)
_, self.back_enc2, _ = self.roboclaw.ReadEncM2(self.backaddr)
except (ValueError,OSError) as e:
rospy.logwarn("Error when trying to read encoder value: %s", e)
if self.digger_current1 is not None:
rospy.logdebug("Digger Current %d %d", self.digger_current1, self.digger_current2)
if self.front_enc1 is not None and self.front_enc2 is not None and self.back_enc1 and self.back_enc2 is not None:
rospy.logdebug("Front Encoders %d %d", self.front_enc1, self.front_enc2)
rospy.logdebug("Back Encoders %d %d", self.back_enc1, self.back_enc2)
self.encodm.update_publish(self.front_enc1, self.front_enc2, self.back_enc1, self.back_enc2)
# publish diagnostics
self._read_vitals()
self.updater.update()
r_time.sleep()
def digger_extended_callback(self, msg):
self.digger_extended = msg.data
self.last_digger_extended_time = rospy.Time.now()
def digger_spin_callback(self, cmd):
"""Set digger command based on the float message in range (-1, 1)"""
self.last_digger_cmd_time = rospy.Time.now()
rospy.logdebug("Digger message: %f", cmd.data)
# Scale to motor pwm and clip
motor_cmd = int(cmd.data * 127)
motor_cmd = clip(motor_cmd, -127, 127)
self.curr_digger_cmd = motor_cmd
def _send_digger_cmd(self):
"""Sends the current digger command to the Roboclaw devices over Serial"""
# TODO: test this code more thoroughly
# If the linear actuator is not extended, or we haven't heard from it in a while,
# set the digger speed to 0
#if not self.digger_extended or ( (rospy.Time.now() - self.last_digger_extended_time).to_sec > self.timeout):
# self.curr_digger_cmd = 0
#else:
# self.curr_digger_cmd = self.curr_digger_cmd
# TODO: need to check the directionality of these when we hook up everything
try:
if self.curr_digger_cmd >= 0:
self.roboclaw.ForwardM1(self.diggeraddr, self.curr_digger_cmd)
self.roboclaw.BackwardM2(self.diggeraddr, self.curr_digger_cmd)
else:
self.roboclaw.BackwardM1(self.diggeraddr, -self.curr_digger_cmd)
self.roboclaw.ForwardM2(self.diggeraddr, -self.curr_digger_cmd)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
"""Set motor command based on the incoming twist message"""
self.last_vel_cmd_time = rospy.Time.now()
rospy.logdebug("Twist: -Linear X: %d -Angular Z: %d", twist.linear.x, twist.angular.z)
linear_x = -twist.linear.x
angular_z = twist.angular.z
if linear_x > self.LINEAR_MAX_SPEED:
linear_x = self.LINEAR_MAX_SPEED
elif linear_x < -self.LINEAR_MAX_SPEED:
linear_x = -self.LINEAR_MAX_SPEED
# Take linear x and angular z values and compute command
drive1_cmd = linear_x/self.LINEAR_MAX_SPEED + angular_z/self.ANGULAR_MAX_SPEED
drive2_cmd = linear_x/self.LINEAR_MAX_SPEED - angular_z/self.ANGULAR_MAX_SPEED
# Scale to motor pwm
drive1_cmd = int(drive1_cmd * 127)
drive2_cmd = int(drive2_cmd * 127)
# Clip to command bounds
drive1_cmd = clip(drive1_cmd, -127, 127)
drive2_cmd = clip(drive2_cmd, -127, 127)
# update the current commands
self.curr_drive1_cmd = drive1_cmd
self.curr_drive2_cmd = drive2_cmd
rospy.logdebug("drive1 command = %d", self.curr_drive1_cmd)
rospy.logdebug("drive2 command = %d", self.curr_drive2_cmd)
def _send_drive_cmd(self):
"""Sends the current motor commands to the Roboclaw devices over Serial"""
try:
if self.curr_drive1_cmd >= 0:
self.roboclaw.ForwardM1(self.frontaddr, self.curr_drive1_cmd)
self.roboclaw.ForwardM1(self.backaddr, self.curr_drive1_cmd)
else:
self.roboclaw.BackwardM1(self.frontaddr, -self.curr_drive1_cmd)
self.roboclaw.BackwardM1(self.backaddr, -self.curr_drive1_cmd)
if self.curr_drive2_cmd >= 0:
self.roboclaw.BackwardM2(self.frontaddr, self.curr_drive2_cmd)
self.roboclaw.BackwardM2(self.backaddr, self.curr_drive2_cmd)
else:
self.roboclaw.ForwardM2(self.frontaddr, -self.curr_drive2_cmd)
self.roboclaw.ForwardM2(self.backaddr, -self.curr_drive2_cmd)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
def _decode_error(self, bits):
ERRORS = {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")}
# no error
if bits == 0x0000:
return [(diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal")]
summary = []
for ebit in ERRORS:
if bits and ebits:
summary.append(ERRORS[ebits])
return summary
def _read_vitals(self):
"""Check battery voltage and temperatures from roboclaw"""
try:
statusfront = self.roboclaw.ReadError(self.frontaddr)[1]
statusback = self.roboclaw.ReadError(self.backaddr)[1]
statusdigger = self.roboclaw.ReadError(self.diggeraddr)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
self.state_fronts = self._decode_error(statusfront)
self.state_backs = self._decode_error(statusback)
self.state_digger = self._decode_error(statusdigger)
try:
# read main and logic voltages
self.front_voltage = float(self.roboclaw.ReadMainBatteryVoltage(self.frontaddr)[1] / 10)
self.front_logic = float(self.roboclaw.ReadLogicBatteryVoltage(self.frontaddr)[1] / 10)
self.back_voltage = float(self.roboclaw.ReadMainBatteryVoltage(self.backaddr)[1] / 10)
self.back_logic = float(self.roboclaw.ReadLogicBatteryVoltage(self.frontaddr)[1] / 10)
self.digger_voltage = float(self.roboclaw.ReadMainBatteryVoltage(self.diggeraddr)[1] / 10)
self.digger_logic = float(self.roboclaw.ReadLogicBatteryVoltage(self.diggeraddr)[1] / 10)
# read currents
self.front_currents = self.roboclaw.ReadCurrents(self.frontaddr)
self.back_currents = self.roboclaw.ReadCurrents(self.backaddr)
self.digger_currents = self.roboclaw.ReadCurrents(self.diggeraddr)
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
def pub_vitals(self, stat):
"""Publish vitals (called by diagnostics updater)"""
for front_state, front_message in self.state_fronts:
stat.summary(front_state, front_message)
for back_state, back_message in self.state_backs:
stat.summary(back_state, back_message)
for digger_state, digger_message in self.state_digger:
stat.summary(digger_state, digger_message)
stat.add("Front Main Batt V:", self.front_voltage)
stat.add("Front Logic Batt V:", self.front_logic)
stat.add("Back Main Batt V:", self.back_voltage)
stat.add("Back Logic Batt V:", self.back_logic)
stat.add("Digger Main Batt V:", self.digger_voltage)
stat.add("Digger Logic Batt V:", self.digger_logic)
stat.add("Front Left Current:", float(self.front_currents[1] / 100))
stat.add("Front Right Current:", float(self.front_currents[2] / 100))
stat.add("Back Left Current:", float(self.back_currents[1] / 100))
stat.add("Back Right Current:", float(self.back_currents[2] / 100))
stat.add("Digger Left Current:", float(self.digger_currents[1] / 100))
stat.add("Digger Right Current:", float(self.digger_currents[2] / 100))
return stat
def shutdown(self):
"""Handle shutting down the node"""
rospy.loginfo("Shutting down")
# so they don't get called after we're dead
if hasattr(self, "cmd_vel_sub"):
self.cmd_vel_sub.unregister()
if hasattr(self, "digger_sub"):
self.digger_sub.unregister()
# stop motors
try:
self.roboclaw.ForwardM1(self.frontaddr, 0)
self.roboclaw.ForwardM2(self.frontaddr, 0)
self.roboclaw.ForwardM1(self.backaddr, 0)
self.roboclaw.ForwardM2(self.backaddr, 0)
self.roboclaw.ForwardM1(self.diggeraddr, 0)
self.roboclaw.ForwardM2(self.diggeraddr, 0)
rospy.loginfo("Closed Roboclaw serial connection")
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
self.roboclaw.ForwardM1(self.frontaddr, 0)
self.roboclaw.ForwardM2(self.frontaddr, 0)
self.roboclaw.ForwardM1(self.backaddr, 0)
self.roboclaw.ForwardM2(self.backaddr, 0)
self.roboclaw.ForwardM1(self.diggeraddr, 0)
self.roboclaw.ForwardM2(self.diggeraddr, 0)
except OSError as e:
rospy.logfatal("Could not shutdown motors!!!!")
rospy.logfatal(e)