def connect(self, dev_name, baud_rate, address, test_mode=False):
"""Connects the node to the Roboclaw controller, or the test stub
Parameters:
:param str dev_name: Serial device name (e.g. /dev/ttyACM0)
:param int baud_rate: Serial baud rate (e.g. 115200)
:param int address: Serial address (default 0x80)
:param bool test_mode: if connecting to the controller test stub
"""
rospy.loginfo("Connecting to roboclaw")
if not test_mode:
roboclaw = Roboclaw(dev_name, baud_rate)
else:
roboclaw = RoboclawStub(dev_name, baud_rate)
self._rbc_ctls.append(RoboclawControl(roboclaw, address))
#!/usr/bin/env python
from __future__ import print_function
import sys
from roboclaw_driver import Roboclaw
if len(sys.argv) == 1:
print()
print("Example Usage: {} /dev/ttyACM0".format(sys.argv[0]))
print()
exit()
dev_name = sys.argv[1]
baud_rate = 115200
address = 0x80
roboclaw = Roboclaw(dev_name, baud_rate)
roboclaw.Open()
temp_response = roboclaw.ReadTemp(address)[1] / 10.0
volts_response = roboclaw.ReadMainBatteryVoltage(address)[1] / 10.0
print("Reading Roboclaw at: {}".format(dev_name))
print("Temp: {}C, Battery: {}V".format(temp_response, volts_response))
def __init__(self):
"""init variables and ros stuff"""
self._have_shown_message = False # flag to not spam logging
# ints in [-127,127] that get sent to roboclaw to drive forward or backward
self.curr_drive1_cmd = 0
self.curr_drive2_cmd = 0
#rospy.init_node("roboclaw_node")
rospy.init_node("roboclaw_node", log_level=rospy.INFO)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev = rospy.get_param("~dev")
self.baudrate = int(rospy.get_param("~baudrate"))
self.frontaddr = int(rospy.get_param("~frontaddr"))
self.backaddr = int(rospy.get_param("~backaddr"))
self.diggeraddr = int(rospy.get_param("~diggeraddr"))
# open roboclaw serial device connection
self.roboclaw = Roboclaw(self.dev, self.baudrate)
# diagnostics
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.pub_vitals))
# TODO (p1): we probably just want to crash here or something if we don't have a connection
try:
version = self.roboclaw.ReadVersion(self.frontaddr)
rospy.logdebug("Front Version " + str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.backaddr)
rospy.logdebug("Back Version "+ str(repr(version[1])))
except Exception as e:
rospy.logwarn("Problem getting back roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
try:
version = self.roboclaw.ReadVersion(self.diggeraddr)
rospy.logdebug("Digger Version "+ str(repr(version[1])))
self.roboclaw.SetM1EncoderMode(self.diggeraddr, 1)
self.roboclaw.SetM2EncoderMode(self.diggeraddr, 1)
rospy.logdebug("Digger Encoder Mode "+ str(self.roboclaw.ReadEncoderModes(self.diggeraddr)))
except Exception as e:
rospy.logwarn("Problem getting digger roboclaw version")
rospy.logdebug(e)
raise SerialException("Connectivity issue. Could not read version")
self.roboclaw.SpeedM1M2(self.frontaddr, 0, 0)
self.roboclaw.ResetEncoders(self.frontaddr)
self.roboclaw.SpeedM1M2(self.backaddr, 0, 0)
self.roboclaw.ResetEncoders(self.backaddr)
# TODO (p2): test resetting
#self.roboclaw.SpeedM1M2(self.diggeraddr, 0, 0)
#self.roboclaw.ResetEncoders(self.diggeraddr)
self.LINEAR_MAX_SPEED = float(rospy.get_param("~linear/x/max_velocity"))
self.ANGULAR_MAX_SPEED = float(rospy.get_param("~angular/z/max_velocity"))
self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter"))
self.BASE_WIDTH = float(rospy.get_param("~base_width"))
self.TIMEOUT = float(rospy.get_param("~timeout"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_vel_cmd_time = rospy.Time.now()
self.last_digger_cmd_time = rospy.Time.now()
self.last_digger_extended_time = rospy.Time.now()
self.digger_extended = False
self.cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
self.digger_sub = rospy.Subscriber("/digger_spin/cmd", Float32, self.digger_spin_callback, queue_size=1)
self.digger_extended_sub = rospy.Subscriber("/digger_extended", Bool, self.digger_extended_callback, queue_size=1)
rospy.sleep(1) # wait for things to initialize
rospy.logdebug("dev %s", self.dev)
rospy.logdebug("baudrate %d", self.baudrate)
rospy.logdebug("front address %d", self.frontaddr)
rospy.logdebug("back address %d", self.backaddr)
rospy.logdebug("digger address %d", self.diggeraddr)
rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
rospy.logdebug("timeout %f", self.TIMEOUT)