main commit

ctrl_hw/__init__.py

@@ -0,0 +1 @@
+from .racecar import Racecar

ctrl_hw/init_car.py

@@ -0,0 +1,39 @@
+from .nvidia_racecar import NvidiaRacecar
+import traitlets
+
+car = NvidiaRacecar()
+
+def check_config(vehicle):
+    print("i2c_address: ", veh.i2c_address)
+    print("steering_channel: ", veh.steering_channel)
+    print("throttle_channel: ", veh.throttle_channel)
+
+def performance_steering(vehicle):
+    print("Check steering left-most to right-most")
+    for i in [float(i)/20 for i in range(-20,21)]:
+        vehicle.steering(i)
+
+def performance_throttle(vehicle):
+    print("Check throttle backrward to forward")
+    for i in [float(i)/20 for i in range(-20,21)]:
+        vehicle.throttle(i)
+
+# def calibraion_steering_manual(vehicle):
+# def calibration_steering_with_imu(vehicle):    
+# def calibration_steering_with_camera(vehicle):
+# def adjust_throttle_manual(vehicle):
+# def adjust_throttle_with_imu(vehicle):
+
+def print_steering(vehicle):
+    print("Steering :" + vehicle.steering()*100 + "%")
+
+def print_throttle(vehicle):
+    print("Throttle :" + vehicle.throttle()*100 + "%")
+
+def validate_car(vehicle):
+
+
+if __name__ == "__main__":
+    print("None Yet")
+
+

ctrl_hw/nvidia_racecar.py

@@ -0,0 +1,27 @@
+from .racecar import Racecar
+import traitlets
+from adafruit_servokit import ServoKit
+
+
+class NvidiaRacecar(Racecar):
+
+    i2c_address = traitlets.Integer(default_value=0x40)
+    steering_gain = traitlets.Float(default_value=-0.65)
+    steering_offset = traitlets.Float(default_value=0)
+    steering_channel = traitlets.Integer(default_value=0)
+    throttle_gain = traitlets.Float(default_value=0.8)
+    throttle_channel = traitlets.Integer(default_value=1)
+
+    def __init__(self, *args, **kwargs):
+        super(NvidiaRacecar, self).__init__(*args, **kwargs)
+        self.kit = ServoKit(channels=16, address=self.i2c_address)
+        self.steering_motor = self.kit.continuous_servo[self.steering_channel]
+        self.throttle_motor = self.kit.continuous_servo[self.throttle_channel]
+
+    @traitlets.observe('steering')
+    def _on_steering(self, change):
+        self.steering_motor.throttle = change['new'] * self.steering_gain + self.steering_offset
+
+    @traitlets.observe('throttle')
+    def _on_throttle(self, change):
+        self.throttle_motor.throttle = change['new'] * self.throttle_gain

ctrl_hw/racecar.py

@@ -0,0 +1,24 @@
+import traitlets
+
+
+class Racecar(traitlets.HasTraits):
+    steering = traitlets.Float()
+    throttle = traitlets.Float()
+
+    @traitlets.validate('steering')
+    def _clip_steering(self, proposal):
+        if proposal['value'] > 1.0:
+            return 1.0
+        elif proposal['value'] < -1.0:
+            return -1.0
+        else:
+            return proposal['value']
+
+    @traitlets.validate('throttle')
+    def _clip_throttle(self, proposal):
+        if proposal['value'] > 1.0:
+            return 1.0
+        elif proposal['value'] < -1.0:
+            return -1.0
+        else:
+            return proposal['value']

rs_t265/rs_t265_test.ipynb

@@ -0,0 +1,155 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "id": "17ae2462",
+   "metadata": {},
+   "source": [
+    "# Use IMU and Shutter Camera of T265"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "id": "8487aa1c",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import cv2\n",
+    "import pyrealsense2 as rs\n",
+    "import numpy as np\n",
+    "from math import tan, pi\n",
+    "\n",
+    "def get_extrinsics(src, dst):\n",
+    "    extrinsics = src.get_extrinsics_to(dst)\n",
+    "    R = np.reshape(extrinsics.rotation, [3,3]).T\n",
+    "    T = np.array(extrinsics.translation)\n",
+    "    return (R, T)\n",
+    "\n",
+    "def camera_matrix(intrinsics):\n",
+    "    return np.array([[intrinsics.fx,             0, intrinsics.ppx],\n",
+    "                     [            0, intrinsics.fy, intrinsics.ppy],\n",
+    "                     [            0,             0,              1]])\n",
+    "\n",
+    "def fisheye_distortion(intrinsics):\n",
+    "    return np.array(intrinsics.coeffs[:4])\n",
+    "\n",
+    "# Set up a mutex to share data between threads \n",
+    "from threading import Lock\n",
+    "frame_mutex = Lock()\n",
+    "frame_data = {\"left\"  : None,\n",
+    "              \"right\" : None,\n",
+    "              \"timestamp_ms\" : None\n",
+    "              }\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "id": "0d5649cf",
+   "metadata": {},
+   "source": [
+    "#it's hard to run callback function fully \n",
+    "\n",
+    "In this step, please connect t265."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "id": "5ee187cd",
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "RuntimeError",
+     "evalue": "No device connected",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mRuntimeError\u001b[0m                              Traceback (most recent call last)",
+      "\u001b[0;32m<ipython-input-2-21c0dbb5b464>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     21\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     22\u001b[0m \u001b[0;31m# Start streaming with our callback\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 23\u001b[0;31m \u001b[0mpipe\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstart\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcfg\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcallback\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
+      "\u001b[0;31mRuntimeError\u001b[0m: No device connected"
+     ]
+    }
+   ],
+   "source": [
+    "def callback(frame):\n",
+    "    global frame_data\n",
+    "    if frame.is_frameset():\n",
+    "        frameset = frame.as_frameset()\n",
+    "        f1 = frameset.get_fisheye_frame(1).as_video_frame()\n",
+    "        f2 = frameset.get_fisheye_frame(2).as_video_frame()\n",
+    "        left_data = np.asanyarray(f1.get_data())\n",
+    "        right_data = np.asanyarray(f2.get_data())\n",
+    "        ts = frameset.get_timestamp()\n",
+    "        frame_mutex.acquire()\n",
+    "        frame_data[\"left\"] = left_data\n",
+    "        frame_data[\"right\"] = right_data\n",
+    "        frame_data[\"timestamp_ms\"] = ts\n",
+    "        frame_mutex.release()\n",
+    "\n",
+    "# Declare RealSense pipeline, encapsulating the actual device and sensors\n",
+    "pipe = rs.pipeline()\n",
+    "\n",
+    "# Build config object and stream everything\n",
+    "cfg = rs.config()\n",
+    "\n",
+    "# Start streaming with our callback\n",
+    "pipe.start(cfg, callback)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "c1e8ac4f",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# check the frame_data value \n",
+    "print(frame_data[\"left\"])\n",
+    "print(frame_data[\"right\"])\n",
+    "print(frame_data[\"timestamp_ms\"])"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "id": "916fccd1",
+   "metadata": {},
+   "source": [
+    "### Make widget for T265"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "3c1a0044",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from jupyter_clickable_image_widget import ClickableImageWidget\n",
+    "\n",
+    "camera_widget = ipywidgets.Image(width)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "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.6.9"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}