def spin(self, rate_val: int = None):
""" Wrapping the spinner function. """
# Here's where we're doing the actual spinning: read the pin, set up a message, publish,
# rate.sleep(), repeat.
if rate_val is None:
rate_val = _DEFAULT_RATE_VAL
rate = rospy.Rate(rate_val)
while not rospy.is_shutdown():
for pin_obj in self._generic_pin_objects.values():
if pin_obj.type == 'input':
val = pin_obj.get()
header = Header()
header.stamp = rospy.Time.now()
# Different implementations might give us this in int or bool form.
# Check both and do the appropriate thing with each.
if (isinstance(val, int)
and val == 1) or (isinstance(val, bool) and val):
val = True
elif (isinstance(val, int)
and val == 0) or (isinstance(val, bool) and not val):
val = False
else:
rospy.logerr("Not sure how to deal with " + str(val))
try:
self._publishers[str(pin_obj.pin)].publish(
InputState(header, val, str(pin_obj.pin)))
except KeyError:
rospy.logfatal_once("KeyError, you tried getting " +
str(pin_obj.pin) + " but only " +
str(self._publishers.keys()) +
" is acceptable")
rate.sleep()
def test_log(self):
rosout_logger = logging.getLogger('rosout')
import rospy
self.assertTrue(len(rosout_logger.handlers) == 2)
self.assertTrue(rosout_logger.handlers[0],
rosgraph.roslogging.RosStreamHandler)
self.assertTrue(rosout_logger.handlers[1],
rospy.impl.rosout.RosOutHandler)
default_ros_handler = rosout_logger.handlers[0]
# Remap stdout for testing
lout = StringIO()
lerr = StringIO()
test_ros_handler = rosgraph.roslogging.RosStreamHandler(colorize=False,
stdout=lout,
stderr=lerr)
try:
# hack to replace the stream handler with a debug version
rosout_logger.removeHandler(default_ros_handler)
rosout_logger.addHandler(test_ros_handler)
import rospy
rospy.loginfo("test 1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test 1" in lout_last)
rospy.logwarn("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test 2" in lerr_last)
rospy.logerr("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test 3" in lerr_last)
rospy.logfatal("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test 4" in lerr_last)
# logXXX_once
lout_len = -1
for i in range(3):
rospy.loginfo_once("test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
else: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logwarn_once("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logerr_once("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logfatal_once("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
# logXXX_throttle
lout_len = -1
for i in range(3):
rospy.loginfo_throttle(3, "test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in lout_last)
lerr_len = -1
for i in range(3):
rospy.logwarn_throttle(3, "test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logerr_throttle(3, "test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logfatal_throttle(3, "test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in lerr_last)
# logXXX_throttle_identical
lout_len = -1
for i in range(5):
if i < 2:
test_text = "test 1"
else:
test_text = "test 1a"
rospy.loginfo_throttle_identical(2, test_text)
lout_last = lout.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
elif i == 1: # Confirm the length of lout hasn't changed
self.assert_(lout_len == len(lout.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lout has changed
self.assert_(lout_len != len(lout.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lout_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 2"
else:
test_text = "test 2a"
rospy.logwarn_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 3"
else:
test_text = "test 3a"
rospy.logerr_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 4"
else:
test_text = "test 4a"
rospy.logfatal_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
rospy.loginfo("test child logger 1", logger_name="log1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test child logger 1" in lout_last)
rospy.logwarn("test child logger 2", logger_name="log2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test child logger 2" in lerr_last)
rospy.logerr("test child logger 3", logger_name="log3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test child logger 3" in lerr_last)
rospy.logfatal("test child logger 4", logger_name="log4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test child logger 4" in lerr_last)
finally:
# restoring default ros handler
rosout_logger.removeHandler(test_ros_handler)
lout.close()
lerr.close()
rosout_logger.addHandler(default_ros_handler)
YLIM = 6
XLIM = 6
# global variables and declarations
# pylint: disable-msg=C0325
# pylint: disable-msg=C0103
# parameters for augmented MCL
w_slow = 0
w_fast = 0
alpha_slow = 0.1
alpha_fast = 0.7
Z_XM = get_likelihood_field(20, MAP_L, "gaussian", [0.212, 0.2, 20])
a = np.sum(Z_XM)
while (np.isnan(a)):
rospy.logfatal_once("caught Nan in get_likelihood_field")
pass
# to plot the targets and scans optionally
# based on GLOBAL_VARIABLE
if (ANIMATE):
rospy.loginfo("interactive plotting is turned on")
plt.ion()
fig, ax = plt.subplots(1, 1)
# ax.set_autoscaley_on(True)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_xlim([-XLIM, XLIM])
ax.set_ylim([-YLIM, YLIM])
ax.grid()
ax.legend()
line_particles = []
def test_log(self):
# we can't do anything fancy here like capture stdout as rospy
# grabs the streams before we do. Instead, just make sure the
# routines don't crash.
real_stdout = sys.stdout
real_stderr = sys.stderr
try:
try:
from cStringIO import StringIO
except ImportError:
from io import StringIO
sys.stdout = StringIO()
sys.stderr = StringIO()
import rospy
rospy.loginfo("test 1")
self.assert_("test 1" in sys.stdout.getvalue())
rospy.logwarn("test 2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test 3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test 4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test 4" in sys.stderr.getvalue())
# logXXX_once
for i in range(3):
sys.stdout = StringIO()
rospy.loginfo_once("test 1")
if i == 0:
self.assert_("test 1" in sys.stdout.getvalue())
else:
self.assert_("" == sys.stdout.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logwarn_once("test 2")
if i == 0:
self.assert_("test 2" in sys.stderr.getvalue())
else:
self.assert_("" == sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logerr_once("test 3")
if i == 0:
self.assert_("test 3" in sys.stderr.getvalue())
else:
self.assert_("" == sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logfatal_once("test 4")
if i == 0:
self.assert_("test 4" in sys.stderr.getvalue())
else:
self.assert_("" == sys.stderr.getvalue())
# logXXX_throttle
for i in range(3):
sys.stdout = StringIO()
rospy.loginfo_throttle(3, "test 1")
if i == 0:
self.assert_("test 1" in sys.stdout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stdout.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in sys.stdout.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logwarn_throttle(3, "test 2")
if i == 0:
self.assert_("test 2" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logerr_throttle(3, "test 3")
if i == 0:
self.assert_("test 3" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logfatal_throttle(3, "test 4")
if i == 0:
self.assert_("test 4" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in sys.stderr.getvalue())
rospy.loginfo("test child logger 1", logger_name="log1")
self.assert_("test child logger 1" in sys.stdout.getvalue())
rospy.logwarn("test child logger 2", logger_name="log2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test child logger 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test child logger 3", logger_name="log3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test child logger 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test child logger 4", logger_name="log4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test child logger 4" in sys.stderr.getvalue())
finally:
sys.stdout = real_stdout
sys.stderr = real_stderr
nh = rospy.init_node('ibuki_actuators_'+dev_name, anonymous = True)
param_config = utils.read_yaml('xilva_core', 'ibuki')
_IP = param_config['IP']
_PORT = param_config['PORT']
rate = rospy.Rate(_RATE)
mbed_joint = Joint()
logtext = logs.Roslog("ibuki_actuator")
sub = rospy.Subscriber('/ibuki/encoded_commands', EvansString, callback, mbed_joint)
while not rospy.is_shutdown():
if (mbed_joint.payload != ''):
break
rospy.sleep(0.5)
rospy.loginfo("Waiting commands...")
while not rospy.is_shutdown():
embed_msg = mbed_joint.payload
try:
motorsock.sendto(embed_msg, (_IP[dev_name], _PORT[dev_name]))
except socket.error as error:
if error.errno == errno.ENETUNREACH:
rospy.logfatal_once(logtext.report_log(99))
else:
raise
rate.sleep()