ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Trying to save HDF5 file in ROS Odometry node; .h5 file not being saved

asked 2022-07-15 00:17:01 -0600

hunterlineage1 gravatar image

updated 2022-07-18 00:56:41 -0600

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")
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2022-07-20 02:57:39 -0600

maybe add some explicit file location to save to ? Key question is what are you saving it for ?

I like to write numpy to .csv for easy visualisation / analysis e.g. for numpy array "data":

with open("/home/mine/logs/log.csv", mode="a") as linefile:
    data.tofile(linefile, sep=',')
    linefile.write('\n')
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-07-15 00:17:01 -0600

Seen: 172 times

Last updated: Jul 20 '22