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

Dynamically updating 3D plot in python using saved Odometry timestamp, quaternion, and translation data

asked 2022-07-19 14:09:35 -0500

hunterlineage1 gravatar image

I have data saved in the form of a numpy array that looks like the following: [[timestamp, translation_x, translation_y, translation_z, quaternion_x, quaternion_y, quaternion_z, quaternion_w]]. There are multiple rows in this matrix.

I am looking to plot (in 3D) the above data in a dynamically updating map based on the quaternion and translation corresponding to their timestamps. So I want a certain orientation and translation of a moving axis at its corresponding timestamp. The data was collected at a frequency of 31fps. How do I achieve this? Which python library do I use, I am aware of matplotlib, RViz, open3d.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-07-20 02:33:02 -0500

updated 2022-07-21 00:04:49 -0500

I assume your data is from a ROS odometry "pose": is that where you got it from ? or where from ?

Normally a robot's pose is broadcast as a ROS tf transform, which is well visualised in RVIZ: so if that was the source, just select it in RVIZ.

If from other source, you can use python to put it into tf-format, broadcast it, and then see in RVIZ.

If this is of interest, I can post some sample code to put odom pose into a tf. e.g. this worked for me:

#!/usr/bin/env python3

import rospy
import tf2_ros
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped

rospy.init_node("posetf")
tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(4))
tf_listener = tf2_ros.TransformListener(tf_buffer)
br = tf2_ros.TransformBroadcaster()

def get_odom(msg):                      # CallBack: at ~ 30 Hz nav.message ...
    # ================ broadcast new tf : transout =======
    transout = TransformStamped()
    transout.header.stamp = msg.header.stamp
    transout.header.frame_id = msg.header.frame_id          # e.g. this is likely "odom" frame
    transout.child_frame_id = "newpose"
    transout.transform.translation = msg.pose.pose.position
    transout.transform.rotation = msg.pose.pose.orientation
    br.sendTransform(transout)

# ===================================  Start subscribers etc ... ========================================
rospy.Subscriber('/odom', Odometry, get_odom, queue_size=1)     # nav_msgs/Odometry
rospy.spin()

The "newpose" tf then simply appears in RVIZ list of TF Frames ... easy to see as it moves around...

edit flag offensive delete link more

Comments

@Slider This is super helpful! Yes, I did get the data from ROS odometry "pose." Please help by showing sample code for using python to put it into tf-format, broadcast it, and then visualizing in RVIZ, and also sample code for putting odom pose into a tf.

Update: thank you for the code sample. How would I be able to use pre-recorded odom pose data in the form of [[timestamp, translation_x, translation_y, translation_z, quaternion_x, quaternion_y, quaternion_z, quaternion_w]] numpy array to play in RVIZ? I am trying to work on a project which uses recorded data. The goal for me is to be able to play the odom pose data in the form of a numpy array, use python to put it into tf-format, broadcast it, and then see in RVIZ. With the code sample you showed, will I be able to record the data and play it again in RVIZ?

hunterlineage1 gravatar image hunterlineage1  ( 2022-07-20 15:32:56 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2022-07-19 14:09:35 -0500

Seen: 145 times

Last updated: Jul 21 '22