Support eye

jsk_spot_robot/jsk_spot_startup/apps/app.installed

@@ -3,4 +3,5 @@ apps:
   dispay: Hello World
 - app: jsk_spot_startup/head_lead_demo
   dispay: Head Lead Demo
-
+- app: jsk_spot_startup/sample_eye_rotation
+  dispay: Sample Eye Rotation

jsk_spot_robot/jsk_spot_startup/apps/sample_eye_rotation/sample-eye-rotation.l

@@ -0,0 +1,31 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "geometry_msgs")
+(load "package://spoteus/spot-interface.l")
+(spot-init)
+
+(setq *pi* 3.14159265358979)
+
+(setq *right-eye-topic* "/righteye_driver/look_at")
+(setq *left-eye-topic* "/lefteye_driver/look_at")
+
+(setq *right-eye-target* (instance geometry_msgs::Point :init))
+(setq *left-eye-target* (instance geometry_msgs::Point :init))
+
+(ros::advertise *right-eye-topic* geometry_msgs::Point 1)
+(ros::advertise *left-eye-topic* geometry_msgs::Point 1)
+
+(setq index 0)
+(ros::rate 10)
+(while (ros::ok)
+       (ros::sleep)
+       (send *right-eye-target* :x (* -1 (cos (/ (* 2 *pi* index) 10))))
+       (send *right-eye-target* :y (* 1 (sin (/ (* 2 *pi* index) 10))))
+       (send *left-eye-target* :x (* 1 (cos (/ (* 2 *pi* index) 10))))
+       (send *left-eye-target* :y (* 1 (sin (/ (* 2 *pi* index) 10))))
+       (ros::publish *right-eye-topic* *right-eye-target*)
+       (ros::publish *left-eye-topic* *left-eye-target*)
+       (ros::ros-info "publish to right ~A" *right-eye-target*)
+       (ros::ros-info "publish to right ~A" *left-eye-target*)
+       (setq index (+ 1 index))
+       )

jsk_spot_robot/jsk_spot_startup/apps/sample_eye_rotation/sample_eye_rotation.app

@@ -0,0 +1,4 @@
+display: Sample Eye Rotation
+platform: spot
+launch: jsk_spot_startup/sample_eye_rotation.xml
+interface: jsk_spot_startup/sample_eye_rotation.interface

jsk_spot_robot/jsk_spot_startup/apps/sample_eye_rotation/sample_eye_rotation.interface

@@ -0,0 +1,3 @@
+published_topics: {}
+subscribed_topics: {}
+

jsk_spot_robot/jsk_spot_startup/apps/sample_eye_rotation/sample_eye_rotation.xml

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="jsk_spot_startup" type="sample-eye-rotation.l" name="sample_eye_rotation"/>
+</launch>

jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch

@@ -1,2 +1,21 @@
 <launch>
+    <arg name="port_righteye" default="/dev/eye_left"/>
+    <arg name="port_lefteye" default="/dev/eye_right"/>
+
+    <node pkg="rosserial_python" type="serial_node.py" name="righteye_driver">
+        <rosparam subst_value="true">
+            port: $(arg port_righteye)
+            baud: 57600
+            mode_right: true
+        </rosparam>
+    </node>
+
+    <node pkg="rosserial_python" type="serial_node.py" name="lefteye_driver">
+        <rosparam subst_value="true">
+            port: $(arg port_lefteye)
+            baud: 57600
+            mode_right: false
+        </rosparam>
+    </node>
+
 </launch>

jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch

@@ -1,7 +1,7 @@
 <launch>
   <arg name="credential_config" default="/var/lib/robot/credentials/spot_credential.yaml" />
   <arg name="launch_robot_state_publisher" default="true" />
-  <arg name="launch_peripheral" default="false" />
+  <arg name="launch_peripheral" default="true" />
   <arg name="launch_interaction" default="false" />
 
   <arg name="use_driver" default="true" />

