{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# ROS integration\n",
"\n",
"rigid_body_motion supports certain ROS functionality, provided the Python environment has been set up with the required ROS packages. This guide assumes that you are at least somewhat familiar with ROS concepts such as nodes, publishers/subscribers and messages. If not, the [ROS tutorials](http://wiki.ros.org/ROS/Tutorials) are a good place to start.\n",
"\n",
"You also need to set up a couple of dependencies which can be done very conveniently if you are using an Anaconda Python distribution. See [the ROS dependencies installation guide](installation.rst#ros-integration) for further information."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Note\n",
" \n",
"The following examples require the `pooch`, `xarray`, `netcdf4` and `ipywidgets` libraries.\n",
"
"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import pandas as pd\n",
"import rigid_body_motion as rbm"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Loading data from rosbag files\n",
"\n",
"Data can be loaded from rosbag files into numpy arrays. So far, `geometry_msgs/TransformStamped` and `nav_msgs/Odometry` messages are supported. This is done through the [RosbagReader](_generated/rigid_body_motion.ros.RosbagReader.rst) class:"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"reader = rbm.ros.RosbagReader(rbm.example_data[\"rosbag\"])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The reader can be used as a context manager to facilitate opening and closing of the rosbag. The `get_topics_and_types` method returns a dict with topic names and corresponding message types:"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'/pupil/left_eye/transform': 'geometry_msgs/TransformStamped',\n",
" '/pupil/right_eye/transform': 'geometry_msgs/TransformStamped',\n",
" '/t265/transform': 'geometry_msgs/TransformStamped'}"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"with reader:\n",
" info = reader.get_topics_and_types()\n",
"info"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Note\n",
" \n",
"If you get an `ModuleNotFoundError: No module named 'rosbag'` at this point, there is an issue with the ROS dependencies. See [the ROS dependencies troubleshooting guide](installation.rst#troubleshooting) to fix this.\n",
"
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The data included in the example rosbag is from a [head/eye tracking study](https://dl.acm.org/doi/pdf/10.1145/3379156.3391365) and contains head-in-world pose estimated by the Intel RealSense T265 as well as eye-in-head pose for both eyes estimated by the Pupil Core eye tracker. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The `load_messages` method returns a dict with the data from a specified topic:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'timestamps': array([1.58060323e+09, 1.58060323e+09, 1.58060323e+09, ...,\n",
" 1.58060357e+09, 1.58060357e+09, 1.58060357e+09]),\n",
" 'position': array([[15.9316, 0.8211, 10.5429],\n",
" [15.9354, 0.8208, 10.5382],\n",
" [15.9393, 0.8204, 10.5335],\n",
" ...,\n",
" [29.8883, 2.8952, 7.6317],\n",
" [29.8888, 2.8943, 7.6249],\n",
" [29.8892, 2.8935, 7.6182]]),\n",
" 'orientation': array([[-0.9687, 0.0917, 0.2306, 0.0039],\n",
" [-0.969 , 0.0915, 0.2295, 0.005 ],\n",
" [-0.9693, 0.0912, 0.2285, 0.0061],\n",
" ...,\n",
" [-0.9915, 0.0915, 0.0929, -0.0022],\n",
" [-0.9914, 0.0922, 0.0927, -0.0017],\n",
" [-0.9913, 0.0932, 0.0925, -0.001 ]])}"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"with reader:\n",
" head = reader.load_messages(\"/t265/transform\")\n",
"head"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we can construct a reference frame tree with this data:"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"rbm.register_frame(\"world\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The T265 uses the VR coordinate convention (x right, y up, z towards the back of the device) which differs from the default ROS convention (x forward, y left, z up):"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"R_T265_ROS = np.array([[0.0, 0.0, -1.0], [-1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])\n",
"\n",
"rbm.ReferenceFrame.from_rotation_matrix(\n",
" R_T265_ROS, parent=\"world\", name=\"t265/world\"\n",
").register()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The recorded data describes the transformation from the T265 world frame to the tracker frame:"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"rbm.register_frame(\n",
" \"t265/tracker\",\n",
" parent=\"t265/world\",\n",
" translation=head[\"position\"],\n",
" rotation=head[\"orientation\"],\n",
" timestamps=pd.to_datetime(head[\"timestamps\"], unit=\"s\"),\n",
")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For visualization, we define an additional `\"head\"` frame that represents the tracker pose with the ROS coordinate convention:"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"rbm.ReferenceFrame.from_rotation_matrix(\n",
" R_T265_ROS, parent=\"t265/tracker\", name=\"head\", inverse=True,\n",
").register()"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"world\n",
"└── t265/world\n",
" └── t265/tracker\n",
" └── head\n"
]
}
],
"source": [
"rbm.render_tree(\"world\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Visualization with RViz\n",
"\n",
"This section will show you how to broadcast reference frame transforms on the `/tf` topic as well as publish other messages that are useful for visualization in RViz. If you are not familiar with RViz and/or tf, check out the [RViz user guide](http://wiki.ros.org/rviz/UserGuide) and the [tf package documentation](http://wiki.ros.org/tf). You can download an `.rviz` file where all topics created in the following are already set up [here](_static/example.rviz)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We start by creating a node for this notebook with the [init_node()](_generated/rigid_body_motion.ros.init_node.rst) method. This method will also automatically start a `roscore` when `start_master=True` and another ROS master isn't already running:"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[1mstarted roslaunch server http://DESKTOP:41375/\u001b[0m\n",
"ros_comm version 1.15.9\n",
"\n",
"\n",
"SUMMARY\n",
"========\n",
"\n",
"PARAMETERS\n",
" * /rosdistro: noetic\n",
" * /rosversion: 1.15.9\n",
"\n",
"NODES\n",
"\n",
"auto-starting new master\n",
"\u001b[1mprocess[master]: started with pid [15323]\u001b[0m\n",
"\u001b[1mROS_MASTER_URI=http://localhost:11311\u001b[0m\n",
"\u001b[1msetting /run_id to master\u001b[0m\n",
"\u001b[1mprocess[rosout-1]: started with pid [15333]\u001b[0m\n",
"started core service [/rosout]\n"
]
}
],
"source": [
"master = rbm.ros.init_node(\"rbm_vis\", start_master=True)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Transforms between reference frames can easily be broadcast on the `/tf` topic with the [ReferenceFrameTransformBroadcaster](_generated/rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.rst) class:"
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {},
"outputs": [],
"source": [
"tf_world_head = rbm.ros.ReferenceFrameTransformBroadcaster(\"head\", base=\"world\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"When calling [publish](_generated/rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.rst#rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.publish) the transform between world and head frame will be broadcast on the `/tf` topic. Since the head frame is a moving frame this will broadcast the first valid transform between the two frames by default:"
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {},
"outputs": [],
"source": [
"tf_world_head.publish()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"RViz should now show both frames:\n",
"\n",
"![](\"_static/img/rviz_0.png\")
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The broadcaster stores all valid transforms between the two frames in the `translation`, `rotation` and `timestamps` attributes:"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(66629, 3)"
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"tf_world_head.translation.shape"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can broadcast the transform between two frames at different points in time by specifying an index into these arrays as an argument to [publish](_generated/rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.rst#rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.publish):"
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [],
"source": [
"tf_world_head.publish(1000)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Note \n",
"\n",
"When \"going back in time\", i.e., broadcasting transforms with timestamps older than the latest broadcast timestamp, RViz will not update the tf display and you may get a `TF_OLD_DATA` warning in the console. When this happens, click on the \"Reset\" button in the lower left corner in RViz.\n",
"
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The entire transformation between moving frames can be visualized with the [ReferenceFrameMarkerPublisher](_generated/rigid_body_motion.ros.ReferenceFrameMarkerPublisher.rst) which publishes the translation of all valid timestamps as a `visualization_msgs/Marker` message on the `//path` topic: "
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [],
"source": [
"marker_publisher = rbm.ros.ReferenceFrameMarkerPublisher(\"head\", base=\"world\")\n",
"marker_publisher.publish()"
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[['/head/path', 'visualization_msgs/Marker']]"
]
},
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"import rospy\n",
"\n",
"rospy.get_published_topics(\"/head\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"RViz should now show the trajectory of the head frame as a white line:\n",
"\n",
"![](\"_static/img/rviz_1.png\")
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Next, we will load data from the Pupil Core eye tracker to demonstrate how multiple moving frames can be visualized. With the [load_dataset](_generated/rigid_body_motion.ros.RosbagReader.rst#rigid_body_motion.ros.RosbagReader.load_dataset) method, the data is imported as an `xarray.Dataset` and `cache=True` enables local caching in the netCDF4 format for faster loading."
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [],
"source": [
"with reader:\n",
" left_eye = reader.load_dataset(\"/pupil/left_eye/transform\", cache=True)\n",
"\n",
"left_eye = left_eye.dropna(\"time\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The transform between the T265 and the Pupil Core scene camera was determined in the study with a camera calibration routine and is provided here as hard-coded parameters:"
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {},
"outputs": [],
"source": [
"t_t265_pupil = (24.5e-3, -29e-3, 0.0)\n",
"r_t265_pupil = (-0.00125, -1.0, 6.3463e-05, 3.977e-06)\n",
"\n",
"rbm.register_frame(\n",
" \"pupil/scene_cam\",\n",
" parent=\"t265/tracker\",\n",
" translation=t_t265_pupil,\n",
" rotation=r_t265_pupil,\n",
")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The reference frame of the left eye is constructed from the Dataset:"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {},
"outputs": [],
"source": [
"rbm.ReferenceFrame.from_dataset(\n",
" left_eye,\n",
" \"position\",\n",
" \"orientation\",\n",
" \"time\",\n",
" parent=\"pupil/scene_cam\",\n",
" name=\"pupil/left_eye\",\n",
").register()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The eye tracker data uses yet another coordinate convention (x right, y down, z forward) which we need to take into account when visualizing the eye frame:"
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {},
"outputs": [],
"source": [
"R_PUPIL_ROS = np.array([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]])\n",
"\n",
"rbm.ReferenceFrame.from_rotation_matrix(\n",
" R_PUPIL_ROS, parent=\"pupil/left_eye\", name=\"left_eye\"\n",
").register(update=True)"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"world\n",
"└── t265/world\n",
" └── t265/tracker\n",
" ├── head\n",
" └── pupil/scene_cam\n",
" └── pupil/left_eye\n",
" └── left_eye\n"
]
}
],
"source": [
"rbm.render_tree(\"world\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we create another broadcaster for the transform between head and eye frame. With `publish_pose=True`, the broadcaster also publishes a `geometry_msgs/PoseStamped` message on the `//pose` topic. This message can be visualized in RViz as an arrow which avoids cluttering for frames that are close together. In order to publish messages in sync with the world/head transform, we specify `subscribe=\"head\"`. This way, the broadcaster checks for new messages on the `/tf` topic where the `child_frame_id` is `\"head\"` and publishes its own transform with the closest timestamp:"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [],
"source": [
"tf_head_left_eye = rbm.ros.ReferenceFrameTransformBroadcaster(\n",
" \"left_eye\", base=\"head\", publish_pose=True, subscribe=\"head\"\n",
")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Calling the [spin](_generated/rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.rst#rigid_body_motion.ros.ReferenceFrameTransformBroadcaster.spin) method dispatches the broadcaster to a separate thread where it will keep checking for new world/head transforms:"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [],
"source": [
"tf_head_left_eye.spin()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, the [play_publisher](_generated/rigid_body_motion.ros.play_publisher.rst) method provides a notebook widget to play back data from a broadcaster. With `step=2` it broadcasts every second transform and with `speed=0.5` the data is played back at half the recorded speed:"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "7d04e1102a2c45349cbd67dccf715495",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"HBox(children=(IntSlider(value=0, description='Index', max=66628), Button(description='◄◄', layout=Layout(widt…"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "1ecb6c4a53a342dd9bcd4385e7c43507",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"Output()"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"rbm.ros.play_publisher(tf_world_head, step=2, speed=0.5)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"After pressing the play button, you should see the head frame moving along the white path, with the eye-in-head pose drawn as a yellow arrow:\n",
"\n",
"![](\"_static/img/rviz_2.png\")
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, if you have started the ROS master from Python, you should shut it down at the end:"
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[rosout-1] killing on exit\n",
"[master] killing on exit\n"
]
}
],
"source": [
"master.shutdown()"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python [conda env:rbm]",
"language": "python",
"name": "conda-env-rbm-py"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.10"
},
"nbsphinx": {
"execute": "never"
}
},
"nbformat": 4,
"nbformat_minor": 4
}