def debug_robot():
rc.Open('/dev/ttyUSB0', 115200)
while True:
status = rc.ReadError(MTR_ADDRESS)[1]
encoder = rc.ReadEncM1(MTR_ADDRESS)
print encoder
def config():
rc.Open('/dev/ttyUSB1',115200)
print rc.ReadMinMaxMainVoltages(MTR_ADDRESS)
rc.SetPinFunctions(MTR_ADDRESS,2,2,2)
aa = rc.ReadPinFunctions(MTR_ADDRESS)
print aa
rc.WriteNVM(MTR_ADDRESS)
def debug_robot():
rc.Open('/dev/ttyUSB0',115200)
while True:
status = rc.ReadError(MTR_ADDRESS)[1]
dummy,c1,c2 = rc.ReadCurrents(MTR_ADDRESS)
bvoltage =rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0
diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (bvoltage, c1/100.0, c2/100.0, status)
print diagstr
def move_robot():
global pub
global last_time
rc.Open('/dev/ttyUSB0', 115200)
print rc.ReadMinMaxMainVoltages(MTR_ADDRESS)
pub = rospy.Publisher('motor_diagnostics', String, queue_size=10)
rospy.init_node('roboclaw_ifc')
last_time = rospy.get_rostime()
rospy.Timer(rospy.Duration(1), timer_callback)
rospy.Subscriber("/cmd_vel", Twist, twist_callback, queue_size=1)
rospy.spin()
rc.DutyM1M2(MTR_ADDRESS, 0, 0)
def move_robot():
global pub
global last_time
rc.Open('/dev/ttyUSB0',115200)
print rc.ReadMinMaxMainVoltages(MTR_ADDRESS)
rospy.init_node('roboclaw_ifc')
vel_list = [.2, .4 , .2 , .1 , .0]
for vel in vel_list:
speed = int(16484 * vel)
print speed
rc.SpeedM1M2(MTR_ADDRESS, speed, speed)
rospy.sleep(5)
rc.DutyM1M2(MTR_ADDRESS, 0, 0)
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:
roboclaw.Open(self.dev_name, self.baud_rate)
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.Close()
rospy.loginfo("Closed Roboclaw serial connection")
#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)
quit()
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")
# TODO need someway to check if address is correct
try:
mutex.acquire()
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")
finally:
mutex.release()
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
mutex.acquire()
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
finally:
mutex.release()
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_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 __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_height")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev_name = rospy.get_param("~dev", "/dev/ttyACM2") #may need to change the usb port
self.baud_rate = int(rospy.get_param("~baud", "38400")) #may need to change the baud rate. see roboclaw usermanual
self.address = int(rospy.get_param("~address", "129")) #roboclaw is current setup to use address 128 which is setting 7
#Error Handle
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(self.dev_name, self.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_SPEED = float(rospy.get_param("~max_speed", "127"))
self.MAX_DUTY = float(rospy.get_param("~max_duty", "32767"))
self.last_set_speed_time = rospy.get_rostime()
self.height_vel_sub = rospy.Subscriber("roboclaw/height_vel", Twist, self.cmd_vel_callback)
self.height_pub = rospy.Publisher("roboclaw/height", JointState, queue_size=10)
self.m1_duty = 0
self.m2_duty = 0
rospy.sleep(1)
rospy.loginfo("dev %s", self.dev_name)
rospy.loginfo("baud %d", self.baud_rate)
rospy.loginfo("address %d", self.address)
rospy.loginfo("max_speed %f", self.MAX_SPEED)
'''
# brief : This file is speed script for roboclaw driver.
# @author : koji kawabata
# @date : 2019/10/23
'''
import time
import signal
import sys
import roboclaw_driver.roboclaw_driver as rc
rc.Open('/dev/ttyACM0', 115200)
address = 0x80
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
print("run speed command")
try:
rc.SpeedM1(address, 300)
time.sleep(2)
rc.SpeedM2(address, 300)
time.sleep(2)
rc.SpeedM1M2(address, 150, 150)
time.sleep(2)
rc.SpeedM1(address, -300)
time.sleep(2)
rc.SpeedM2(address, -300)
time.sleep(2)
rc.SpeedM1M2(address, -150, -150)
time.sleep(2)
rc.SpeedM1M2(address, 0, 0)
import time
import roboclaw_driver.roboclaw_driver as rc
#Windows comport name
#rc = Roboclaw("COM9",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open("/dev/ttyACM0", 115200)
address = 0x80
while(1):
rc.ForwardM1(address,32) #1/4 power forward
rc.BackwardM2(address,32) #1/4 power backward
time.sleep(2)
rc.BackwardM1(address,32) #1/4 power backward
rc.ForwardM2(address,32) #1/4 power forward
time.sleep(2)
rc.BackwardM1(address,0) #Stopped
rc.ForwardM2(address,0) #Stopped
time.sleep(5)
m1duty = 16
m2duty = -16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward
rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward
time.sleep(2)
m1duty = -16
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)
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("drive_base_node", anonymous=False)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaws")
dev_name = rospy.get_param("~dev", "/dev/ttyAMA0")
baud_rate = int(rospy.get_param("~baud_rate", "115200"))
# rear roboclaw
self.rear_address = int(rospy.get_param("~rear_address", "129"))
if self.rear_address > 0x87 or self.rear_address < 0x80:
rospy.logfatal("Rear address out of range")
rospy.signal_shutdown("Rear address out of range")
# front roboclaw
self.front_address = int(rospy.get_param("~front_address", "128"))
if self.front_address > 0x87 or self.front_address < 0x80:
rospy.logfatal("Front address out of range")
rospy.signal_shutdown("Front 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("Rear Vitals",
self.check_rear_vitals))
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Front Vitals",
self.check_front_vitals))
# collect and report rear roboclaw version
try:
version = roboclaw.ReadVersion(self.rear_address)
except Exception as e:
rospy.logwarn("Problem getting rear roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from rear roboclaw")
else:
rospy.loginfo("Rear roboclaw version: %s", repr(version[1]))
# collect and report front roboclaw version
try:
version = roboclaw.ReadVersion(self.front_address)
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from front roboclaw")
else:
rospy.loginfo("Front roboclaw version: %s", repr(version[1]))
# stop all motors
roboclaw.SpeedM1M2(self.rear_address, 0, 0)
roboclaw.ResetEncoders(self.rear_address)
roboclaw.SpeedM1M2(self.front_address, 0, 0)
roboclaw.ResetEncoders(self.front_address)
# collect parameters
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
self.TICKS_PER_METER = float(
rospy.get_param("~tick_per_meter", "1634.2"))
self.BASE_WIDTH = float(rospy.get_param("~axel_width", "0.233"))
self.BASE_LENGTH = float(rospy.get_param("~axel_length", "0.142"))
self.DIAGNOSTIC = bool(rospy.get_param("~diagnostic", "true"))
# instantiate encoder
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH,
self.BASE_LENGTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.sleep(1)
# report configuration
rospy.loginfo("device : %s", dev_name)
rospy.loginfo("baud_rate : %d", baud_rate)
rospy.loginfo("rear_address : %d", self.rear_address)
rospy.loginfo("front_address : %d", self.front_address)
rospy.loginfo("max_speed : %f", self.MAX_SPEED)
rospy.loginfo("ticks_per_meter : %f", self.TICKS_PER_METER)
rospy.loginfo("base_width : %f", self.BASE_WIDTH)
rospy.loginfo("base_length : %f", self.BASE_LENGTH)
rospy.loginfo("diagnostic : %s", self.DIAGNOSTIC)
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.topic_timeout = int(rospy.get_param("~topic_timeout", "1"))
self.TICKS_PER_RADIAN = float(
rospy.get_param("~ticks_per_rotation", "663")) / (2 * pi)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "0"))
addresses = str(rospy.get_param("~addresses", "128")).split(",")
self.addresses = []
for addr in addresses:
addr = int(addr)
if addr > 0x87 or addr < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
self.addresses.append(addr)
rospy.loginfo("Addresses: %s" % str(self.addresses))
# TODO need some way to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw serial device")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
return
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
for addr in self.addresses:
version = None
try:
version = roboclaw.ReadVersion(addr)
except Exception as e:
rospy.logwarn(
"Problem getting roboclaw version from address %d" % addr)
rospy.logdebug(e)
if version is None or not version[0]:
rospy.logwarn(
"Could not get version from roboclaw address %d" % addr)
rospy.signal_shutdown(
"Could not get version from roboclaw address %d" % addr)
return
rospy.logdebug("Controller %d version is %s" %
(addr, repr(version[1])))
roboclaw.SpeedM1M2(addr, 0, 0)
roboclaw.ResetEncoders(addr)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("/cmd_motors",
MotorSpeeds,
self.cmd_motors_callback,
queue_size=1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("addresses %s", str(self.addresses))
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_radian: %f", self.TICKS_PER_RADIAN)
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", "/dev/ttyACM0")
self.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")
# TODO (Matt): figure out why roboclaw sometimes fails to Open
try:
roboclaw.Open(self.dev_name, self.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 version is None:
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.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 __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")
# 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")
# 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 tht 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:
# version = roboclaw.ReadVersion(self.address)
with self.mutex:
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)
with self.mutex:
roboclaw.SpeedM1M2(self.address, 0, 0)
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", "14964.27409"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.456"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
# if not sure what to use, use this!
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
# if using the joystick on the ROS Control app:
# rospy.Subscriber("joy_teleop/cmd_vel", Twist, self.cmd_vel_callback)
# if using the key_teleop package:
# rospy.Subscriber("key_vel", Twist, self.cmd_vel_callback)
# if using the mouse_teleop package:
# rospy.Subscriber("mouse_vel", Twist, self.cmd_vel_callback)
# if using the WebApp
# rospy.Subscriber("cmd_vel_mux/input/teleop", 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 __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_b")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name_front = rospy.get_param("~dev_front", "/dev/ttyACM0")
dev_name_back = rospy.get_param("~dev_back", "/dev/ttyACM1")
baud_rate = int(rospy.get_param("~baud", "38400"))
rospy.logwarn("after dev name")
self.address_front = int(rospy.get_param("~address_front", "128"))
self.address_back = int(rospy.get_param("~address_back", "129"))
#check wheather the addresses are in range
if self.address_front > 0x87 or self.address_front < 0x80 or self.address_back > 0x87 or self.address_back < 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_front, baud_rate)
# except Exception as e:
# rospy.logfatal("Could not connect to front Roboclaw")
# rospy.logdebug(e)
# rospy.signal_shutdown("Could not connect to front Roboclaw")
try:
roboclaw.Open(dev_name_back, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to back Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to back Roboclaw")
#run diagnostics
self.updater = diagnostic_updater.Updater(
) ###check later may be do it for both the motors
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
#check if you can get the version of the roboclaw...mosly you dont
try:
version = roboclaw.ReadVersion(self.address_front)
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from front roboclaw")
else:
rospy.logdebug(repr(version[1]))
try:
version = roboclaw.ReadVersion(self.address_back)
except Exception as e:
rospy.logwarn("Problem getting back roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from back roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address_front, 0, 0)
roboclaw.ResetEncoders(self.address_front)
roboclaw.SpeedM1M2(self.address_back, 0, 0)
roboclaw.ResetEncoders(self.address_back)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "1.7"))
self.TICKS_PER_METER = float(
rospy.get_param("~tick_per_meter", "89478.2"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.1762"))
self.BASE_LENGTH = float(rospy.get_param("~base_length", "0.2485"))
self.WHEEL_RADIUS = float(rospy.get_param("~wheel_radius", "0.0635"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH,
self.BASE_LENGTH, self.WHEEL_RADIUS)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
# rospy.sleep(1)
rospy.logdebug("dev_front %s dev_back %s", dev_name_front,
dev_name_back)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address_front %d address_back %d", self.address_front,
self.address_back)
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f base_length %f", self.BASE_WIDTH,
self.BASE_LENGTH)
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")
}
self.ref_WR = 0
self.ref_WL = 0
self.WR = 0
self.WL = 0
self.UiR_1 = 0
self.errorL_1 = 0
self.UiL_1 = 0
self.UiR_1 = 0
self.Kp = 7.0 #11.0
self.Ki = 2.0 #30.25*0.1 #=3.025
self.Kd = 3.0 #1.0/0.1 #=10
self.errorR_1 = 0
self.errorR_2 = 0
self.errorL_2 = 0
self.errorL_1 = 0
self.UR_2 = 0
self.UR_1 = 0
self.UL_2 = 0
self.UL_1 = 0
self.dt = 0.1
self.value = 0
self.last_UL = 0
self.last_UR = 0
rospy.init_node("roboclaw_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/tty_roboclaw")
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")
######################################## TOPICS #############################################
self.encoder_pub = rospy.Publisher('/encoders', String, queue_size=10)
#self.currents_pub = rospy.Publisher('/currents', String, queue_size=10)
#self.voltage_pub = rospy.Publisher('/voltage', String, queue_size=10)
self.speed_pub = rospy.Publisher('/speed', String, queue_size=10)
self.WL_pub = rospy.Publisher('/WL', String, queue_size=10)
self.WR_pub = rospy.Publisher('/WR', String, queue_size=10)
self.RefL_pub = rospy.Publisher('/RefL', String, queue_size=10)
self.RefR_pub = rospy.Publisher('/RefR', String, queue_size=10)
self.pid_pub = rospy.Publisher('/pid', String, queue_size=10)
# 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_SPEED = float(rospy.get_param("~max_speed", "2.0"))
self.TICKS_PER_METER = float(
rospy.get_param("~tick_per_meter", "14853.7362"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.38"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
################################ SUBSCRIBER ####################################
#rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.Subscriber("/joy", Joy, self.joy_event)
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)
