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)
