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