def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.addressBackMotors, 0)
roboclaw.ForwardM2(self.addressBackMotors, 0)
roboclaw.ForwardM1(self.addressFrontMotors, 0)
roboclaw.ForwardM2(self.addressFrontMotors, 0)
else:
roboclaw.SpeedM1M2(self.addressBackMotors, vr_ticks, vl_ticks)
roboclaw.SpeedM1M2(self.addressFrontMotors, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 0.1:
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 + self.BASE_LENGTH) # m/s
vl = linear_x - twist.angular.z * (self.BASE_WIDTH +
self.BASE_LENGTH)
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
# rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.rear_address, 0) # left
roboclaw.ForwardM2(self.rear_address, 0) # right
roboclaw.ForwardM1(self.front_address, 0) # left
roboclaw.ForwardM2(self.front_address, 0) # right
else:
roboclaw.SpeedM1M2(self.rear_address, vl_ticks, vr_ticks)
roboclaw.SpeedM1M2(self.front_address, vl_ticks, vr_ticks)
except OSError as e:
rospy.logwarn("Rear SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
if self.twist is None:
return
# self.last_set_speed_time = rospy.get_rostime()
if self.twist.linear.x != 0 or self.twist.angular.z != 0:
self.stopped = False
linear_x = self.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 + self.twist.angular.z * self.BASE_WIDTH # m/s
vl = linear_x - self.twist.angular.z * self.BASE_WIDTH
self.twist = None
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
#print("-- vr_ticks:{} vl_ticks:{}".format(vr_ticks, vl_ticks))
rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
# if vr_ticks is 0 and vl_ticks is 0:
# roboclaw.ForwardM1(self.address, 0)
# roboclaw.ForwardM2(self.address, 0)
# else:
# roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
self.vr_ticks = 0
self.vl_ticks = 0
else:
gain = 0.5
self.vr_ticks = gain * vr_ticks + (1 - gain) * self.vr_ticks
self.vl_ticks = gain * vl_ticks + (1 - gain) * self.vl_ticks
roboclaw.SpeedM1M2(self.address, int(self.vr_ticks),
int(self.vl_ticks))
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
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 cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = -twist.linear.x
angular_z = twist.angular.z
# Set speed limits
if abs(linear_x) > self.MAX_SPEED_LINEAR:
linear_x = np.sign(linear_x) * self.MAX_SPEED_LINEAR
if abs(angular_z) > self.MAX_SPEED_ANGULAR:
angular_z = np.sign(angular_z) * self.MAX_SPEED_ANGULAR
vel_new = np.array([linear_x, angular_z])
self.vel_old = self.a * vel_new + (1.0 - self.a) * self.vel_old
linear_x, angular_z = self.vel_old[0], self.vel_old[1]
print("==== cmd_callback ====")
print("> linear: ", linear_x, " | ", self.MAX_SPEED_LINEAR)
print("> angular: ", angular_z, " | ", self.MAX_SPEED_ANGULAR)
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
print("--------------------")
print("> vr: ", vr)
print("> vl: ", vl)
print("=======================")
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 abs(vr_ticks) <= 1e-5 and abs(vl_ticks) <= 1e-5:
with self.mutex:
self.vel_old = np.array([0.0, 0.0])
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
with self.mutex:
roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting motor drive")
roboclaw.Flush()
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > self.TIMEOUT:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
roboclaw.SpeedM1M2(self.address, 0, 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 = 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 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.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("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)
else:
print "failed "
rc.Open("/dev/ttyACM0", 115200)
address = 0x80
signal.signal(signal.SIGINT, handler)
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
qpps_m1 = 110
qpps_m2 = 110
try:
while (1):
rc.SpeedM1M2(address, qpps_m1, -qpps_m2)
for i in range(200):
displayspeed()
time.sleep(0.01)
rc.SpeedM1M2(address, -qpps_m1, qpps_m2)
for i in range(200):
displayspeed()
time.sleep(0.01)
except Exception as e:
print(e)
finally:
rc.SpeedM1M2(address, 0, 0)
def __init__(self):
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
rospy.init_node("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")
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 handler(signal, frame):
rc.SpeedM1M2(address, 0, 0)
sys.exit(0)
if crc == 1:
# print("M1 moter current "+'{:.3}'.format(l_current)+"[A]")
# print("M2 moter current "+'{:.3}'.format(r_current)+"[A]")
print("elapsed_time:{0}".format(elapsed_time) + "[sec] " +
"M1 moter current " + '{:.3}'.format(l_currentA) + "[A]")
print("elapsed_time:{0}".format(elapsed_time) + "[sec] " +
"M2 moter current " + '{:.3}'.format(r_currentA) + "[A]")
else:
print("Failed")
rc.Open('/dev/ttyACM0', 115200)
signal.signal(signal.SIGINT, handler)
address = 0x80
try:
# loop until ctl + c
m1_qpps = m2_qpps = 200
rc.SpeedM1M2(address, m1_qpps, m2_qpps)
time.sleep(0.5)
start = time.time()
while True:
displayCurrent()
time.sleep(0.01)
except Exception as e:
print(e)
finally:
rc.SpeedM1M2(address, 0, 0)
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)
except Exception as e:
print(e)
finally:
print("Finish")
rc.ForwardM1(address, 0)
rc.ForwardM2(address, 0)
def __init__(self):
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("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)
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_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)
