state estimation dataset collected

.devcontainer/Dockerfile

@@ -1,4 +1,4 @@
-FROM ros:foxy
+FROM ros:humble
 ENV DEBIAN_FRONTEND=noninteractive
 SHELL ["/bin/bash", "-c"]
 RUN apt-get update && apt-get install -y -qq --no-install-recommends \ 
@@ -11,9 +11,9 @@ RUN apt-get update && apt-get install -y -qq --no-install-recommends \
                                       freeglut3-dev \
                                       git \
                                       python3-pip \ 
-                                      ros-foxy-rmw-cyclonedds-cpp ros-foxy-rosidl-generator-dds-idl \
+                                      ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
                                       libyaml-cpp-dev \
-                                      ros-foxy-xacro \
+                                      ros-humble-xacro \
                                       libboost-all-dev\
                                       build-essential \
                                       cmake \
@@ -23,8 +23,8 @@ RUN apt-get update && apt-get install -y -qq --no-install-recommends \
 RUN pip3 install mujoco pin matplotlib
 
 RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
-git clone https://github.com/ros2/rmw_cyclonedds -b foxy && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
-cd .. && colcon build --packages-select cyclonedds && source /opt/ros/foxy/setup.bash && colcon build
+git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
+cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
 
 # Env vars for the nvidia-container-runtime.
 ENV NVIDIA_VISIBLE_DEVICES all

examples/highlevel_ros2_interface.ipynb

@@ -47,7 +47,7 @@
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
+      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n",
       "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
      ]
     }
@@ -56,7 +56,7 @@
     "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
     "import time\n",
     "ros2_init()\n",
-    "robot = GO2Real(mode='lowlevel')\n",
+    "robot = GO2Real(mode='highlevel')\n",
     "ros2_exec_manager = ROS2ExecutorManager()\n",
     "ros2_exec_manager.add_node(robot)\n",
     "ros2_exec_manager.start()"
@@ -89,24 +89,46 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 17,
+   "execution_count": 50,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "{'stamp_nanosec': 1707447716.9414463,\n",
-       " 'position': array([0.01655228, 0.00701126, 0.30197507]),\n",
-       " 'orientation': array([-0.00573182,  0.0074235 ,  0.08994701,  0.99590236])}"
-      ]
-     },
-     "execution_count": 17,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
-    "robot.getOdometry()"
+    "odometry = []\n",
+    "imus = []\n",
+    "joints = []\n",
+    "foot_contacts = []\n",
+    "stamps = []\n",
+    "\n",
+    "for i in range(60*200):\n",
+    "    time.sleep(1/200)\n",
+    "    odom = robot.getOdometry()\n",
+    "    contact = robot.getFootContacts()\n",
+    "    joint = robot.getJointStates()\n",
+    "    imu = robot.getIMU()\n",
+    "    imus.append(imu)\n",
+    "    joints.append(joint)\n",
+    "    foot_contacts.append(contact)\n",
+    "    stamps.append(time.time())\n",
+    "    odometry.append(odom)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 51,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import pickle\n",
+    "with open('data/april16-seq2.pkl', 'wb') as f:\n",
+    "    pickle.dump(\n",
+    "        {\n",
+    "            'imu':imus,\n",
+    "            'odometry':odometry,\n",
+    "            'joints':joints,\n",
+    "            'foot_contacts':foot_contacts,\n",
+    "            'stamps':stamps\n",
+    "        }, f\n",
+    "    )"
    ]
   }
  ],
@@ -126,7 +148,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.18"
+   "version": "3.8.10"
   }
  },
  "nbformat": 4,