jsk_spot_robot/jsk_spot_startup/scripts/install_udev_rules.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+sudo cp $(rospack find jsk_spot_startup)/udev_rules/* /etc/udev/rules.d/
+sudo udevadm control --reload-rules

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/.gitignore

@@ -0,0 +1 @@
+.pio

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/README.md

@@ -0,0 +1,13 @@
+# eye_display
+
+Install PlatformIO first.
+
+Use https://www.switch-science.com/products/8098
+
+## Build and burn
+
+```bash
+roscd jsk_spot_startup/sketchbooks/eye-display
+pio run --target uploadfs --upload-port <port to display>
+pio run --target upload --upload-port <port to display>
+```

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/lib/.gitignore

@@ -0,0 +1 @@
+ros_lib

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/platformio.ini

@@ -0,0 +1,36 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:default]
+platform = 
+	https://github.com/platformio/platform-espressif32.git
+platform_packages = 
+	framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#2.0.7
+framework = arduino
+board = esp32dev
+board_build.mcu = esp32c3
+board_build.partitions = huge_app.csv
+board_build.variant = esp32c3
+board_build.f_cpu = 160000000L
+board_build.f_flash = 80000000L
+board_build.flash_mode = dio
+board_build.arduino.ldscript = esp32c3_out.ld
+board_build.arduino.upstream_packages = no
+build_unflags = 
+	-DARDUINO_ESP32_DEV
+	-DARDUINO_VARIANT="esp32"
+build_flags = 
+	-DARDUINO_ESP32C3_DEV
+	-DCORE_DEBUG_LEVEL=2
+	-DARDUINO_VARIANT="esp32c3"
+monitor_speed = 115200
+monitor_filters = time
+lib_deps = 
+	lovyan03/LovyanGFX @ ^0.5.0

jsk_spot_robot/jsk_spot_startup/sketchbooks/eye-display/src/eye.hpp

@@ -0,0 +1,140 @@
+#include <Arduino.h>
+#include <SPIFFS.h>
+#include <math.h>
+
+#define LGFX_USE_V1
+#include <LovyanGFX.hpp>
+
+class LGFX_M5Stamp_SPI_GC9A01 : public lgfx::LGFX_Device
+{
+  lgfx::Panel_GC9A01 _panel_instance;
+  lgfx::Bus_SPI _bus_instance;  // SPIバスのインスタンス
+  lgfx::Light_PWM _light_instance;
+
+public:
+  LGFX_M5Stamp_SPI_GC9A01(void)
+  {
+    {                                     // バス制御の設定を行います。
+      auto cfg = _bus_instance.config();  // バス設定用の構造体を取得します。
+
+      // SPIバスの設定
+      cfg.spi_host =
+          SPI2_HOST;  // 使用するSPIを選択  ESP32-S2,C3 : SPI2_HOST or SPI3_HOST / ESP32 : VSPI_HOST or HSPI_HOST
+      // ※ ESP-IDFバージョンアップに伴い、VSPI_HOST ,
+      // HSPI_HOSTの記述は非推奨になるため、エラーが出る場合は代わりにSPI2_HOST , SPI3_HOSTを使用してください。
+      cfg.spi_mode = 0;  // SPI通信モードを設定 (0 ~ 3)
+      cfg.freq_write = 40000000;  // 送信時のSPIクロック (最大80MHz, 80MHzを整数で割った値に丸められます)
+      cfg.freq_read = 16000000;  // 受信時のSPIクロック
+      cfg.spi_3wire = true;      // 受信をMOSIピンで行う場合はtrueを設定
+      cfg.use_lock = true;       // トランザクションロックを使用する場合はtrueを設定
+      cfg.dma_channel =
+          SPI_DMA_CH_AUTO;  // 使用するDMAチャンネルを設定 (0=DMA不使用 / 1=1ch / 2=ch / SPI_DMA_CH_AUTO=自動設定)
+      // ※
+      // ESP-IDFバージョンアップに伴い、DMAチャンネルはSPI_DMA_CH_AUTO(自動設定)が推奨になりました。1ch,2chの指定は非推奨になります。
+      cfg.pin_sclk = 4;   // SPIのSCLKピン番号を設定
+      cfg.pin_mosi = 6;   // SPIのMOSIピン番号を設定
+      cfg.pin_miso = -1;  // SPIのMISOピン番号を設定 (-1 = disable)
+      cfg.pin_dc = 1;     // SPIのD/Cピン番号を設定  (-1 = disable)
+      // SDカードと共通のSPIバスを使う場合、MISOは省略せず必ず設定してください。
+      _bus_instance.config(cfg);               // 設定値をバスに反映します。
+      _panel_instance.setBus(&_bus_instance);  // バスをパネルにセットします。
+    }
+
+    {                                       // 表示パネル制御の設定を行います。
+      auto cfg = _panel_instance.config();  // 表示パネル設定用の構造体を取得します。
+      cfg.pin_cs = 7;                       // CSが接続されているピン番号   (-1 = disable)
+      cfg.pin_rst = 0;                      // RSTが接続されているピン番号  (-1 = disable)
+      cfg.pin_busy = -1;                    // BUSYが接続されているピン番号 (-1 = disable)
+      // ※
+      // 以下の設定値はパネル毎に一般的な初期値が設定されていますので、不明な項目はコメントアウトして試してみてください。
+      cfg.memory_width = 240;    // ドライバICがサポートしている最大の幅
+      cfg.memory_height = 240;   // ドライバICがサポートしている最大の高さ
+      cfg.panel_width = 240;     // 実際に表示可能な幅
+      cfg.panel_height = 240;    // 実際に表示可能な高さ
+      cfg.offset_x = 0;          // パネルのX方向オフセット量
+      cfg.offset_y = 0;          // パネルのY方向オフセット量
+      cfg.offset_rotation = 0;   // 回転方向の値のオフセット 0~7 (4~7は上下反転)
+      cfg.dummy_read_pixel = 8;  // ピクセル読出し前のダミーリードのビット数
+      cfg.dummy_read_bits = 1;   // ピクセル以外のデータ読出し前のダミーリードのビット数
+      cfg.readable = true;       // データ読出しが可能な場合 trueに設定
+      cfg.invert = true;         // パネルの明暗が反転してしまう場合 trueに設定
+      cfg.rgb_order = false;     // パネルの赤と青が入れ替わってしまう場合 trueに設定
+      cfg.dlen_16bit = false;    // データ長を16bit単位で送信するパネルの場合 trueに設定
+      cfg.bus_shared = true;  // SDカードとバスを共有している場合 trueに設定(drawJpgFile等でバス制御を行います)
+
+      _panel_instance.config(cfg);
+    }
+    {  // バックライト制御の設定を行います。（必要なければ削除）
+      auto cfg = _light_instance.config();  // バックライト設定用の構造体を取得します。
+
+      cfg.pin_bl = 10;      // バックライトが接続されているピン番号
+      cfg.invert = false;   // バックライトの輝度を反転させる場合 true
+      cfg.freq = 44100;     // バックライトのPWM周波数
+      cfg.pwm_channel = 3;  // 使用するPWMのチャンネル番号
+
+      _light_instance.config(cfg);
+      _panel_instance.setLight(&_light_instance);  // バックライトをパネルにセットします。
+    }
+    setPanel(&_panel_instance);  // 使用するパネルをセットします。
+  }
+};
+
+class Eye
+{
+private:
+  LGFX_M5Stamp_SPI_GC9A01 lcd;
+  LGFX_Sprite sprite_eye;
+  LGFX_Sprite sprite_outline;
+  LGFX_Sprite sprite_pupil;
+  LGFX_Sprite sprite_reflex;
+
+  float zoom_ratio;
+
+  int image_width;
+  int image_height;
+
+public:
+  void init(const char* path_jpg_outline, const char* path_jpg_pupil, const char* path_jpg_reflex,
+            const int image_width, const int image_height, int rotation = 0)
+  {
+    this->image_width = image_width;
+    this->image_height = image_height;
+
+    lcd.init();
+    lcd.setRotation(rotation);
+
+    sprite_eye.createSprite(image_width, image_height);
+    sprite_eye.fillScreen(TFT_WHITE);
+
+    sprite_outline.createSprite(image_width, image_height);
+    sprite_outline.fillScreen(TFT_WHITE);
+    sprite_outline.drawJpgFile(SPIFFS, path_jpg_outline);
+    sprite_pupil.createSprite(image_width, image_height);
+    sprite_pupil.fillScreen(TFT_WHITE);
+    sprite_pupil.drawJpgFile(SPIFFS, path_jpg_pupil);
+    sprite_reflex.createSprite(image_width, image_height);
+    sprite_reflex.fillScreen(TFT_WHITE);
+    sprite_reflex.drawJpgFile(SPIFFS, path_jpg_reflex);
+
+    zoom_ratio = (float)lcd.width() / image_width;
+    float ztmp = (float)lcd.height() / image_height;
+    if (zoom_ratio > ztmp)
+    {
+      zoom_ratio = ztmp;
+    }
+
+    lcd.setPivot(lcd.width() >> 1, lcd.height() >> 1);
+    lcd.fillScreen(TFT_WHITE);
+  }
+
+  void update_look(float dx = 0.0, float dy = 0.0, float scale = 10.0, float random_scale = 5.0)
+  {
+    long rx = (int)(random_scale * random(100) / 100);
+    long ry = (int)(random_scale * random(100) / 100);
+    sprite_eye.fillScreen(TFT_WHITE);
+    sprite_outline.pushSprite(&sprite_eye, 0, 0, TFT_WHITE);
+    sprite_pupil.pushSprite(&sprite_eye, (int)(scale * dx), (int)(scale * dy), TFT_WHITE);
+    sprite_reflex.pushSprite(&sprite_eye, (int)(scale * dx) + rx, (int)(scale * dy) + ry, TFT_WHITE);
+    sprite_eye.pushRotateZoom(&lcd, lcd.width() >> 1, lcd.height() >> 1, 0, zoom_ratio, zoom_ratio, TFT_WHITE);
+  }
+};