def __init__(self): self.brake_change_threshold = rospy.get_param('~brake_change_threshold', 0) self.steer_change_threshold = rospy.get_param('~steer_change_threshold', 0) self.arduino_cmd_msg = Arduino() self.arduino_pub = rospy.Publisher('arduino_cmd', Arduino) rate = rospy.get_param('~rate', 10) rospy.loginfo('Sending messages to the Arduino board every %dms' % int(1000.0/rate)) # monitor the last time we received a button_state_emergency message self.last_watchdog = None self.arduino_sub = rospy.Subscriber('button_state_emergency', Bool, self.arduinoCB) self.throttle_sub = setupSub('throttle', Float64, self.throttleCB) self.brake_sub = setupSub('brake_angle', Float64, self.brakeCB) self.steer_sub = setupSub('steer_angle', Float64, self.steerCB) self.diag_updater = DIAG.Updater() self.diag_updater.setHardwareID('none') # a frequency diagnostic: make sure we are sending the arduino_cmd message at the # appropriate frequency fs_param = DIAG.FrequencyStatusParam({'min': rate, 'max': rate}, 0.1, 5) self.fs_diag = DIAG.FrequencyStatus(fs_param) self.diag_updater.add(self.fs_diag) # watchdog diagnostic: reports an error when the board does not respond self.diag_updater.add( DIAG.FunctionDiagnosticTask('board watchdog', self.watchdog_diag) ) # emergency monitor self.emergency_state = False self.diag_updater.add( DIAG.FunctionDiagnosticTask('emergency button', self.emergency_diag) ) self.timer = rospy.Timer(rospy.Duration(1.0/rate), self.timer_cb)
def __init__(self, node_, transport_): self.node = node_ self.webrtc = None #filled by parent self.transport = transport_ self.diagnostics = diagnostic_updater.CompositeDiagnosticTask( 'Signalling & Transport') self.diagnostics.addTask( diagnostic_updater.FunctionDiagnosticTask('Signalling', self.diagnostic_task)) self.diagnostics.addTask(self.transport.diagnostics)
def __init__(self, node_, name_, transport, param_prefix=None, stun_server = default_stun_server, webrtc_element_name = 'webrtc_element', audio_src_bin_descr = default_audio_src_bin_descr, video_src_bin_descr = default_video_src_bin_descr, video_sink_bin_descr = default_video_sink_bin_descr, audio_sink_bin_descr = default_audio_sink_bin_descr, ): self.node = node_ self.chan = webrtc_sigchan(node_, transport) self.name = name_ self.stun_server = stun_server self.webrtc_element_name = webrtc_element_name self.audio_src_bin_descr = audio_src_bin_descr self.video_src_bin_descr = video_src_bin_descr self.video_sink_bin_descr = video_sink_bin_descr self.audio_sink_bin_descr = audio_sink_bin_descr if param_prefix != None: (self.stun_server, self.webrtc_element_name, self.audio_src_bin_descr, self.video_src_bin_descr, self.video_sink_bin_descr, self.audio_sink_bin_descr) = self.fetch_params(param_prefix) self.bin = self.build_initial_pipe() self.webrtc = self.bin.get_by_name(self.webrtc_element_name) self.chan.webrtc = self.webrtc self.connect_callbacks(self.webrtc) self.diagnostics = diagnostic_updater.CompositeDiagnosticTask(self.name + ' Status') self.diagnostics.addTask(diagnostic_updater.FunctionDiagnosticTask(self.name + ' Pipes', self.diagnostic_task)) self.diagnostics.addTask(self.chan.diagnostics) # diagnostics info self.audio_src_built = False self.video_src_built = False self.audio_sink_built = False self.video_sink_built = False
def __init__(self, node, loop, param_prefix=None, server=None, node_id=None, peer_id=None, offer=True, autodial=True ): self.node = node self.loop = loop self.server = server self.node_id = node_id self.peer_id = peer_id self.offer = offer self.autodial = autodial if param_prefix != None: (self.server, self.node_id, self.peer_id, self.offer, self.autodial) = self.fetch_params(param_prefix) self.conn = None # websockets connection self.diagnostics = diagnostic_updater.FunctionDiagnosticTask('Transport', self.diagnostic_task) self.remote_sends_ice_cb = None self.remote_sends_sdp_cb = None self.create_offer_cb = None self.webrtc_ready = False self.session_ready = False self.node.get_logger().info('using node_id "' + str(self.node_id) + '"') self.node.get_logger().info('using peer_id "' + str(self.peer_id) + '"') self.node.get_logger().info('using server "' + self.server + '"')
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) rospy.on_shutdown( self.shutdown) # shutdown signal will trigger shutdown function 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") self.rc = Roboclaw(dev_name, baud_rate) # TODO need someway to check if address is correct try: self.rc.Open() 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 = self.rc.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])) self.rc.SpeedM1M2(self.address, 0, 0) self.rc.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", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.TWIST_COMMAND = rospy.get_param("~twist_command", "roboclaw/cmd_vel") self.SINGLE_MOTOR = bool(rospy.get_param("~single_motor", "false")) self.PUBLISH_TF = bool(rospy.get_param("~publish_tf", "true")) self.CHILD_FRAME = rospy.get_param("~child_frame", "base_link") self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH, self.PUBLISH_TF, self.CHILD_FRAME) self.last_set_speed_time = rospy.get_rostime() # queue_size = 1 to make sure Roboclaw stops spining # this was an issue if cmd_vel frequency was too high 1000/sec # using swri_ropspy.Subscriber() swri_ropspy.Timer() for single threaded callbacks # multithreaded cmd_callback would eventually cause crash while the while-loop was reading encoders # https://github.com/swri-robotics/marti_common/blob/master/swri_rospy/nodes/single_threaded_example swri_rospy.Subscriber(self.TWIST_COMMAND, Twist, self.cmd_vel_callback, queue_size=2) swri_rospy.Timer(rospy.Duration(0.1), self.timer_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) rospy.logdebug("twist_command %s", self.TWIST_COMMAND) rospy.logdebug("single_motor %d", self.SINGLE_MOTOR) rospy.logdebug("publish_tf %d", self.PUBLISH_TF) rospy.logdebug("child_frame %s", self.CHILD_FRAME)
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 __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/ttyACM0") baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") # We have a single 'roboclaw' object handling serial communications. # We're about to launch different threads that each want to talk. # 1 - Diagnostics thread calling into our self.check_vitals # 2 - '/cmd_vel' thread calling our self.cmd_vel_callback # 3 - our self.run tht publishes to '/odom' # To prevent thread collision in the middle of serial communication # (which causes sync errors) all access to roboclaw from now # must be synchronized using this mutually exclusive lock object. self.mutex = threading.Lock() self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: # version = roboclaw.ReadVersion(self.address) with self.mutex: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) # roboclaw.SpeedM1M2(self.address, 0, 0) # roboclaw.ResetEncoders(self.address) with self.mutex: roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) self.TICKS_PER_METER = float( rospy.get_param("~ticks_per_meter", "14964.27409")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.456")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() # if not sure what to use, use this! rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) # if using the joystick on the ROS Control app: # rospy.Subscriber("joy_teleop/cmd_vel", Twist, self.cmd_vel_callback) # if using the key_teleop package: # rospy.Subscriber("key_vel", Twist, self.cmd_vel_callback) # if using the mouse_teleop package: # rospy.Subscriber("mouse_vel", Twist, self.cmd_vel_callback) # if using the WebApp # rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self.cmd_vel_callback) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def main(): rclpy.init() node = rclpy.create_node('diagnostic_updater_example') # The Updater class advertises to /diagnostics, and has a # ~diagnostic_period parameter that says how often the diagnostics # should be published. updater = diagnostic_updater.Updater(node) # The diagnostic_updater.Updater class will fill out the hardware_id # field of the diagnostic_msgs.msg.DiagnosticStatus message. You need to # use the setHardwareID() method to set the hardware ID. # # The hardware ID should be able to identify the specific device you are # working with. If it is not appropriate to fill out a hardware ID in # your case, you should call setHardwareID("none") to avoid warnings. # (A warning will be generated as soon as your node updates with no # non-OK statuses.) updater.setHardwareID('none') # Or... updater.setHardwareID('Device-%i-%i' % (27, 46)) # Diagnostic tasks are added to the Updater. They will later be run when # the updater decides to update. # As opposed to the C++ API, there is only one add function. It can take # several types of arguments: # - add(task): where task is a DiagnosticTask # - add(name, fn): add a DiagnosticTask embodied by a name and function updater.add('Function updater', dummy_diagnostic) dc = DummyClass() updater.add('Method updater', dc.produce_diagnostics) # Internally, updater.add converts its arguments into a DiagnosticTask. # Sometimes it can be useful to work directly with DiagnosticTasks. Look # at FrequencyStatus and TimestampStatus in update_functions for a # real-life example of how to make a DiagnosticTask by deriving from # DiagnosticTask. # Alternatively, a FunctionDiagnosticTask is a derived class from # DiagnosticTask that can be used to create a DiagnosticTask from # a function. This will be useful when combining multiple diagnostic # tasks using a CompositeDiagnosticTask. lower = diagnostic_updater.FunctionDiagnosticTask('Lower-bound check', check_lower_bound) upper = diagnostic_updater.FunctionDiagnosticTask('Upper-bound check', check_upper_bound) # If you want to merge the outputs of two diagnostic tasks together, you # can create a CompositeDiagnosticTask, also a derived class from # DiagnosticTask. For example, we could combine the upper and lower # bounds check into a single DiagnosticTask. bounds = diagnostic_updater.CompositeDiagnosticTask('Bound check') bounds.addTask(lower) bounds.addTask(upper) # We can then add the CompositeDiagnosticTask to our Updater. When it is # run, the overall name will be the name of the composite task, i.e., # "Bound check". The summary will be a combination of the summary of the # lower and upper tasks (see DiagnosticStatusWrapper.mergeSummary for # details on how the merging is done). The lists of key-value pairs will be # concatenated. updater.add(bounds) # You can broadcast a message in all the DiagnosticStatus if your node # is in a special state. updater.broadcast(b'0', 'Doing important initialization stuff.') pub1 = node.create_publisher(std_msgs.msg.Bool, 'topic1', 10) sleep(2) # It isn't important if it doesn't take time. # Some diagnostic tasks are very common, such as checking the rate # at which a topic is publishing, or checking that timestamps are # sufficiently recent. FrequencyStatus and TimestampStatus can do these # checks for you. # # Usually you would instantiate them via a HeaderlessTopicDiagnostic # (FrequencyStatus only, for topics that do not contain a header) or a # TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that # do contain a header). # # Some values are passed to the constructor as pointers. If these values # are changed, the FrequencyStatus/TimestampStatus will start operating # with the new values. # # Refer to diagnostic_updater.FrequencyStatusParam and # diagnostic_updater.TimestampStatusParam documentation for details on # what the parameters mean: freq_bounds = {'min': 0.5, 'max': 2} # If you update these values, the # HeaderlessTopicDiagnostic will use the new values. pub1_freq = (diagnostic_updater.HeaderlessTopicDiagnostic( 'topic1', updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10))) # Note that TopicDiagnostic, HeaderlessDiagnosedPublisher, # HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from # CompositeDiagnosticTask, so you can add your own fields to them using # the addTask method. # # Each time pub1_freq is updated, lower will also get updated and its # output will be merged with the output from pub1_freq. pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful). # If we know that the state of the node just changed, we can force an # immediate update. updater.force_update() # We can remove a task by refering to its name. if not updater.removeByName('Bound check'): node.get_logger().error( 'The Bound check task was not found when trying to remove it.') while rclpy.ok(): msg = std_msgs.msg.Bool() sleep(0.1) # Calls to pub1 have to be accompanied by calls to pub1_freq to keep # the statistics up to date. msg.data = False pub1.publish(msg) pub1_freq.tick() # We can call updater.update whenever is convenient. It will take care # of rate-limiting the updates. updater.update() rclpy.spin_once(node, timeout_sec=1)
# - add(name, fn): add a DiagnosticTask embodied by a name and function updater.add("Function updater", dummy_diagnostic) dc = DummyClass() updater.add("Method updater", dc.produce_diagnostics) # Internally, updater.add converts its arguments into a DiagnosticTask. # Sometimes it can be useful to work directly with DiagnosticTasks. Look # at FrequencyStatus and TimestampStatus in update_functions for a # real-life example of how to make a DiagnosticTask by deriving from # DiagnosticTask. # Alternatively, a FunctionDiagnosticTask is a derived class from # DiagnosticTask that can be used to create a DiagnosticTask from # a function. This will be useful when combining multiple diagnostic # tasks using a CompositeDiagnosticTask. lower = diagnostic_updater.FunctionDiagnosticTask("Lower-bound check", check_lower_bound) upper = diagnostic_updater.FunctionDiagnosticTask("Upper-bound check", check_upper_bound) # If you want to merge the outputs of two diagnostic tasks together, you # can create a CompositeDiagnosticTask, also a derived class from # DiagnosticTask. For example, we could combine the upper and lower # bounds check into a single DiagnosticTask. bounds = diagnostic_updater.CompositeDiagnosticTask("Bound check") bounds.addTask(lower) bounds.addTask(upper) # We can then add the CompositeDiagnosticTask to our Updater. When it is # run, the overall name will be the name of the composite task, i.e., # "Bound check". The summary will be a combination of the summary of the # lower and upper tasks (see DiagnosticStatusWrapper.mergeSummary for
def __init__(self): # TODO better way to deal bit mask self.ERRORS = { 0x000000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x000001: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "E-Stop"), 0x000002: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x000004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x000008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main Voltage High"), 0x000010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic Voltage High"), 0x000020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic Voltage Low"), 0x000040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M1 Driver Fault"), 0x000080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M2 Driver Fault"), 0x000100: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M1 Speed"), 0x000200: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M2 Speed"), 0x000400: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M1 Position"), 0x000800: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M2 Position"), 0x001000: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M1 Current"), 0x002000: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "M2 Current"), 0x010000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 Over Current"), 0x020000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 Over Current"), 0x040000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main Voltage High"), 0x080000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main Voltage Low"), 0x100000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x200000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x400000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "S4 Signal Triggered"), 0x800000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "S5 Signal Triggered"), 0x01000000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Speed Error Limit"), 0x02000000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Position Error Limit") } self.mainbattv = 0.0 self.logicbattv = 0.0 self.temp1c = 0.0 self.temp2c = 0.0 self.current1a = 0.0 self.current2a = 0.0 self.mainbatterystate = BatteryState() #rospy.init_node("roboclaw_node", log_level=rospy.DEBUG) rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) dev_name = rospy.get_param("~dev", "/dev/ttyACM0") baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) rospy.loginfo( "Connecting to RoboClaw with address {} on port {} and {} baud". format(self.address, dev_name, baud_rate)) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") self.encoders = rospy.get_param("~encoders", True) self.roboclaw = Roboclaw(dev_name, baud_rate) # TODO need someway to check if address is correct try: self.roboclaw.Open() 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 that 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: with self.mutex: version = self.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.loginfo("Version: {}".format(version[1].rstrip())) with self.mutex: self.roboclaw.SpeedM1M2(self.address, 0, 0) self.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", "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, queue_size=1) self.mainbatterystate_pub = rospy.Publisher('~mainbattery', BatteryState, queue_size=1) # setup main battery state self.mainbatterystate.header.frame_id = 'main_battery' self.mainbatterystate.location = 'main battery screw terminal' self.mainbatterystate.power_supply_status = 0 # 2 = DISCHARGING self.mainbatterystate.power_supply_technology = 3 # LIPO rospy.Timer(rospy.Duration(1.0), self.publish_batterystate) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("encoders %s", self.encoders) 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("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 make_diagnostic_task(self): diagnostic_task = diagnostic_updater.FunctionDiagnosticTask( self.name + ' Status', self.diagnostic_task) return diagnostic_task
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") self.baud_rate = int(rospy.get_param("~baud", "115200")) self._has_showed_message = False 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") self.roboclaw = Roboclaw(self.dev_name, self.baud_rate) self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = self.roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) if version is None: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) self.roboclaw.SpeedM1M2(self.address, 0, 0) #self.roboclaw.ResetEncoders(self.address) self.LINEAR_MAX_SPEED = float( rospy.get_param("linear/x/max_velocity", "2.0")) self.ANGULAR_MAX_SPEED = float( rospy.get_param("angular/z/max_velocity", "2.0")) self.TICKS_PER_METER = float(rospy.get_param("tick_per_meter", "10")) self.BASE_WIDTH = float(rospy.get_param("base_width", "0.315")) #self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() self.sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=5) self.TIMEOUT = 2 rospy.sleep(1) rospy.logdebug("dev %s", self.dev_name) rospy.logdebug("baud %d", self.baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")} rospy.init_node("roboclaw_node_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)
return stat def callback(self, data, wheel): self.temperature[wheel] = float(data.state.temperature_pcb) self.voltage[wheel] = float(data.state.voltage_input) if __name__ == '__main__': rospy.init_node("bb1_motor_driver") updater = diagnostic_updater.Updater() updater.setHardwareID("none") motor_driver = MotorDriver() lower_battery = diagnostic_updater.FunctionDiagnosticTask( "Undervoltage check", motor_driver.check_lower_bound_battery) upper_battery = diagnostic_updater.FunctionDiagnosticTask( "Overvoltage check", motor_driver.check_upper_bound_battery) upper_temperature = diagnostic_updater.FunctionDiagnosticTask( "Overtemperature check", motor_driver.check_upper_bound_temperature) bounds = diagnostic_updater.CompositeDiagnosticTask("Bound check") bounds.addTask(lower_battery) bounds.addTask(upper_battery) updater.add(bounds) updater.broadcast(0, "Initializing...") pub_battery = rospy.Publisher("bb1_motor_driver", std_msgs.msg.Bool,
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.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") #rospy.init_node("roboclaw_node", log_level=rospy.DEBUG) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.dev_name = rospy.get_param("~dev") self.baud_rate = int(rospy.get_param("~baud")) self.frontaddr = int(rospy.get_param("~frontaddr")) self.backaddr = int(rospy.get_param("~backaddr")) self.accel = int(rospy.get_param("~accel")) self._has_showed_message = False self.last_motor1_command = 0.0 self.last_motor2_command = 0.0 # self.accel_limit = 635 # ramp up to full speed (127) in 1/period * 127. current setting is 0.2 seconds self.roboclaw = Roboclaw(self.dev_name, self.baud_rate) status = self.roboclaw.Open() self.roboclaw.SetM1DefaultAccel(self.frontaddr, self.accel) # default is 655360 self.roboclaw.SetM2DefaultAccel(self.frontaddr, self.accel) self.roboclaw.SetM1DefaultAccel(self.backaddr, self.accel) self.roboclaw.SetM2DefaultAccel(self.backaddr, self.accel) rospy.loginfo("Roboclaw Port Status: " + str(status)) self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = self.roboclaw.ReadVersion(self.frontaddr) except Exception as e: rospy.logwarn("Problem getting front roboclaw version") rospy.logdebug(e) if version is None: rospy.logwarn("Could not get version from front roboclaw") else: rospy.logdebug("Version " + str(repr(version[1]))) try: version = self.roboclaw.ReadVersion(self.backaddr) except Exception as e: rospy.logwarn("Problem getting rear roboclaw version") rospy.logdebug(e) if version is None: rospy.logwarn("Could not get version from rear roboclaw") else: rospy.logdebug("Version " + str(repr(version[1]))) self.roboclaw.SpeedM1M2(self.frontaddr, 0, 0) self.roboclaw.ResetEncoders(self.frontaddr) self.roboclaw.SpeedM1M2(self.backaddr, 0, 0) self.roboclaw.ResetEncoders(self.backaddr) 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.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() self.sub = rospy.Subscriber("motor_vel", Mobility, 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("front address %d", self.frontaddr) rospy.logdebug("back address %d", self.frontaddr) rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node_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.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("udacity_bot_teleop_joystick/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, component, name, error_threshold=10, rate=1.0, create_watchdog=False): self.component = component self.name = name # Determines the OK, WARN and ERROR status flags self.error_threshold = error_threshold # Create a diagnostics updater self.diag_updater = diagnostic_updater.Updater() # Set the period from the rate self.diag_updater.period = 1.0 / rate # Set the hardware ID to name + pin try: self.diag_updater.setHardwareID(self.name + '_pin_' + str(self.component.pin)) except: self.diag_updater.setHardwareID(self.name) # Create a frequency monitor that tracks the publishing frequency for this component if self.component.rate > 0: if "Servo" in str(self.component): freq_bounds = diagnostic_updater.FrequencyStatusParam( { 'min': self.component.device.joint_update_rate, 'max': self.component.device.joint_update_rate }, 0.3, 5) self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic( self.component.name + '_freq', self.diag_updater, freq_bounds) else: freq_bounds = diagnostic_updater.FrequencyStatusParam( { 'min': self.component.rate, 'max': self.component.rate }, 0.3, 5) self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic( self.component.name + '_freq', self.diag_updater, freq_bounds) # Create an error monitor that tracks timeouts, serial exceptions etc self.error_diag = diagnostic_updater.FunctionDiagnosticTask( self.name, self.get_error_rate) self.diag_updater.add(self.error_diag) # Create a watchdog diagnostic to monitor the connection status if create_watchdog: self.watchdog_diag = diagnostic_updater.FunctionDiagnosticTask( self.name, self.get_connection_status) self.diag_updater.add(self.watchdog_diag) # Counters used for diagnostics self.reads = 0 self.total_reads = 0 self.errors = 0 self.error_rates = [0.0] # Connection status for device self.watchdog = False
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)