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)