From 8c65a29ac119e0461d2ca48e4255e80b9191dfc9 Mon Sep 17 00:00:00 2001
From: Florian Zirker <florian.zirker@kapsweyer.de>
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 @@
+<launch>
+
+   <param name="use_sim_time" value="true" />
+   <!-- <arg name="bagfile" value="/home/flo//Nextcloud/Studium/Master/2-IM/AMR_Autonome-Mobile-Roboter/Wifi-Pioneer/bag-files/wlan_pioneer_2018-12-18-18-43-47.bag" /> -->
+   <arg name="bagfile" value="/home/flo//Nextcloud/Studium/Master/2-IM/AMR_Autonome-Mobile-Roboter/Wifi-Pioneer/bag-files/wlan_pioneer_2018-12-17-17-14-03.bag" />
+
+   <node pkg="wlan_pioneer" type="export-data.py" name="export_data" args="$(arg bagfile)" />
+   <node pkg="rosbag" name="rosbag" type="play" args="--clock $(arg bagfile)" required="true"/>
+
+</launch>
+
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 @@
-<launch>
-
-   <param name="use_sim_time" value="true" />
-   <include file="$(find p2os_launch)/launch/gmapping.launch" />
-   <node name="rviz" pkg="rviz" type="rviz" />
-   <node pkg="rosbag" name="rosbag" type="play" args="--clock /home/pioneer/Desktop/bagfiles/wlan_pioneer_2018-12-18-18-43-47.bag"/>
-   
-
-   <node pkg="rosbag" type="record" name="rosbag_record_gmapping_post"
-        args="record -o /tmp/wlan_pioneer_postprocessing /pose /scan /tf /wlan_signal /map" />
-
-</launch>
-
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()
+
+