class Node:
def __init__(self):
# TODO better way to deal bit mask
self.ERRORS = {
0x000000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x000001: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "E-Stop"),
0x000002:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x000004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x000008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main Voltage High"),
0x000010:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic Voltage High"),
0x000020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic Voltage Low"),
0x000040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M1 Driver Fault"),
0x000080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M2 Driver Fault"),
0x000100: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M1 Speed"),
0x000200: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M2 Speed"),
0x000400: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M1 Position"),
0x000800: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M2 Position"),
0x001000: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M1 Current"),
0x002000: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"M2 Current"),
0x010000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 Over Current"),
0x020000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 Over Current"),
0x040000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main Voltage High"),
0x080000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main Voltage Low"),
0x100000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x200000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x400000: (diagnostic_msgs.msg.DiagnosticStatus.OK,
"S4 Signal Triggered"),
0x800000: (diagnostic_msgs.msg.DiagnosticStatus.OK,
"S5 Signal Triggered"),
0x01000000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Speed Error Limit"),
0x02000000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Position Error Limit")
}
self.mainbattv = 0.0
self.logicbattv = 0.0
self.temp1c = 0.0
self.temp2c = 0.0
self.current1a = 0.0
self.current2a = 0.0
self.mainbatterystate = BatteryState()
#rospy.init_node("roboclaw_node", log_level=rospy.DEBUG)
rospy.init_node("roboclaw_node")
rospy.on_shutdown(self.shutdown)
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
self.address = int(rospy.get_param("~address", "128"))
rospy.loginfo(
"Connecting to RoboClaw with address {} on port {} and {} baud".
format(self.address, dev_name, baud_rate))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
self.encoders = rospy.get_param("~encoders", True)
self.roboclaw = Roboclaw(dev_name, baud_rate)
# TODO need someway to check if address is correct
try:
self.roboclaw.Open()
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
# We have a single 'roboclaw' object handling serial communications.
# We're about to launch different threads that each want to talk.
# 1 - Diagnostics thread calling into our self.check_vitals
# 2 - '/cmd_vel' thread calling our self.cmd_vel_callback
# 3 - our self.run that publishes to '/odom'
# To prevent thread collision in the middle of serial communication
# (which causes sync errors) all access to roboclaw from now
# must be synchronized using this mutually exclusive lock object.
self.mutex = threading.Lock()
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
with self.mutex:
version = self.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.loginfo("Version: {}".format(version[1].rstrip()))
with self.mutex:
self.roboclaw.SpeedM1M2(self.address, 0, 0)
self.roboclaw.ResetEncoders(self.address)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.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.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)
self.mainbatterystate_pub = rospy.Publisher('~mainbattery',
BatteryState,
queue_size=1)
# setup main battery state
self.mainbatterystate.header.frame_id = 'main_battery'
self.mainbatterystate.location = 'main battery screw terminal'
self.mainbatterystate.power_supply_status = 0 # 2 = DISCHARGING
self.mainbatterystate.power_supply_technology = 3 # LIPO
rospy.Timer(rospy.Duration(1.0), self.publish_batterystate)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("encoders %s", self.encoders)
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 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.logdebug("Did not get command for 1 second, stopping")
rospy.logwarn_throttle(
1, "Did not get command for 1 second, stopping")
try:
with self.mutex:
self.roboclaw.ForwardM1(self.address, 0)
self.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
try:
with self.mutex:
status1, enc1, crc1 = self.roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
with self.mutex:
status2, enc2, crc2 = self.roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if (isinstance(enc1, Number) and isinstance(enc2, Number)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(
enc2, enc1) # update_publish(enc_left, enc_right)
self.updater.update()
r_time.sleep()
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:
with self.mutex:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
elif self.encoders:
with self.mutex:
self.roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
else:
with self.mutex:
self.roboclaw.DutyM1M2(self.address, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
# TODO: Need to make this work when more than one error is raised
def check_vitals(self, stat):
try:
with self.mutex:
status = self.roboclaw.ReadError(self.address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
try:
state, message = self.ERRORS[status]
except KeyError:
state = diagnostic_msgs.msg.DiagnosticStatus.ERROR
message = "Unknown or various errors: 0x{0:x}".format(status)
stat.summary(state, message)
try:
with self.mutex:
self.mainbattv = float(
self.roboclaw.ReadMainBatteryVoltage(self.address)[1]) / 10
self.logicbattv = float(
self.roboclaw.ReadLogicBatteryVoltage(
self.address)[1]) / 10
self.temp1c = float(self.roboclaw.ReadTemp(
self.address)[1]) / 10
self.temp2c = float(self.roboclaw.ReadTemp2(
self.address)[1]) / 10
self.current1a = float(
self.roboclaw.ReadCurrents(self.address)[1]) / 100
self.current2a = float(
self.roboclaw.ReadCurrents(self.address)[2]) / 100
stat.add("Main Batt V:", self.mainbattv)
stat.add("Logic Batt V:", self.logicbattv)
stat.add("Temp1 C:", self.temp1c)
stat.add("Temp2 C:", self.temp2c)
stat.add("Current 1:", self.current1a)
stat.add("Current 2:", self.current2a)
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
# TODO: better battery health and state
def publish_batterystate(self, event):
self.mainbatterystate.header.stamp = rospy.Time.now()
self.mainbatterystate.voltage = self.mainbattv
if (self.mainbattv >= 5):
self.mainbatterystate.present = True
else:
self.mainbatterystate.present = False
# if (volts<=11.2):
# self.mainbatterystate.power_supply_health = 3
# else:
# self.mainbatterystate.power_supply_health = 1
self.mainbatterystate_pub.publish(self.mainbatterystate)
# TODO: need clean shutdown so motors stop even if new msgs are arriving
def shutdown(self):
rospy.loginfo("Shutting down")
try:
with self.mutex:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
with self.mutex:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
class Node:
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("roboclaw_node", log_level=rospy.DEBUG)
rospy.on_shutdown(
self.shutdown) # shutdown signal will trigger shutdown function
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
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")
self.rc = Roboclaw(dev_name, baud_rate)
# TODO need someway to check if address is correct
try:
self.rc.Open()
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 = self.rc.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]))
self.rc.SpeedM1M2(self.address, 0, 0)
self.rc.ResetEncoders(self.address)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.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.TWIST_COMMAND = rospy.get_param("~twist_command",
"roboclaw/cmd_vel")
self.SINGLE_MOTOR = bool(rospy.get_param("~single_motor", "false"))
self.PUBLISH_TF = bool(rospy.get_param("~publish_tf", "true"))
self.CHILD_FRAME = rospy.get_param("~child_frame", "base_link")
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH,
self.PUBLISH_TF, self.CHILD_FRAME)
self.last_set_speed_time = rospy.get_rostime()
# queue_size = 1 to make sure Roboclaw stops spining
# this was an issue if cmd_vel frequency was too high 1000/sec
# using swri_ropspy.Subscriber() swri_ropspy.Timer() for single threaded callbacks
# multithreaded cmd_callback would eventually cause crash while the while-loop was reading encoders
# https://github.com/swri-robotics/marti_common/blob/master/swri_rospy/nodes/single_threaded_example
swri_rospy.Subscriber(self.TWIST_COMMAND,
Twist,
self.cmd_vel_callback,
queue_size=2)
swri_rospy.Timer(rospy.Duration(0.1), self.timer_callback)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
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)
rospy.logdebug("twist_command %s", self.TWIST_COMMAND)
rospy.logdebug("single_motor %d", self.SINGLE_MOTOR)
rospy.logdebug("publish_tf %d", self.PUBLISH_TF)
rospy.logdebug("child_frame %s", self.CHILD_FRAME)
def run(self):
# TODO
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(5)
# see timer_callback()
# spin() processes the callback queue until the node is stopped
# this replaces the <while not rospy.is_shutdown()> loop now that reading encoders is
# implemented in timer_callback()
rospy.spin()
def timer_callback(self, event):
# orignal <while not rospy.is_shutdown():> loop tobe rewritten here
#rospy.loginfo(rospy.get_caller_id() + " timer_callback() started")
# stop motors if last twist topic recieved is older than 1 sec
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.logwarn("Did not get command for 1 second, stopping")
try:
self.rc.ForwardM1(self.address, 0)
self.rc.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
# read encoder 1 *************************************************
rospy.logdebug("reading encoder 1, right motor")
try:
status1, enc1, crc1 = self.rc.ReadEncM1(self.address)
rospy.logdebug("status1 : %s", status1)
rospy.logdebug("enc1 : %s", enc1)
rospy.logdebug("crc1 : %s", crc1)
if not isinstance(enc1, int):
rospy.logwarn(
"enc1 is not integer, self.rc.ReadEncM1() not reading properly"
)
except ValueError:
rospy.logwarn("enc1 value error")
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
# read encoder 2 *************************************************
rospy.logdebug("reading encoder 2, left motor")
try:
status2, enc2, crc2 = self.rc.ReadEncM2(self.address)
rospy.logdebug("status2 : %s", status2)
rospy.logdebug("enc2 : %s", enc2)
rospy.logdebug("crc2 : %s", crc2)
if not isinstance(enc2, int):
rospy.logwarn(
"enc2 is not integer, self.rc.ReadEncM2() not reading properly"
)
except ValueError:
rospy.logwarn("enc2 value error")
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
# update odom *************************************************
if ('enc1' in vars()) and ('enc2' in vars()):
rospy.logdebug("enc1: %s enc2: %s", enc1, enc2)
rospy.logdebug("updating odom")
self.encodm.update_publish(enc1, enc2)
self.updater.update()
else:
rospy.logdebug("encoder not in vars")
#rospy.loginfo(rospy.get_caller_id() + "timer_callback() done")
def cmd_vel_callback(self, twist):
# Make sure this callback only occurs after reading encoders
# and only once or not at all
self.last_set_speed_time = rospy.get_rostime()
rospy.logdebug("twist message recieved: linear_x = %f angluar_z = %f",
twist.linear.x, twist.angular.z)
# Apply max speed input correction
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
rospy.logdebug("Max speed correction: linear_x = %f",
twist.linear.x)
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
rospy.logdebug("Max speed correction: linear_x = %f",
twist.linear.x)
# if Dual motor differential driver mode
if self.SINGLE_MOTOR == 0:
# a postive yaw angle should turn left meaning...
# the right motor M2 must turn faster and...
# the left motor M1 must turn slower
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 ba hack way to keep a poorly tuned PID from making noise at speed 0
# FIX getting a back callback with status1 = self.rc.ForwardM1(self.address, 0)
if vr_ticks is 0 and vl_ticks is 0:
status1 = self.rc.ForwardM1(self.address, 0)
status2 = self.rc.ForwardM2(self.address, 0)
rospy.logdebug("self.rc.ForwardM1(self.address, 0) : %s",
status1)
rospy.logdebug("self.rc.ForwardM2(self.address, 0) : %s",
status2)
else:
status1 = self.rc.SpeedM1M2(self.address, vl_ticks,
vr_ticks)
rospy.logdebug(
"self.rc.SpeedM1M2(self.address, vl_ticks, vr_ticks) : %s",
status1)
except ValueError:
rospy.logwarn("tick value error")
pass
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
# if Single motor driver mode using M1
else:
vx_ticks = int(linear_x * self.TICKS_PER_METER) # ticks/s
rospy.logdebug("vx_ticks:%d", vx_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vx_ticks is 0:
status1 = self.rc.ForwardM1(self.address, 0)
rospy.logdebug("self.rc.ForwardM1(self.address, 0) : %s",
status1)
else:
status1 = self.rc.SpeedM1(self.address, vx_ticks)
rospy.logdebug(
"self.rc.SpeedM1(self.address, vx_ticks) : %s",
status1)
except ValueError:
rospy.logwarn("tick value error")
pass
except OSError as e:
rospy.logwarn("SpeedM1 OSError: %d", e.errno)
rospy.logdebug(e)
rospy.logdebug("Exiting cmd_vel_callback")
# TODO: Need to make this work when more than one error is raised
def check_vitals(self, stat):
try:
status = self.rc.ReadError(self.address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add(
"Main Batt V:",
float(self.rc.ReadMainBatteryVoltage(self.address)[1] / 10))
stat.add(
"Logic Batt V:",
float(self.rc.ReadLogicBatteryVoltage(self.address)[1] / 10))
stat.add("Temp1 C:", float(self.rc.ReadTemp(self.address)[1] / 10))
stat.add("Temp2 C:",
float(self.rc.ReadTemp2(self.address)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
# TODO: need clean shutdown so motors stop even if new msgs are arriving
def shutdown(self):
rospy.logwarn("Shutting down")
try:
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
class Node:
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("roboclaw_node")
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.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")
self.rc = Roboclaw(dev_name, baud_rate)
# TODO need someway to check if address is correct
try:
self.rc.Open()
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 = self.rc.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]))
self.rc.SpeedM1M2(self.address, 0, 0)
self.rc.ResetEncoders(self.address)
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("address %d", self.address)
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 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:
self.rc.ForwardM1(self.address, 0)
self.rc.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
try:
status1, enc1, crc1 = self.rc.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = self.rc.ReadEncM2(self.address)
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()
r_time.sleep()
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:
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
else:
self.rc.SpeedM1M2(self.address, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
# TODO: Need to make this work when more than one error is raised
def check_vitals(self, stat):
try:
status = self.rc.ReadError(self.address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add("Main Batt V:", float(self.rc.ReadMainBatteryVoltage(self.address)[1] / 10))
stat.add("Logic Batt V:", float(self.rc.ReadLogicBatteryVoltage(self.address)[1] / 10))
stat.add("Temp1 C:", float(self.rc.ReadTemp(self.address)[1] / 10))
stat.add("Temp2 C:", float(self.rc.ReadTemp2(self.address)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
# TODO: need clean shutdown so motors stop even if new msgs are arriving
def shutdown(self):
rospy.loginfo("Shutting down")
try:
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
class Node:
""" Class for running roboclaw ros node for 2 motors in a diff drive setup"""
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")
#rospy.init_node("roboclaw_node", log_level=rospy.DEBUG)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev_name = rospy.get_param("~dev")
self.baud_rate = int(rospy.get_param("~baud"))
self.frontaddr = int(rospy.get_param("~frontaddr"))
self.backaddr = int(rospy.get_param("~backaddr"))
self.accel = int(rospy.get_param("~accel"))
self._has_showed_message = False
self.last_motor1_command = 0.0
self.last_motor2_command = 0.0
# self.accel_limit = 635 # ramp up to full speed (127) in 1/period * 127. current setting is 0.2 seconds
self.roboclaw = Roboclaw(self.dev_name, self.baud_rate)
status = self.roboclaw.Open()
self.roboclaw.SetM1DefaultAccel(self.frontaddr,
self.accel) # default is 655360
self.roboclaw.SetM2DefaultAccel(self.frontaddr, self.accel)
self.roboclaw.SetM1DefaultAccel(self.backaddr, self.accel)
self.roboclaw.SetM2DefaultAccel(self.backaddr, self.accel)
rospy.loginfo("Roboclaw Port Status: " + str(status))
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
version = self.roboclaw.ReadVersion(self.frontaddr)
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
if version is None:
rospy.logwarn("Could not get version from front roboclaw")
else:
rospy.logdebug("Version " + str(repr(version[1])))
try:
version = self.roboclaw.ReadVersion(self.backaddr)
except Exception as e:
rospy.logwarn("Problem getting rear roboclaw version")
rospy.logdebug(e)
if version is None:
rospy.logwarn("Could not get version from rear roboclaw")
else:
rospy.logdebug("Version " + str(repr(version[1])))
self.roboclaw.SpeedM1M2(self.frontaddr, 0, 0)
self.roboclaw.ResetEncoders(self.frontaddr)
self.roboclaw.SpeedM1M2(self.backaddr, 0, 0)
self.roboclaw.ResetEncoders(self.backaddr)
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.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
self.sub = rospy.Subscriber("motor_vel",
Mobility,
self.cmd_vel_callback,
queue_size=5)
self.TIMEOUT = 2
rospy.sleep(1)
rospy.logdebug("dev %s", self.dev_name)
rospy.logdebug("baud %d", self.baud_rate)
rospy.logdebug("front address %d", self.frontaddr)
rospy.logdebug("back address %d", self.frontaddr)
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)
def run(self):
"""Run the main ros loop"""
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() > self.TIMEOUT:
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)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
if (not self._has_showed_message):
rospy.loginfo(
"Did not get command for 1 second, stopping")
self._has_showed_message = True
else:
self._has_showed_message = False
# TODO need find solution to the OSError11 looks like sync problem with serial status1, enc1, crc1 = None, None, None
status2, enc1, enc2, crc2 = None, None, None, None
try:
status1, enc1, crc1 = self.roboclaw.ReadEncM1(
self.frontaddr)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = self.roboclaw.ReadEncM2(
self.frontaddr)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if ((enc1 is not None) and (enc2 is not None)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
r_time.sleep()
def cmd_vel_callback(self, mobility):
with self.lock:
"""Command the motors based on the incoming twist message"""
now_time = rospy.get_rostime()
dt = (now_time - self.last_set_speed_time).to_sec()
self.last_set_speed_time = now_time
rospy.logdebug(
"Front Left: %d Front Right: %d Rear Left: %d Rear Right: %d",
mobility.front_left, mobility.front_right, mobility.rear_left,
mobility.rear_left)
# linear_x = -twist.linear.x
# angular_z = twist.angular.z
velocities = [
mobility.front_left, mobility.front_right, mobility.rear_left,
mobility.rear_right
]
#clamp
velocities = [
max(-self.LINEAR_MAX_SPEED, min(x, self.LINEAR_MAX_SPEED))
for x in velocities
]
#scale to motor pwm
velocities = [
int((x / self.LINEAR_MAX_SPEED) * 127) for x in velocities
]
# Enforce acceleration limits
# TODO: test this block. it has not been tested. if it doesn't work, just comment it out for now
# TODO: use dt, the time since last command to enfore better acceleration limits
#if (motor1_command - self.last_motor1_command)/dt > self.accel_limit:
# motor1_command = self.last_motor1_command + (self.accel_limit)*dt
# rospy.logdebug("Motor command exceeded acceleration limits, was clipped to %d", motor1_command)
#elif (motor1_command - self.last_motor1_command)/dt < -self.accel_limit:
# motor1_command = self.last_motor1_command - (self.accel_limit)*dt
# rospy.logdebug("Motor command exceeded acceleration limits, was clipped to %d", motor1_command)
#if (motor2_command - self.last_motor2_command)/dt > self.accel_limit:
# motor2_command = self.last_motor2_command + (self.accel_limit)*dt
# rospy.logdebug("Motor command exceeded acceleration limits, was clipped to %d", motor2_command)
#elif (motor2_command - self.last_motor2_command)/dt < -self.accel_limit:
# motor2_command = self.last_motor2_command - (self.accel_limit)*dt
# rospy.logdebug("Motor command exceeded acceleration limits, was clipped to %d", motor2_command)
# Clip commands to within bounds (-127,127)
#motor1_command = int(max(-127, min(127, motor1_command)))
#motor2_command = int(max(-127, min(127, motor2_command)))
rospy.loginfo("%d, %d, %d, %d", int(velocities[0]),
int(velocities[1]), int(velocities[2]),
int(velocities[3]))
try:
if (velocities[0] >= 0):
rospy.loginfo("setting front left wheel to " +
str(velocities[0]))
self.roboclaw.ForwardM1(self.frontaddr, velocities[0])
else:
self.roboclaw.BackwardM1(self.frontaddr, (-velocities[0]))
if (velocities[1] >= 0):
rospy.loginfo("setting front right wheel to " +
str(velocities[1]))
self.roboclaw.ForwardM2(self.frontaddr, velocities[1])
else:
self.roboclaw.BackwardM2(self.frontaddr, -velocities[1])
if (velocities[2] >= 0):
rospy.loginfo("setting back left wheel to " +
str(velocities[2]))
self.roboclaw.ForwardM1(self.backaddr, velocities[2])
else:
self.roboclaw.BackwardM1(self.backaddr, -velocities[2])
if (velocities[3] >= 0):
rospy.loginfo("setting back right wheel to " +
str(velocities[3]))
self.roboclaw.ForwardM2(self.backaddr, velocities[3])
else:
self.roboclaw.BackwardM2(self.backaddr, -velocities[3])
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
#self.last_motor1_command = motor1_command
#self.last_motor2_command = motor2_command
def check_vitals(self, stat):
"""Check battery voltage and temperatures from roboclaw"""
try:
statusfront = self.roboclaw.ReadError(self.frontaddr)[1]
statusrear = self.roboclaw.ReadError(self.backaddr)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
statefront, messagefront = self.ERRORS[statusfront]
staterear, messagerear = self.ERRORS[statusfront]
stat.summary(statefront, messagefront)
stat.summary(staterear, messagerear)
try:
stat.add(
"Front Main Batt V:",
float(
self.roboclaw.ReadMainBatteryVoltage(self.frontaddr)[1] /
10))
stat.add(
"Front Logic Batt V:",
float(
self.roboclaw.ReadLogicBatteryVoltage(self.frontaddr)[1] /
10))
stat.add("Front Temp1 C:",
float(self.roboclaw.ReadTemp(self.frontaddr)[1] / 10))
stat.add("Front Temp2 C:",
float(self.roboclaw.ReadTemp2(self.frontaddr)[1] / 10))
front_currents = self.roboclaw.ReadCurrents(self.frontaddr)
stat.add("Front Left Current:", float(front_currents[1] / 100))
stat.add("Front Right Current:", float(front_currents[2] / 100))
back_currents = self.roboclaw.ReadCurrents(self.backaddr)
stat.add("Back Left Current:", float(back_currents[1] / 100))
stat.add("Back Right Current:", float(back_currents[2] / 100))
stat.add(
"Back Main Batt V:",
float(
self.roboclaw.ReadMainBatteryVoltage(self.backaddr)[1] /
10))
stat.add(
"Back Logic Batt V:",
float(
self.roboclaw.ReadLogicBatteryVoltage(self.backaddr)[1] /
10))
stat.add("Back Temp1 C:",
float(self.roboclaw.ReadTemp(self.backaddr)[1] / 10))
stat.add("Back Temp2 C:",
float(self.roboclaw.ReadTemp2(self.backaddr)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
def shutdown(self):
"""Handle shutting down the node"""
rospy.loginfo("Shutting down")
if hasattr(self, "sub"):
self.sub.unregister() # so it doesn't get called after we're dead
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)
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)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
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
self._have_read_vitals = False # flag to check when vitals have been read
# 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.DEBUG)
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"))
self.accel = int(rospy.get_param("~accel"))
# open roboclaw device connection
self.roboclaw = Roboclaw(self.dev, self.baudrate)
# set acceleration limits (default is 655360, we found 100000 to be decent)_
self.roboclaw.SetM1DefaultAccel(self.frontaddr, self.accel)
self.roboclaw.SetM2DefaultAccel(self.frontaddr, self.accel)
self.roboclaw.SetM1DefaultAccel(self.backaddr, self.accel)
self.roboclaw.SetM2DefaultAccel(self.backaddr, self.accel)
self.roboclaw.SetM1DefaultAccel(self.diggeraddr, self.accel)
self.roboclaw.SetM2DefaultAccel(self.diggeraddr, self.accel)
# 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 = rosp.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
print(self.roboclaw.ReadEncM1(self.diggeraddr))
print(self.roboclaw.ReadEncM2(self.diggeraddr))
try:
_, 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.back_enc1 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"""
# If the linear actuator is not extended, or we haven't heard from it in a while,
# set the digger speed to 0
if self.digger_extended and ( (rospy.Time.now() - self.last_digger_extended_time).to_sec < self.timeout):
self.curr_digger_cmd = self.curr_digger_cmd
else:
self.curr_digger_cmd = 0
# 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.ForwardM2(self.diggeraddr, self.curr_digger_cmd)
else:
self.roboclaw.BackwardM1(self.diggeraddr, -self.curr_digger_cmd)
self.roboclaw.BackwardM2(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.ForwardM2(self.frontaddr, self.curr_drive2_cmd)
self.roboclaw.ForwardM2(self.backaddr, self.curr_drive2_cmd)
else:
self.roboclaw.BackwardM2(self.frontaddr, -self.curr_drive2_cmd)
self.roboclaw.BackwardM2(self.backaddr, -self.curr_drive2_cmd)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
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.statefront, self.messagefront = ERRORS[statusfront]
self.stateback, self.messageback = ERRORS[statusback]
self.statedigger, self.messagedigger = ERRORS[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)
self._have_read_vitals = True
#rospy.logdebug("Front V %d", self.front_voltage)
#rospy.logdebug("Back V %d", self.back_voltage)
#rospy.logdebug("Digger V %d", self.digger_voltage)
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)"""
if self._have_read_vitals:
stat.summary(self.statefront, self.messagefront)
stat.summary(self.stateback, self.messageback)
stat.summary(self.statedigger, self.messagedigger)
stat.add("Front Main Batt V:", self.front_voltage)
stat.add("Front Logic Batt V:", self.front_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)
class Node:
""" Class for running roboclaw ros node for 2 motors in a diff drive setup"""
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(
"roboclaw_node",
log_level=rospy.DEBUG) #TODO: remove 2nd param when done debugging
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev_name = rospy.get_param("~dev")
self.baud_rate = int(rospy.get_param("~baud", "115200"))
self._has_showed_message = False
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")
self.roboclaw = Roboclaw(self.dev_name, self.baud_rate)
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
version = self.roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
if version is None:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
self.roboclaw.SpeedM1M2(self.address, 0, 0)
#self.roboclaw.ResetEncoders(self.address)
self.LINEAR_MAX_SPEED = float(
rospy.get_param("linear/x/max_velocity", "2.0"))
self.ANGULAR_MAX_SPEED = float(
rospy.get_param("angular/z/max_velocity", "2.0"))
self.TICKS_PER_METER = float(rospy.get_param("tick_per_meter", "10"))
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()
self.sub = rospy.Subscriber("cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=5)
self.TIMEOUT = 2
rospy.sleep(1)
rospy.logdebug("dev %s", self.dev_name)
rospy.logdebug("baud %d", self.baud_rate)
rospy.logdebug("address %d", self.address)
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)
def run(self):
"""Run the main ros loop"""
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() > self.TIMEOUT:
try:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
if (not self._has_showed_message):
rospy.loginfo("Did not get command for 1 second, stopping")
self._has_showed_message = True
else:
self._has_showed_message = False
# 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
#try:
# status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
#except ValueError:
# pass
#except OSError as e:
# rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
# rospy.logdebug(e)
#try:
# status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
#except ValueError:
# pass
#except OSError as e:
# rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
# rospy.logdebug(e)
#if ((enc1 is not None) and (enc2 is not None)):
# rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
# self.encodm.update_publish(enc1, enc2)
self.updater.update()
r_time.sleep()
def cmd_vel_callback(self, twist):
"""Command the motors based on the incoming twist message"""
self.last_set_speed_time = rospy.get_rostime()
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
motor1_command = linear_x / self.LINEAR_MAX_SPEED + angular_z / self.ANGULAR_MAX_SPEED
motor2_command = linear_x / self.LINEAR_MAX_SPEED - angular_z / self.ANGULAR_MAX_SPEED
# Scale to motor pwm
motor1_command = int(motor1_command * 127)
motor2_command = int(motor2_command * 127)
# Clip commands to within bounds (-127,127)
motor1_command = max(-127, min(127, motor1_command))
motor2_command = max(-127, min(127, motor2_command))
rospy.logdebug("motor1 command = %d", int(motor1_command))
rospy.logdebug("motor2 command = %d", int(motor2_command))
try:
if motor1_command >= 0:
self.roboclaw.ForwardM1(self.address, motor1_command)
else:
self.roboclaw.BackwardM1(self.address, -motor1_command)
if motor2_command >= 0:
self.roboclaw.ForwardM2(self.address, motor2_command)
else:
self.roboclaw.BackwardM2(self.address, -motor2_command)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
def check_vitals(self, stat):
"""Check battery voltage and temperatures from roboclaw"""
try:
status = self.roboclaw.ReadError(self.address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add(
"Main Batt V:",
float(
self.roboclaw.ReadMainBatteryVoltage(self.address)[1] /
10))
stat.add(
"Logic Batt V:",
float(
self.roboclaw.ReadLogicBatteryVoltage(self.address)[1] /
10))
stat.add("Temp1 C:",
float(self.roboclaw.ReadTemp(self.address)[1] / 10))
stat.add("Temp2 C:",
float(self.roboclaw.ReadTemp2(self.address)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
def shutdown(self):
"""Handle shutting down the node"""
rospy.loginfo("Shutting down")
if hasattr(self, "sub"):
self.sub.unregister() # so it doesn't get called after we're dead
try:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
rospy.loginfo("Closed Roboclaw serial connection")
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
self.roboclaw.ForwardM1(self.address, 0)
self.roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)