From 8c65a29ac119e0461d2ca48e4255e80b9191dfc9 Mon Sep 17 00:00:00 2001 From: Florian Zirker Date: Tue, 15 Jan 2019 17:13:48 +0100 Subject: [PATCH] bag file lesen neu --- launch/export-data.launch | 11 +++++++ launch/gmapping-bagfile.launch | 13 --------- src/export-data.py | 53 ++++++++++++++++++++++++---------- 3 files changed, 48 insertions(+), 29 deletions(-) create mode 100644 launch/export-data.launch delete mode 100644 launch/gmapping-bagfile.launch diff --git a/launch/export-data.launch b/launch/export-data.launch new file mode 100644 index 0000000..4c58174 --- /dev/null +++ b/launch/export-data.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/launch/gmapping-bagfile.launch b/launch/gmapping-bagfile.launch deleted file mode 100644 index 295e9db..0000000 --- a/launch/gmapping-bagfile.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - diff --git a/src/export-data.py b/src/export-data.py index 65af273..16180cc 100755 --- a/src/export-data.py +++ b/src/export-data.py @@ -4,30 +4,51 @@ import rosbag import sys import os from wlan_pioneer.msg import WlanSignalMsg -from geometry_msgs.msg import Point import datetime +import tf2_msgs +import geometry_msgs +import rospy +import tf2_ros + + +rospy.init_node('wlan_bag_listener') inFileName = os.path.abspath(sys.argv[1]) outFileName = os.path.join(os.path.dirname(inFileName), os.path.basename(inFileName).split(".")[0] + ".csv") -count = 0 -pos = Point() -bag = rosbag.Bag(inFileName) -outFile = open(outFileName, "w") -outFile.write("SecsSinceEpoch;xPos;yPos;level2G4;level5G\n") +countOut = 0 +outFile = open(outFileName, "w", buffering=1) +trans = geometry_msgs.msg.TransformStamped() +tfBuffer = tf2_ros.Buffer() +listener = tf2_ros.TransformListener(tfBuffer) +rate = rospy.Rate(10.0) -for topic, msg, timestamp in bag.read_messages(topics=['/wlan_signal', '/pose']): +def wlanCallback(msg, args): + trans = geometry_msgs.msg.TransformStamped() + outFile = args[0] + if (msg.level_2G4 == -128 and msg.level_5G == -128): + return - if (topic == "/pose"): - pos = msg.pose.pose.position + try: + trans = tfBuffer.lookup_transform("map", "base_link", rospy.Time()) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + pass - if (topic == "/wlan_signal"): - timeStr = datetime.datetime.fromtimestamp(msg.timestamp.secs).isoformat() - outFile.write("%s;%f;%f;%d;%d\n" - % (timeStr, pos.x, pos.y, msg.level_2G4, msg.level_5G)) + rospy.loginfo ("line %d: %s,%f,%f,%d,%d" % + (0, msg.timestamp.secs, trans.transform.translation.x, trans.transform.translation.y, + msg.level_2G4, msg.level_5G)) - count += 1 + outFile.write("%s,%f,%f,%d,%d\n" + % (msg.timestamp.secs, trans.transform.translation.x, trans.transform.translation.y, + msg.level_2G4, msg.level_5G)) -print "%d rows written to %s" % (count, outFileName) + +rospy.Subscriber("wlan_signal", WlanSignalMsg, wlanCallback, (outFile,)) +outFile.write("SecsSinceEpoch,xPos,yPos,level2G4,level5G\n") + +rospy.spin() + +#rospy.loginfo "%d Messages read from bag file\n%d rows written to %s" % (countBag, countOut, outFileName) outFile.close() -bag.close() + +