Trying to save HDF5 file in ROS Odometry node; .h5 file not being saved
I am using ROS Noetic on Ubuntu 20.04. I am running a ros node which saves odometry data in a logfile. The log file successfully saves the odometry data; it looks like this:
pose: position: x: 0.0027880480174993327 y: -0.0029414520079247327 z: 0.00563770744057384 orientation: x: 0.0027310495883615888 y: 0.001136773836533139 z: -0.0004563577307679678 w: 0.9999955204154727, header: seq: 0 stamp: secs: 1657741393 nsecs: 358656466 frame_id: “map”
The file extension is .log for the above portion. When I try to save the odometry data in the form of a .h5 file or a .npy file, no error appears on my terminal but the saved odometry files are nowhere to be found. Please see my code:
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
import numpy as np
from numpy import savetxt, save
import h5py
odomdata = np.zeros((1,8))
global hf
hf = h5py.File('odomData.h5', 'w')
def callback(data):
global odomdata
# rospy.loginfo(rospy.get_caller_id() + 'data: %s', data.data) # for String
# rospy.loginfo(data.pose.pose) # for Odometry
rospy.loginfo(data.poses) # for Path
x_pos = data.poses[-1].pose.position.x
y_pos = data.poses[-1].pose.position.y
z_pos = data.poses[-1].pose.position.z
x_orient = data.poses[-1].pose.orientation.x
y_orient = data.poses[-1].pose.orientation.y
z_orient = data.poses[-1].pose.orientation.z
w_orient = data.poses[-1].pose.orientation.w
sec = data.poses[-1].header.stamp.secs
nsec = data.poses[-1].header.stamp.nsecs * (10**-9)
float_time = sec + nsec
odomarray = np.array([[float_time, x_pos, y_pos, z_pos, x_orient, y_orient, z_orient, w_orient]])
# print(odomarray)
odomdata = np.append(odomdata, odomarray, axis=0)
save('odomData.npy', odomdata)
print("I'M SAVING NPY NOW 1")
# print(odomdata)
def listener():
rospy.init_node('odomlistener')
# rospy.Subscriber('ssl_slam_odom_estimation_node', String, callback) #subscribe to odometry topic;
rospy.Subscriber('/ssl_slam/trajectory', Path, callback)
# rospy.Subscriber('odom', Odometry, callback)
save('odomData.npy', odomdata)
print("I'M SAVING NPY NOW 2")
if rospy.is_shutdown(): # trying to save the data upon closing
save('odomData.npy', odomdata)
print("I'M SAVING NPY NOW 3")
hf.create_dataset('dataset_1', data=odomdata)
hf.close()
print("I'M SAVING H5 NOW 1")
rospy.spin() # spin() simply keeps python from exiting until this node is stopped
if __name__ == '__main__':
try:
listener()
finally:
save('odomData.npy', odomdata)
print("I'M SAVING NPY NOW 4")
hf.create_dataset('dataset_1', data=odomdata)
hf.close()
print("I'M SAVING H5 NOW 2")