bag file lesen neu

This commit is contained in:
Florian Zirker 2019-01-15 17:13:48 +01:00
parent a8b91943b8
commit 8c65a29ac1
3 changed files with 48 additions and 29 deletions

11
launch/export-data.launch Normal file
View file

@ -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>

View file

@ -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>

View file

@ -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()