def add_meas(self,
ros_meas,
delta_multiplier=USE_DELTA_TIERS,
force_fuse=True):
"""Adds a measurement to all ledger filters.
If Measurement is after last correction step, it will be fused on next correction step
Arguments:
ros_meas {etddf.Measurement.msg} -- Measurement taken
Keyword Arguments:
delta_multiplier {int} -- Delta multiplier to use for this measurement (default: {USE_DELTA_TIERS})
force_fuse {bool} -- If measurement is in the past, fuse it on the next update step anyway (default: {True})
Note: the ledger will still reflect the correct measurement time
"""
src_id = self.asset2id[ros_meas.src_asset]
if ros_meas.measured_asset in self.asset2id.keys():
measured_id = self.asset2id[ros_meas.measured_asset]
elif ros_meas.measured_asset == "":
measured_id = -1
else:
rospy.logerr_once("ETDDF doesn't recognize: " +
ros_meas.measured_asset + " ... ignoring")
return
self.main_filter.add_meas(ros_meas, src_id, measured_id,
delta_multiplier, force_fuse)
for key in self.delta_tiers.keys():
self.delta_tiers[key].add_meas(ros_meas, src_id, measured_id,
delta_multiplier, force_fuse)
def run(self):
while not rospy.is_shutdown():
try:
while not self._queue.empty():
# don't let requests accumulate
while self._queue.qsize() > 2:
text, responseQueue = self._queue.get()
if responseQueue:
responseQueue.put('skipped')
text, responseQueue = self._queue.get()
rospy.loginfo('Saying:' + text)
self._talkingPub.publish(True)
if ENGINE == 'google':
self._googleTTS.say(text)
else:
self._espeak.say(text, sync=True)
self._talkingPub.publish(False)
if responseQueue:
responseQueue.put('done')
rospy.loginfo('Done saying it')
except:
rospy.logerr_once(traceback.format_exc())
rospy.sleep(PERIOD)
def set_controller_mode(self, data):
if data.controller_mode not in ['SPEED', 'PWM']:
rospy.logerr_once(
'ERROR: Given controller mode is not supported, either enter "SPEED" or "PWM"'
+ ', I got: ' + str(data.controller_mode))
return set_controller_modeResponse("Cannot change mode to: " +
str(data.controller_mode))
self._mode = data.controller_mode
return set_controller_modeResponse(
'Successfully changed the controller mode to {0}'.format(
self._mode))
def __init__(self):
self.ready = False
self.got_position = False
self.airborne= False
rospy.init_node('pixhawk_takeoff', anonymous=True)
self.takeoff_height = rospy.get_param('~takeoff_height')
self.subscriber_pos = rospy.Subscriber('~global_pos_in', NavSatFix, self.callback_global_pos)
self.service_arming = rospy.ServiceProxy('~arming_out', CommandBool)
self.service_set_mode = rospy.ServiceProxy('~mode_out', SetMode)
self.service_push_wp = rospy.ServiceProxy('~waypoint_push', WaypointPush)
self.ready = True
while not self.got_position and not rospy.is_shutdown():
rospy.sleep(0.1)
rospy.loginfo_once('Waiting for vehicle position...')
# #{ Prepare mission
wp0 = Waypoint()
wp0.frame = 3 # GLOBAL_REL_ALT
wp0.command = 22 # TAKEOFF
wp0.is_current = True
wp0.x_lat = self.latitude
wp0.y_long = self.longitude
wp0.z_alt = self.takeoff_height
wp0.autocontinue = True
wp1 = Waypoint()
wp1.frame = 3 # GLOBAL_REL_ALT
wp1.command = 17 # NAV_LOITER_UNLIM
wp1.is_current = False
wp1.x_lat = self.latitude
wp1.y_long = self.longitude
wp1.z_alt = self.takeoff_height
wp1.autocontinue = True
# #}
resp = self.service_push_wp.call(0, [wp0, wp1])
print(resp.success)
print(resp.wp_transfered)
self.switch_to_mode('AUTO.MISSION')
while not self.call_arming() and not rospy.is_shutdown():
rospy.logerr_once('Arming failed. Retrying...')
rospy.sleep(0.01)
rospy.loginfo('Vehicle armed')
def detect_tags(self, img):
self.img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
self.detections = self.detector.detect(self.img)
if len((self.detections)) > 1:
rospy.logerr_once("Only one tag can be in the frame!")
elif len((self.detections)) < 1:
rospy.logerr_once("No tags detected!")
self.tag = TagDetection()
else:
self.tag.id = self.detections[0]['id']
self.tag.center.x = self.detections[0]['center'][0]
self.tag.center.y = self.detections[0]['center'][1]
temp = self.detections[0]['lb-rb-rt-lt']
self.tag.corner_bl.x = temp[0][0]
self.tag.corner_bl.y = temp[0][1]
self.tag.corner_br.x = temp[1][0]
self.tag.corner_br.y = temp[1][1]
self.tag.corner_tr.x = temp[2][0]
self.tag.corner_tr.y = temp[2][1]
self.tag.corner_tl.x = temp[3][0]
self.tag.corner_tl.y = temp[3][1]
return self.tag
def writeToFile(self):
"""
Function to be called on shutdown. Stops timer and writes data to a file
"""
parentDir = os.path.dirname(__file__)
fname = os.path.abspath(os.path.join(parentDir, '..','results',self.filename+'.xlsx'))
with pd.ExcelWriter(fname) as writer:
rospy.loginfo("Writing results to file: {}".format(fname))
for topic in self.loglist:
topic.timer.shutdown()
if topic.logging_df.shape[0] > 2:
topic.logging_df.to_excel(writer, sheet_name=topic.topic)
if __name__=="__main__":
rospy.init_node("Freq_logger")
topics = rospy.get_param("/topics", default=None)
if topics is None:
rospy.logwarn("No topics specified. Looking for All topics")
topics = rostopic.get_topic_list()
# use the wrapper list holding object
LL = LoggerList(topics)
while not rospy.is_shutdown():
try:
rospy.spin()
except rospy.ROSInterruptException as e:
rospy.logerr_once(e)
def process_yaml(filename, output_dir_name, bag_name) -> List[BaseSerializer]:
parsed_yaml = dict()
serializer_list = list()
if output_dir_name[-1] != '/':
output_dir_name += '/'
output_dir_name += bag_name.split(".")[0] + '/'
with open(filename, 'r') as stream:
try:
parsed_yaml = yaml.safe_load(stream)
except yaml.YAMLError as exc:
rospy.logerr_once(exc.__str__())
for item in parsed_yaml:
name = list(item.keys())[0]
type = item[name]["type"]
topic_name = item[name]["topic_name"]
ser = None
if type == "Mesh":
ser = MeshSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "PointCloud":
ser = PCLSerializer(topic_name, directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Image":
is_depth = bool(item[name]['is_depth'])
channels = int(item[name]['channels'])
ser = ImageSerializer(topic_name,
is_depth,
channels,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "ORB_Image":
is_depth = bool(item[name]['is_depth'])
ser = ImageORBSerializer(topic_name,
is_depth,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Odometry":
ser = OdomSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Pose":
ser = PoseSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "CamInfo":
ser = CamInfoSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "FloatArray":
ser = FloatArrSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "IntArray":
ser = IntArrSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Twist":
ser = TwistSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Map":
ser = MapSerializer(topic_name, directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "Path":
ser = PathSerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
elif type == "ToeArray":
ser = ToeArraySerializer(topic_name,
directory_name=output_dir_name)
rospy.logdebug("Created {} Serializer listening to {}".format(
type, topic_name))
else:
rospy.logerr_once("[BHARVEST] Type {} is not an implemented "
"serializer type.".format(type))
continue
serializer_list.append(ser)
return serializer_list
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)
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
