diff --git a/eye_display/CMakeLists.txt b/eye_display/CMakeLists.txt index 4b49dbd4b..37ad9dbb6 100644 --- a/eye_display/CMakeLists.txt +++ b/eye_display/CMakeLists.txt @@ -9,4 +9,5 @@ include_directories() catkin_install_python(PROGRAMS node_scripts/build.py node_scripts/demo_move_eye.py node_scripts/pub_eye_status.py + node_scripts/ros_to_i2c.py node_scripts/asset_param_list.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts) diff --git a/eye_display/README.md b/eye_display/README.md index 04475b225..9225db953 100644 --- a/eye_display/README.md +++ b/eye_display/README.md @@ -97,20 +97,85 @@ $rosparam get eye_display/eye_asset/names If you want to control the device through I2C bus, please use following env. -- `stampc3-i2c-right`: Stamp C3 device on right eye -- `stampc3-i2c-left`: Stamp C3 device on left eye -- `stamps3-i2c-right`: Stamp C3 device on right eye -- `stamps3-i2c-left`: Stamp C3 device on left eye +- `stampc3-i2c`: Stamp C3 device +- `stamps3-i2c`: Stamp S3 device ```bash roscd eye_display -pio run -e stampc3-i2c-right -pio run -e stampc3-i2c-right -t uploadfs --upload-port -pio run -e stampc3-i2c-right -t upload --upload-port +pio run -e stampc3-i2c +pio run -e stampc3-i2c -t uploadfs --upload-port +pio run -e stampc3-i2c -t upload --upload-port ``` Then you can control the device with I2C. +```bash +roslaunch eye_display demo.launch use_i2c:=true i2c_device:= i2c_bus:= +``` + +See `node_scripts/ros_to_i2c.py` for control protocol. + +To monitor the serial output in the dual I2C mode. Use the following logger tool. + +```bash +./node_scripts/dual_serial_logger.py /dev/ttyACM0 /dev/ttyACM1 115200 +``` +#### Dual eye mode + +You can start two device with `demo_dual.launch` + +```bash +roslaunch eye_display demo_dual.launch use_i2c:=false port_right:=/dev/ttyACM0 port_left:=/dev/ttyACM1 baud:=115200 debug:=true +``` + +You can control dual eye status with demo scripts +```bash +rosrun eye_display pub_eye_status.py --dual --rate 0.3 --names sleepy surprised happy +``` + +#### extra images + +If you need more than the standard images (outline, iris, pupil, reflex, upperlid), use the extra images. + + +``` + path_extra1: "/krmt_reflex_shine1.png" + extra1_default_pos_x: 75 + extra1_default_pos_y: 75 + extra1_default_theta: 0 + extra1_position_x: [ 0, 20, 40, 20, 0, -20, -40, -20] + extra1_position_y: [ 40, 20, 0, -20, -40, -20, 0, 20] + extra1_rotation_theta: [ 0, 40, 80, 40, 0, -40, -80, -40] + extra1_zoom: [ 1.0, 1.1, 1.2, 1.3, 1.4, 1.2, 1.0, 0.8, 0.7, 0.8, 0.9] + path_extra2: "/krmt_reflex_heart.png" + extra2_position_x: [ 0, 20, 0, -20] + extra2_position_y: [ 0, 10, 0, -10] + extra2_rotation_theta: [ 0, -40, -80, -40, 0, 40, 80, 40] + extra2_zoom: [1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 1.5, 1.0, 0.5] + extra2_default_pos_x: 75 + extra2_default_pos_y: 75 + extra2_default_theta: 0 +``` + +#### Advanced control + +You can control the eye status in more detail through the `eye_status` topics. +Use the following low-level commands. +`` is defined in the names list in the configuration YAML file (e.g., `names: [normal, happy, blink]`). +`` can be one of `iris`, `pupil`, `reflex`, `upperlid`, `extra1`, `extra2`. + +``` +eye_asset_image_path: : : +eye_asset_default_pos_x: : : +eye_asset_default_pos_y: : : +eye_asset_default_theta: : : +eye_asset_default_zoom: : : +eye_asset_position_x: : : +eye_asset_position_y: : : +eye_asset_rotation_theta: : : +eye_asset_zoom: : : +``` + ### Description of direction ![eye_display_direction](./doc/eye_display_direction.svg) @@ -140,6 +205,8 @@ rosrun eye_display update_ros_lib.sh This feature requires git 2.27+, so if you use Ubuntu<=20.04, please install latest version of git https://git-scm.com/downloads/linux ``` -$ git clone --filter=blob:none --sparse https://github.com/jsk-ros-pkg/jsk_3rdparty.git -b eye_display -$ git sparse-checkout set eye_display +git clone --filter=blob:none --sparse https://github.com/jsk-ros-pkg/jsk_3rdparty.git +cd jsk_3rdparty +git sparse-checkout set eye_display +git checkout eye_display ``` diff --git a/eye_display/include/eye.hpp b/eye_display/include/eye.hpp index 78963e41c..36f7c110a 100644 --- a/eye_display/include/eye.hpp +++ b/eye_display/include/eye.hpp @@ -5,10 +5,21 @@ #include #include #include +#include #include #include +#include "util.h" + +// should be implemented in {ros,i2c}_lib.h +extern void logdebug(const char *fmt, ...); +extern void loginfo(const char *fmt, ...); +extern void logwarn(const char *fmt, ...); +extern void logerror(const char *fmt, ...); +extern void logfatal(const char *fmt, ...); + +constexpr int EXTRA_EYE_ASSET_SIZE=3; struct EyeAsset { std::string name = "default"; std::string path_outline = "/outline.jpg"; // static @@ -16,14 +27,32 @@ struct EyeAsset { std::string path_pupil = "/pupil.jpg"; // move along with iris std::string path_reflex = "/reflex.jpg" ; // move along with puppil + random motion std::string path_upperlid = "/upperlid.jpg"; // use upperlid_position_map to set y-axis motoin - std::vector upperlid_position = {0}; // upperlid = motion layer + std::vector iris_zoom = {}; // eyeball (iris, pupil, reflex) + std::vector upperlid_position_x = {0}; // upperlid = motion layer + std::vector upperlid_position_y = {9}; // upperlid = motion layer + std::vector upperlid_rotation_theta = {0}; // upperlid = motion layer + std::vector upperlid_zoom = {}; // upperlid = motion layer + std::vector path_extra = {}; + std::vector> extra_position_x = {}; + std::vector> extra_position_y = {}; + std::vector> extra_rotation_theta = {}; + std::vector> extra_zoom = {}; int direction = 0; bool invert_rl = false; + float iris_default_pos_x = 0.0f; + float iris_default_pos_y = 0.2f; + float iris_default_theta = 0.0f; + float iris_default_zoom = 1.0f; int upperlid_pivot_x = 75; int upperlid_pivot_y = 139; int upperlid_default_pos_x = 75; int upperlid_default_pos_y = 7; int upperlid_default_theta = 0; + float upperlid_default_zoom = 1.0f; + std::vector extra_default_pos_x = {}; + std::vector extra_default_pos_y = {}; + std::vector extra_default_theta = {}; + std::vector extra_default_zoom = {}; }; #if defined(STAMPS3) @@ -50,24 +79,26 @@ class EyeManager LGFX_Sprite sprite_pupil; LGFX_Sprite sprite_reflex; LGFX_Sprite sprite_upperlid; + std::vector sprite_extra; + + // useing + float zoom_outline = 2.0f; + float zoom_iris = 2.0f; + float zoom_pupil = 1.0f; + float zoom_reflex = 2.0f; + float zoom_upperlid = 1.0f; + float zoom_extra = 1.0f; float zoom_ratio; int image_width = 139; int image_height = 139; - float look_x = 0.0f; - float look_y = 0.2f; - + unsigned long interval_time = 150; // this will reproduce delay(100) + unsigned long next_time = millis() + interval_time; int frame = 0; - virtual void logdebug(const char *fmt, ...) = 0; - virtual void loginfo(const char *fmt, ...) = 0; - virtual void logwarn(const char *fmt, ...) = 0; - virtual void logerror(const char *fmt, ...) = 0; - virtual void logfatal(const char *fmt, ...) = 0; - void load_eye_images(); - bool draw_image_file(LGFX_Sprite& sprite, const char* filePath); + bool draw_image_file(LGFX_Sprite& sprite, const char* filePath, float zoom); std::string current_eye_asset_name; public: @@ -76,13 +107,17 @@ class EyeManager EyeManager(); void init(const int image_width, const int image_height); - void set_gaze_direction(float look_x, float look_y); + int setup_asset(std::string eye_asset_text); + void set_gaze_direction(float look_x, float look_y, float look_theta); void set_emotion(const std::string eye_status_name); std::string get_emotion(); int update_emotion(); - void update_look(float dx, float dy, - int dx_upperlid, int dy_upperlid, float dtheta_upperlid, + void update_look(float dx, float dy, float dtheta, float dzoom, + int dx_upperlid, int dy_upperlid, float dtheta_upperlid, float dzoom_upperlid, + std::vector dx_extra, std::vector dy_extra, std::vector dtheta_extra, std::vector dzoom_extra, float random_scale); + unsigned long update_next_time(); + long delay_next_time(); }; @@ -103,33 +138,45 @@ void EyeManager::init( lcd.init(); lcd.setRotation(current_eye_asset.direction); + // Due to the limited RAM on StampC3, sprites are decoded at a lower resolution to optimize memory usage. +#ifdef STAMPC3 + zoom_outline = 2.0f; + zoom_iris = 2.0f; + zoom_pupil = 1.0f; + zoom_reflex = 2.0f; + zoom_upperlid = 1.0f; + zoom_extra = 1.0f; +#endif // 目全体を描写するBufferとしてのSpriteを準備 sprite_eye.createSprite(image_width, image_height); sprite_eye.fillScreen(TFT_WHITE); // 目の輪郭を描写するSpriteを準備 - sprite_outline.createSprite(image_width, image_height); - if (current_eye_asset.invert_rl) sprite_outline.setRotation(6); + sprite_outline.createSprite(image_width/zoom_outline, image_height/zoom_outline); // 虹彩を描写するSpriteを準備 - sprite_iris.createSprite(image_width, image_height); - if (current_eye_asset.invert_rl) sprite_iris.setRotation(6); + sprite_iris.createSprite(image_width/zoom_iris, image_height/zoom_iris); // 瞳孔を描写するSpriteを準備 - sprite_pupil.createSprite(image_width, image_height); - if (current_eye_asset.invert_rl) sprite_pupil.setRotation(6); + sprite_pupil.createSprite(image_width/zoom_pupil, image_height/zoom_pupil); // 光の反射を描画するSpriteを準備 - sprite_reflex.createSprite(image_width, image_height); - if (current_eye_asset.invert_rl) sprite_reflex.setRotation(6); + sprite_reflex.createSprite(image_width/zoom_reflex, image_height/zoom_reflex); // 上瞼を描写するSpriteを準備 - sprite_upperlid.createSprite(image_width, image_height); - if (current_eye_asset.invert_rl) sprite_upperlid.setRotation(6); - sprite_upperlid.setPivot(current_eye_asset.upperlid_pivot_x, current_eye_asset.upperlid_pivot_y); + sprite_upperlid.createSprite(image_width/zoom_upperlid, image_height/zoom_upperlid); + sprite_upperlid.setPivot(current_eye_asset.upperlid_pivot_x/zoom_upperlid, current_eye_asset.upperlid_pivot_y/zoom_upperlid); - // Load images from default EyeAsset - //////////////////////////set_emotion(current_eye_asset); + // その他描写するSpriteを準備 + sprite_extra.resize(EXTRA_EYE_ASSET_SIZE); + // do not createSprite() here; it causes PNG loading errors + // Instead, allocate and free sprite memory within update_look() + + if (psramFound()) { + logdebug("[%8ld] PSRAM available, using for PNG decode", millis()); + } else { + logwarn("[%8ld] PSRAM NOT available, PNG decode may fail", millis()); + } // lcdを準備 lcd.setPivot(lcd.width() >> 1, lcd.height() >> 1); @@ -147,33 +194,41 @@ void EyeManager::init( set_emotion(current_eye_asset.name); } -bool EyeManager::draw_image_file(LGFX_Sprite& sprite, const char* filePath) +bool EyeManager::draw_image_file(LGFX_Sprite& sprite, const char* filePath, float zoom = 1.0) { std::string pathStr(filePath); std::string extension = pathStr.substr(pathStr.find_last_of('.') + 1); bool ret = false; if (extension == "jpg" || extension == "jpeg") { - logdebug("loading jpeg: %s", filePath); - ret = sprite.drawJpgFile(SPIFFS, filePath); + logdebug("[%8ld] loading jpg: %s (zoom:%.2f)", millis(), filePath, zoom); + if (zoom == 1) { + return sprite.drawJpgFile(SPIFFS, filePath); + } + ret = sprite.drawJpgFile(SPIFFS, filePath, 0, 0, 0, 0, 0, 0, 1.0/zoom, 1.0/zoom); } else if (extension == "png") { - logdebug("loading png: %s", filePath); - ret = sprite.drawPngFile(SPIFFS, filePath); + logdebug("[%8ld] loading png: %s (zoom:%.2f)", millis(), filePath, zoom); + if (zoom == 1) { + return sprite.drawPngFile(SPIFFS, filePath); + } + ret = sprite.drawPngFile(SPIFFS, filePath, 0, 0, 0, 0, 0, 0, 1.0/zoom, 1.0/zoom); } else { - logerror("invalid image extension %s", filePath); + logerror("[%8ld] invalid image extension %s", millis(), filePath); } if (not ret) { - logerror("Failed to load %s", filePath); + logerror("[%8ld] Failed to load %s", millis(), filePath); } return ret; } // 視線方向を変更(値を設定するだけ) -void EyeManager::set_gaze_direction(float look_x, float look_y) +void EyeManager::set_gaze_direction(float look_x, float look_y, float look_theta) { - this->look_x = look_x; - this->look_y = look_y; - loginfo("Look at (%.1f, %.1f)", look_x, look_y); + EyeAsset& current_eye_asset = eye_asset_map[current_eye_asset_name]; + current_eye_asset.iris_default_pos_x = look_x; + current_eye_asset.iris_default_pos_y = look_y; + current_eye_asset.iris_default_theta = look_theta; + loginfo("[%8ld] Look at (%.1f, %.1f, %.1f)", millis(), look_x, look_y, look_theta); } // 目の状態を更新する @@ -185,85 +240,171 @@ void EyeManager::load_eye_images() const char *path_jpg_pupil = current_eye_asset.path_pupil.c_str(); const char *path_jpg_reflex = current_eye_asset.path_reflex.c_str(); const char *path_jpg_upperlid = current_eye_asset.path_upperlid.c_str(); + logdebug("[%8ld] load_eye_iamges: %s (%s)", millis(), current_eye_asset_name.c_str(), current_eye_asset.invert_rl?"true":"false"); + logdebug(" outline: %s", (path_jpg_outline==NULL)?"NULL":path_jpg_outline); + logdebug(" iris: %s", (path_jpg_iris==NULL)?"NULL":path_jpg_iris); + logdebug(" pupil: %s", (path_jpg_pupil==NULL)?"NULL":path_jpg_pupil); + logdebug(" reflex: %s", (path_jpg_reflex==NULL)?"NULL":path_jpg_reflex); + logdebug(" upperlid: %s", (path_jpg_upperlid==NULL)?"NULL":path_jpg_upperlid); lcd.setRotation(current_eye_asset.direction); if (path_jpg_outline != NULL) { + if (current_eye_asset.invert_rl) sprite_outline.setRotation(6); sprite_outline.fillScreen(TFT_WHITE); - if (not draw_image_file(sprite_outline, path_jpg_outline)) { + if (not draw_image_file(sprite_outline, path_jpg_outline, zoom_outline)) { sprite_outline.fillScreen(TFT_WHITE); } } if (path_jpg_iris != NULL) { + if (current_eye_asset.invert_rl) sprite_iris.setRotation(6); sprite_iris.fillScreen(TFT_WHITE); - if (not draw_image_file(sprite_iris, path_jpg_iris)) { + if (not draw_image_file(sprite_iris, path_jpg_iris, zoom_iris)) { sprite_iris.fillScreen(TFT_WHITE); } } if (path_jpg_pupil != NULL) { + if (current_eye_asset.invert_rl) sprite_pupil.setRotation(6); sprite_pupil.fillScreen(TFT_WHITE); - if (not draw_image_file(sprite_pupil, path_jpg_pupil)) { + if (not draw_image_file(sprite_pupil, path_jpg_pupil, zoom_pupil)) { sprite_pupil.fillScreen(TFT_WHITE); } } if (path_jpg_reflex != NULL) { + if (current_eye_asset.invert_rl) sprite_reflex.setRotation(6); sprite_reflex.fillScreen(TFT_WHITE); - if (not draw_image_file(sprite_reflex, path_jpg_reflex)) { + if (not draw_image_file(sprite_reflex, path_jpg_reflex, zoom_reflex)) { sprite_reflex.fillScreen(TFT_WHITE); } } if (path_jpg_upperlid != NULL) { + if (current_eye_asset.invert_rl) sprite_upperlid.setRotation(6); sprite_upperlid.fillScreen(TFT_WHITE); - if (not draw_image_file(sprite_upperlid, path_jpg_upperlid)) { + if (not draw_image_file(sprite_upperlid, path_jpg_upperlid, zoom_upperlid)) { sprite_upperlid.fillScreen(TFT_WHITE); } } + + // do not draw_image_file() here; it causes PNG loading errors + // Instead, allocate, draw and free sprite memory within update_look() } + // 通常の目の描画 -void EyeManager::update_look(float dx = 0.0, float dy = 0.0, - int dx_upperlid = 0.0, int dy_upperlid = 0.0, float dtheta_upperlid = 0.0, +void EyeManager::update_look(float dx = 0.0f, float dy = 0.0f, float dtheta = 0.0f, float dzoom = 1.0f, + int dx_upperlid = 0.0, int dy_upperlid = 0.0, float dtheta_upperlid = 0.0, float dzoom_upperlid = 1.0, + std::vector dx_extra = {}, std::vector dy_extra = {}, std::vector dtheta_extra = {}, + std::vector dzoom_extra = {}, float random_scale = 5.0) { EyeAsset& current_eye_asset = eye_asset_map[current_eye_asset_name]; - logdebug("[update_look] dx: %.1f, dy: %.1f, dx_upperlid: %d, dy_upperlid: %d, dtheta_upperlid: %d, random_scale: %.1f", dx, dy, dx_upperlid, dy_upperlid, dtheta_upperlid, random_scale); + std::ostringstream oss; + oss << "[" << millis << "] [update_look] " + << std::fixed << std::setprecision(1) // 以下、浮動小数点はすべて%.1f + << "dx: " << dx << ", " + << "dy: " << dy << ", " + << "dtheta: " << dtheta << ", " + << "dzoom: " << dzoom << ", " + << "dx_upperlid: " << dx_upperlid << ", " + << "dy_upperlid: " << dy_upperlid << ", " + << "dtheta_upperlid: " << dtheta_upperlid << ", " + << "dzoom_upperlid: " << dzoom_upperlid << ", " + << "random_scale: " << random_scale; + if ( !dx_extra.empty() ) { oss << ", dx_extra: " << joinVector(dx_extra); } + if ( !dy_extra.empty() ) { oss << ", dy_extra: " << joinVector(dy_extra); } + if ( !dtheta_extra.empty() ) { oss << ", dtheta_extra: " << joinVector(dtheta_extra); } + if ( !dzoom_extra.empty() ) { oss << ", dzoom_extra: " << joinVector(dzoom_extra); } + logdebug(oss.str().c_str()); + if ( dzoom <= 0.0f || dzoom_upperlid <= 0.0f) { + logwarn("zoom parametr must be > 0 (dzoom:%.1f dzoom_upperlid:%.1f", dzoom, dzoom_upperlid); + } long rx = (int)(random_scale * random(100) / 100); long ry = (int)(random_scale * random(100) / 100); sprite_eye.clear(); sprite_eye.fillScreen(TFT_WHITE); - sprite_outline.pushSprite(&sprite_eye, 0, 0, TFT_WHITE); - sprite_iris.pushSprite(&sprite_eye, dx, dy, TFT_WHITE); - sprite_pupil.pushSprite(&sprite_eye, dx, dy, TFT_WHITE); // 瞳孔をランダムに動かす - sprite_reflex.pushSprite(&sprite_eye, dx + rx, dy + ry, TFT_WHITE); // 光の反射をランダムに動かす + if (zoom_outline == 1) + sprite_outline.pushSprite(&sprite_eye, 0, 0, TFT_WHITE); + else + sprite_outline.pushRotateZoom(&sprite_eye, image_width/2, image_height/2, 0.0f, zoom_outline, zoom_outline, TFT_WHITE); + if (zoom_iris == 1 && dzoom == 1.0f && dtheta == 0.0f) + sprite_iris.pushSprite(&sprite_eye, dx, dy, TFT_WHITE); + else + sprite_iris.pushRotateZoomWithAA(&sprite_eye, image_width/2 + dx, image_height/2 + dy, dtheta, zoom_iris*dzoom, zoom_iris*dzoom, TFT_WHITE); + if (zoom_pupil == 1 && dzoom == 1.0f && dtheta == 0.0f) + sprite_pupil.pushSprite(&sprite_eye, dx, dy, TFT_WHITE); + else + sprite_pupil.pushRotateZoomWithAA(&sprite_eye, image_width/2 + dx, image_height/2 + dy, dtheta, zoom_pupil*dzoom, zoom_pupil*dzoom, TFT_WHITE); // 瞳孔をランダムに動かす + if (zoom_reflex == 1 && dzoom == 1.0f && dtheta == 0.0f) + sprite_reflex.pushSprite(&sprite_eye, dx + rx, dy + ry, TFT_WHITE); + else + sprite_reflex.pushRotateZoomWithAA(&sprite_eye, image_width/2 + dx + rx, image_height/2 + dy + ry, dtheta, zoom_reflex*dzoom, zoom_reflex*dzoom, TFT_WHITE); // 光の反射をランダムに動かす sprite_upperlid.pushRotateZoom(&sprite_eye, - current_eye_asset.upperlid_default_pos_x + dx_upperlid, - current_eye_asset.upperlid_default_pos_y + dy_upperlid, - current_eye_asset.upperlid_default_theta + dtheta_upperlid, - 1.0, 1.0, TFT_WHITE); - + current_eye_asset.upperlid_default_pos_x + dx_upperlid, + current_eye_asset.upperlid_default_pos_y + dy_upperlid, + current_eye_asset.upperlid_default_theta + dtheta_upperlid, + zoom_upperlid*dzoom_upperlid, zoom_upperlid, TFT_WHITE); + + if (current_eye_asset.path_extra.size() > 0) { + if ((current_eye_asset.path_extra.size() <= current_eye_asset.extra_default_pos_x.size()) && + (current_eye_asset.path_extra.size() <= current_eye_asset.extra_default_pos_y.size()) && + (current_eye_asset.path_extra.size() <= current_eye_asset.extra_default_theta.size()) && + (current_eye_asset.path_extra.size() == dx_extra.size()) && + (current_eye_asset.path_extra.size() == dy_extra.size()) && + (current_eye_asset.path_extra.size() == dtheta_extra.size()) && + (current_eye_asset.path_extra.size() <= EXTRA_EYE_ASSET_SIZE)) { + dzoom_extra.resize(current_eye_asset.path_extra.size()); + for(int i = 0; i < current_eye_asset.path_extra.size(); i ++ ) { + if (!current_eye_asset.path_extra[i].empty()) { + // allocate, draw and free sprite memory here to ensure enough memory for PNG loading + sprite_extra[i].createSprite(image_width/zoom_extra, image_height/zoom_extra); + if (current_eye_asset.invert_rl) sprite_extra[i].setRotation(6); + const char *path_jpg_extra = current_eye_asset.path_extra[i].c_str(); + sprite_extra[i].fillScreen(TFT_WHITE); + if (not draw_image_file(sprite_extra[i], path_jpg_extra, zoom_extra)) { + sprite_extra[i].fillScreen(TFT_WHITE); + } + sprite_extra[i].pushRotateZoom(&sprite_eye, + current_eye_asset.extra_default_pos_x[i] + dx_extra[i], + current_eye_asset.extra_default_pos_y[i] + dy_extra[i], + current_eye_asset.extra_default_theta[i] + dtheta_extra[i], + dzoom_extra[i], dzoom_extra[i], TFT_WHITE); + sprite_extra[i].deleteSprite(); + } + } + } else { + logerror("size of path_extra is %d, which is inconsistent with default_pos_x: %d, default_pos_y: %d, default_theta: %d, dx_extra: %d, dy_extra: %d, dtheta_extra: %d", + current_eye_asset.path_extra.size(), + current_eye_asset.extra_default_pos_x.size(), current_eye_asset.extra_default_pos_y.size(), current_eye_asset.extra_default_theta.size(), + dx_extra.size(), dy_extra.size(), dtheta_extra.size()); + } + } sprite_eye.pushRotateZoom(&lcd, lcd.width() >> 1, lcd.height() >> 1, 0, zoom_ratio, zoom_ratio); } void EyeManager::set_emotion(const std::string eye_status_name) { + frame = 0; // reset frame cound for synchronize auto it = eye_asset_map.find(eye_status_name); if (it == eye_asset_map.end()) { - logerror("Unknown eye_asset status name %s", eye_status_name.c_str()); - logerror("possible status are"); + logerror("[%8ld] Unknown eye_asset status name %s", millis(), eye_status_name.c_str()); + logerror("[%8ld] possible status are", millis()); for(auto & eye_asset: eye_asset_map) { - logerror("... [%s]", eye_asset.first.c_str()); + logerror("[%8ld] ... [%s]", millis(), eye_asset.first.c_str()); } return; } current_eye_asset_name = it->first; - loginfo("Status updated: %s", it->first.c_str()); load_eye_images(); + + // reset internal clock time, aligned to the nearest multiple of interval_time + next_time = (millis()+interval_time/2)/interval_time * interval_time; + loginfo("[%8ld] Status updated: %s", millis(), it->first.c_str()); } std::string EyeManager::get_emotion() { @@ -272,26 +413,452 @@ std::string EyeManager::get_emotion() { int EyeManager::update_emotion() { EyeAsset& current_eye_asset = eye_asset_map[current_eye_asset_name]; - float upperlid_y; - if (current_eye_asset.upperlid_position.size() > 0) { - upperlid_y = current_eye_asset.upperlid_position[frame % current_eye_asset.upperlid_position.size()]; - }else{ - upperlid_y = 0; + float upperlid_x = 0, upperlid_y = 0, upperlid_theta = 0; + if (current_eye_asset.upperlid_position_x.size() > 0) { + upperlid_x = current_eye_asset.upperlid_position_x[frame % current_eye_asset.upperlid_position_x.size()]; + } + if (current_eye_asset.upperlid_position_y.size() > 0) { + upperlid_y = current_eye_asset.upperlid_position_y[frame % current_eye_asset.upperlid_position_y.size()]; + } + if (current_eye_asset.upperlid_rotation_theta.size() > 0) { + upperlid_theta = current_eye_asset.upperlid_rotation_theta[frame % current_eye_asset.upperlid_rotation_theta.size()]; } - update_look(look_x, look_y, 0, upperlid_y); // dx, dy, dx_upperlid, dy_upperlid, dtheta_upperlid + float upperlid_zoom = 1.0f, look_zoom = 1.0f; + if (current_eye_asset.upperlid_zoom.size() > 0) { + upperlid_zoom = current_eye_asset.upperlid_zoom[frame % current_eye_asset.upperlid_zoom.size()]; + } + if (current_eye_asset.iris_zoom.size() > 0) { + look_zoom = current_eye_asset.iris_zoom[frame % current_eye_asset.iris_zoom.size()]; + } + std::vector dx_extra = {}, dy_extra = {}, dtheta_extra = {}; + std::vector dzoom_extra = {}; + int path_extra_size = current_eye_asset.path_extra.size(); + if (path_extra_size > 0) { + dx_extra.resize(path_extra_size, 0); + dy_extra.resize(path_extra_size, 0); + dtheta_extra.resize(path_extra_size, 0); + dzoom_extra.resize(path_extra_size, 0); + for(int i = 0; i < current_eye_asset.path_extra.size(); i ++ ) { + if ( (current_eye_asset.extra_position_x.size() > i) && (current_eye_asset.extra_position_x[i].size() > 0)) { + dx_extra[i] = current_eye_asset.extra_position_x[i][frame % current_eye_asset.extra_position_x[i].size()]; + } + if ( (current_eye_asset.extra_position_y.size() > i) && (current_eye_asset.extra_position_y[i].size() > 0)) { + dy_extra[i] = current_eye_asset.extra_position_y[i][frame % current_eye_asset.extra_position_y[i].size()]; + } + if ( (current_eye_asset.extra_rotation_theta.size() > i) && (current_eye_asset.extra_rotation_theta[i].size() > 0)) { + dtheta_extra[i] = current_eye_asset.extra_rotation_theta[i][frame % current_eye_asset.extra_rotation_theta[i].size()]; + } + if ( (current_eye_asset.extra_zoom.size() > i) && (current_eye_asset.extra_zoom[i].size() > 0)) { + dzoom_extra[i] = current_eye_asset.extra_zoom[i][frame % current_eye_asset.extra_zoom[i].size()]; + } + } + } + + update_look(current_eye_asset.iris_default_pos_x, current_eye_asset.iris_default_pos_y, + current_eye_asset.iris_default_theta, + current_eye_asset.iris_default_zoom * look_zoom, // dx, dy, dtheta, dzoom + upperlid_x, upperlid_y, upperlid_theta, upperlid_zoom, // dx_upperlid, dy_upperlid, dtheta_upperlid, zdoom_upperlid + dx_extra, dy_extra, dtheta_extra, dzoom_extra + ); frame ++; return frame; } -#if !defined(USE_I2C) && !defined(USE_ROS) // sample code for eye asset without ROS/I2C -class EyeManagerIO: public EyeManager +#define check_eye_asset_map_key(name) \ + if (eye_asset_map.find(name) == eye_asset_map.end()) { \ + logerror("Invalid eye_asset_map_key : [%s]", name.c_str()); \ + logerror("Available keys are..."); \ + for(auto & eye_asset: eye_asset_map) { \ + logerror(" ... [%s]", eye_asset.first.c_str()); \ + } \ + return -1; \ + } + +int EyeManager::setup_asset(std::string eye_asset_text) { + static bool mode_right; + static int direction; + static std::string eye_asset_name_first = ""; + bool read_all_eye_asset_done = false; + + loginfo("Setup eye asset"); + std::istringstream iss(eye_asset_text); + std::string message; + while (std::getline(iss, message)) { + if ( message.empty() ) { continue; } + std::string key, value; + splitKeyValue(message, key, value); + logdebug("received : %s", message.c_str()); + if ( key == "mode_right" ) { + if ( value == "True" ) { + mode_right = true; + } else if ( value == "False" ) { + mode_right = false; + } else { + logerror("Invalid command for mode_right : %s", value.c_str()); + } + } else if ( key == "direction" ) { + direction = std::stoi(value); + } else if ( key == "eye_asset_names" ) { + std::list eye_asset_names = splitComma(value); + if (eye_asset_names.size() > 0) { + eye_asset_name_first = eye_asset_names.front(); + } + // + // initialize eye_asset_map from eye_asset_names + // + //std::map& eye_asset_map = this->eye_asset_map; + for(auto name: eye_asset_names) { + eye_asset_map[name] = EyeAsset(); + eye_asset_map[name].name = name; + eye_asset_map[name].direction = direction; + eye_asset_map[name].invert_rl = not mode_right; + } + } else if ( key == "eye_asset_image_path" ) { + std::string name, type_path, type, path; + splitKeyValue(value, name, type_path); + // check_eye_asset_map_key(name); + splitKeyValue(type_path, type, path); + // + // update eye_asset image map from eye_asset_images + // + EyeAsset *asset = &(eye_asset_map[name]); + if ( type == "outline" ) { + asset->path_outline = path; + } else if ( type == "iris" ) { + asset->path_iris = path; + } else if ( type == "pupil" ) { + asset->path_pupil = path; + } else if ( type == "reflex" ) { + asset->path_reflex = path; + } else if ( type == "upperlid" ) { + asset->path_upperlid = path; + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->path_extra.size() < EXTRA_EYE_ASSET_SIZE) { + asset->path_extra.push_back(path); + } else { + logerror("Extra image buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_path : %s (%s,%s)", type_path.c_str(), type.c_str(), path.c_str()); + return -1; + } + } else if ( key == "eye_asset_position" || key == "eye_asset_position_y") { + std::string name, type_position, type, position; + splitKeyValue(value, name, type_position); + check_eye_asset_map_key(name); + splitKeyValue(type_position, type, position); + // + // update eye_asset image map from eye_asset_upperlid_position + // + EyeAsset *asset = &(eye_asset_map[name]); + std::list ey_asset_position = splitComma(position); + if ( type == "upperlid" ) { + asset->upperlid_position_y.clear(); + for(std::string pos: ey_asset_position) { + asset->upperlid_position_y.push_back(std::stoi(pos)); + } + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_position_y.size() < EXTRA_EYE_ASSET_SIZE) { + std::vector pos_list; + for(std::string pos: ey_asset_position) { + pos_list.push_back(std::stoi(pos)); + } + asset->extra_position_y.push_back(pos_list); + } else { + logerror("Extra position_y buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_position : %s (%s,%s)", type_position.c_str(), type.c_str(), position.c_str()); + return -1; + } + } else if ( key == "eye_asset_position_x" ) { + std::string name, type_position_x, type, position_x; + splitKeyValue(value, name, type_position_x); + check_eye_asset_map_key(name); + splitKeyValue(type_position_x, type, position_x); + // + // update eye_asset image map from ey_asset_position_x + // + EyeAsset *asset = &(eye_asset_map[name]); + std::list eye_asset_position_x = splitComma(position_x); + if ( type == "upperlid" ) { + asset->upperlid_position_x.clear(); + for(std::string pos: eye_asset_position_x) { + asset->upperlid_position_x.push_back(std::stoi(pos)); + } + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_position_x.size() < EXTRA_EYE_ASSET_SIZE) { + std::vector pos_list; + for(std::string pos: eye_asset_position_x) { + pos_list.push_back(std::stoi(pos)); + } + asset->extra_position_x.push_back(pos_list); + } else { + logerror("Extra position_x buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_position_x : %s (%s,%s)", type_position_x.c_str(), type.c_str(), position_x.c_str()); + return -1; + } + } else if ( key == "eye_asset_rotation_theta" ) { + std::string name, type_rotation_theta, type, rotation_theta; + splitKeyValue(value, name, type_rotation_theta); + check_eye_asset_map_key(name); + splitKeyValue(type_rotation_theta, type, rotation_theta); + // + // update eye_asset image map from eye_asset_upperlid_rotation_theta + // + EyeAsset *asset = &(eye_asset_map[name]); + std::list eye_asset_rotation_theta = splitComma(rotation_theta); + if ( type == "upperlid" ) { + asset->upperlid_rotation_theta.clear(); + for(std::string pos: eye_asset_rotation_theta) { + asset->upperlid_rotation_theta.push_back(std::stoi(pos)); + } + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_rotation_theta.size() < EXTRA_EYE_ASSET_SIZE) { + std::vector pos_list; + for(std::string pos: eye_asset_rotation_theta) { + pos_list.push_back(std::stoi(pos)); + } + asset->extra_rotation_theta.push_back(pos_list); + } else { + logerror("Extra rotation_theta buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_rotation_theta : %s (%s,%s)", type_rotation_theta.c_str(), type.c_str(), rotation_theta.c_str()); + return -1; + } + } else if ( key == "eye_asset_zoom" ) { + std::string name, type_zoom, type, zoom; + splitKeyValue(value, name, type_zoom); + check_eye_asset_map_key(name); + splitKeyValue(type_zoom, type, zoom); + // + // update eye_asset image map from eye_asset_upperlid_zoom + // + EyeAsset *asset = &(eye_asset_map[name]); + std::list eye_asset_zoom = splitComma(zoom); + if ( type == "iris" ) { + asset->iris_zoom.clear(); + for(std::string pos: eye_asset_zoom) { + asset->iris_zoom.push_back(std::stof(pos)); + } + } else if ( type == "upperlid" ) { + asset->upperlid_zoom.clear(); + for(std::string pos: eye_asset_zoom) { + asset->upperlid_zoom.push_back(std::stof(pos)); + } + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_zoom.size() < EXTRA_EYE_ASSET_SIZE) { + std::vector pos_list; + for(std::string pos: eye_asset_zoom) { + pos_list.push_back(std::stof(pos)); + } + asset->extra_zoom.push_back(pos_list); + } else { + logerror("Extra zoom buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_zoom : %s (%s,%s)", type_zoom.c_str(), type.c_str(), zoom.c_str()); + return -1; + } + } else if ( key == "eye_asset_default_pos_x" ) { + std::string name, type_default_pos_x, type, default_pos_x; + splitKeyValue(value, name, type_default_pos_x); + check_eye_asset_map_key(name); + splitKeyValue(type_default_pos_x, type, default_pos_x); + // + // update eye_asset image map from eye_asset_default_pos_x + // + EyeAsset *asset = &(eye_asset_map[name]); + if ( type == "iris" ) { + asset->iris_default_pos_x = std::stoi(default_pos_x); + } else if ( type == "upperlid" ) { + asset->upperlid_default_pos_x = std::stoi(default_pos_x); + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_default_pos_x.size() < EXTRA_EYE_ASSET_SIZE) { + asset->extra_default_pos_x.push_back(std::stoi(default_pos_x)); + } else { + logerror("Extra default_pos_x buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_default_pos_x : %s (%s,%s)", type_default_pos_x.c_str(), type.c_str(), default_pos_x.c_str()); + return -1; + } + } else if ( key == "eye_asset_default_pos_y" ) { + std::string name, type_default_pos_y, type, default_pos_y; + splitKeyValue(value, name, type_default_pos_y); + check_eye_asset_map_key(name); + splitKeyValue(type_default_pos_y, type, default_pos_y); + // + // update eye_asset image map from eye_asset_default_pos_y + // + EyeAsset *asset = &(eye_asset_map[name]); + if ( type == "iris" ) { + asset->iris_default_pos_y = std::stoi(default_pos_y); + } else if ( type == "upperlid" ) { + asset->upperlid_default_pos_y = std::stoi(default_pos_y); + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_default_pos_y.size() < EXTRA_EYE_ASSET_SIZE) { + asset->extra_default_pos_y.push_back(std::stoi(default_pos_y)); + } else { + logerror("Extra default_pos_y buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_default_pos_y : %s (%s,%s)", type_default_pos_y.c_str(), type.c_str(), default_pos_y.c_str()); + return -1; + } + } else if ( key == "eye_asset_default_theta" ) { + std::string name, type_default_theta, type, default_theta; + splitKeyValue(value, name, type_default_theta); + check_eye_asset_map_key(name); + splitKeyValue(type_default_theta, type, default_theta); + // + // update eye_asset image map from eye_asset_default_theta + // + EyeAsset *asset = &(eye_asset_map[name]); + if ( type == "iris" ) { + asset->iris_default_theta = std::stoi(default_theta); + } else if ( type == "upperlid" ) { + asset->upperlid_default_theta = std::stoi(default_theta); + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_default_theta.size() < EXTRA_EYE_ASSET_SIZE) { + asset->extra_default_theta.push_back(std::stoi(default_theta)); + } else { + logerror("Extra default_theta buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_default_theta : %s (%s,%s)", type_default_theta.c_str(), type.c_str(), default_theta.c_str()); + return -1; + } + } else if ( key == "eye_asset_default_zoom" ) { + std::string name, type_default_zoom, type, default_zoom; + splitKeyValue(value, name, type_default_zoom); + check_eye_asset_map_key(name); + splitKeyValue(type_default_zoom, type, default_zoom); + // + // update eye_asset image map from eye_asset_default_zoom + // + EyeAsset *asset = &(eye_asset_map[name]); + if ( type == "iris" ) { + asset->iris_default_zoom = std::stof(default_zoom); + } else if ( type == "upperlid" ) { + asset->upperlid_default_zoom = std::stof(default_zoom); + } else if ( type.compare(0, 5, "extra" ) == 0 ){ + if (asset->extra_default_zoom.size() < EXTRA_EYE_ASSET_SIZE) { + asset->extra_default_zoom.push_back(std::stof(default_zoom)); + } else { + logerror("Extra default_zoom buffer overflow"); + return -1; + } + } else { + logerror("Invalid eye_asset type_default_zoom : %s (%s,%s)", type_default_zoom.c_str(), type.c_str(), default_zoom.c_str()); + return -1; + } + } else if ( key == "eye_asset_done" ) { + // skip special command to display and initialize eye for I2C + read_all_eye_asset_done = true; + } else { + logerror("Invlalid command : %s (key : %s, value : %s)", message.c_str(), key.c_str(), value.c_str()); + return -1; + } + } + + if (!read_all_eye_asset_done) { + // display eye_asset_map only when read_all_eye_asset_done + return 0; + } + // display map data + // to show this message, + // call rosservice call eye_display/set_logger_level rosout DEBUG + // or + // roslaunch launch file with debug:=true + for(auto& it: eye_asset_map) { + std::string name = it.first; + EyeAsset &eye_asset = eye_asset_map[name]; + loginfo("[%s]", name.c_str()); + loginfo(" mode_right : %s", eye_asset.invert_rl?"True":"False"); + loginfo(" direction : %d", eye_asset.direction); + loginfo(" outline image : %s", eye_asset.path_outline.c_str()); + loginfo(" iris image : %s", eye_asset.path_iris.c_str()); + loginfo(" pupil image : %s", eye_asset.path_pupil.c_str()); + loginfo(" reflex image : %s", eye_asset.path_reflex.c_str()); + loginfo(" upperlid image : %s", eye_asset.path_upperlid.c_str()); + + for(int i = 0; i < eye_asset.path_extra.size(); i++) { + loginfo(" extra%d image : %s", i, eye_asset.path_extra[i].c_str()); + } + if (eye_asset.iris_zoom.size() > 0) { + loginfo(" iris_positoin_zoom: %s", joinVector(eye_asset.iris_zoom).c_str()); + } + loginfo(" upperlid_position_x: %s", joinVector(eye_asset.upperlid_position_x).c_str()); + loginfo(" upperlid_position_y: %s", joinVector(eye_asset.upperlid_position_y).c_str()); + loginfo(" upperlid_rotation_theta: %s", joinVector(eye_asset.upperlid_rotation_theta).c_str()); + if (eye_asset.upperlid_zoom.size() > 0) { + loginfo(" upperlid_positoin_zoom: %s", joinVector(eye_asset.upperlid_zoom).c_str()); + } + loginfo(" upperlid_default_pos_x : %d", eye_asset.upperlid_default_pos_x); + loginfo(" upperlid_default_pos_y : %d", eye_asset.upperlid_default_pos_y); + loginfo(" upperlid_default_theta : %d", eye_asset.upperlid_default_theta); + loginfo(" upperlid_default_zoom : %f", eye_asset.upperlid_default_zoom); + + for(int i = 0; i < eye_asset.extra_position_x.size(); i++) { + loginfo(" extra%d_position_x : %s", i, joinVector(eye_asset.extra_position_x[i]).c_str()); + } + for(int i = 0; i < eye_asset.extra_position_y.size(); i++) { + loginfo(" extra%d_position_y : %s", i, joinVector(eye_asset.extra_position_y[i]).c_str()); + } + for(int i = 0; i < eye_asset.extra_rotation_theta.size(); i++) { + loginfo(" extra%d_rotation_theta: %s", i, joinVector(eye_asset.extra_rotation_theta[i]).c_str()); + } + for(int i = 0; i < eye_asset.extra_zoom.size(); i++) { + loginfo(" extra%d_zoom: %s", i, joinVector(eye_asset.extra_zoom[i]).c_str()); + } + + for(int i = 0; i < eye_asset.extra_default_pos_x.size(); i++) { + loginfo(" extra%d default_pos_x : %d", i, eye_asset.extra_default_pos_x[i]); + } + for(int i = 0; i < eye_asset.extra_default_pos_y.size(); i++) { + loginfo(" extra%d default_pos_y : %d", i, eye_asset.extra_default_pos_y[i]); + } + for(int i = 0; i < eye_asset.extra_default_theta.size(); i++) { + loginfo(" extra%d default_theta : %d", i, eye_asset.extra_default_theta[i]); + } + } + // eyeの初期化 + if ( !eye_asset_map.empty() ) { + if ( eye_asset_name_first.empty() ) { + eye_asset_name_first = eye_asset_map.begin()->first; + } + loginfo("[%8ld] initialize eye with %s", millis(), eye_asset_name_first.c_str()); + set_emotion(eye_asset_name_first); + } else { + logwarn("Faile to initialize emotion, use default asset"); + } + return 0; +} + +unsigned long EyeManager::update_next_time() { -public: - EyeManagerIO() : EyeManager() {} - void logdebug(const char *fmt, ...) { } // should be implemented in EyeManagerIO class - void loginfo(const char *fmt, ...) { } // should be implemented in EyeManagerIO class - void logwarn(const char *fmt, ...) { } // should be implemented in EyeManagerIO class - void logerror(const char *fmt, ...) { } // should be implemented in EyeManagerIO class - void logfatal(const char *fmt, ...) { } // should be implemented in EyeManagerIO class -}; -#endif + next_time = millis() + interval_time; + return next_time; +} + +long EyeManager::delay_next_time() +{ + long sleep_time = next_time - millis(); + if ( sleep_time > 0 ) { + delay(sleep_time); + } + next_time += interval_time; + return sleep_time; +} diff --git a/eye_display/include/i2c_lib.h b/eye_display/include/i2c_lib.h index 9928264d4..0ae88a436 100644 --- a/eye_display/include/i2c_lib.h +++ b/eye_display/include/i2c_lib.h @@ -2,52 +2,39 @@ #include #include +#include "util.h" constexpr int SDA_PIN = 8; constexpr int SCL_PIN = 9; +#ifdef I2C_SLAVE_ADDR +constexpr int I2C_SLAVE_ADDR = I2C_SLAVE_ADDR; +#else constexpr int I2C_SLAVE_ADDR = 0x42; +#endif -class EyeManagerIO : public EyeManager { -public: - EyeManagerIO() : EyeManager() {} - -#define def_eye_manager_log_func(funcname) \ - void funcname(const char *fmt, ...) override { \ - char *string; \ - va_list args; \ - va_start(args, fmt); \ - if (0 > vasprintf(&string, fmt, args)) string = NULL; \ - va_end(args); \ - if (string) { \ - Serial.print("["); \ - Serial.print(#funcname); \ - Serial.print("] "); \ - Serial.println(string); \ - free(string); \ - } \ - } - def_eye_manager_log_func(logdebug); - def_eye_manager_log_func(loginfo); - def_eye_manager_log_func(logwarn); - def_eye_manager_log_func(logerror); - def_eye_manager_log_func(logfatal); -}; - -extern EyeManagerIO eye; +extern EyeManager eye; void receiveEvent(int howMany) { - std::map& eye_asset_map = eye.eye_asset_map; - // lastReceiveTime = millis(); // Update the last received time - String str; + std::string message; while (0 < WireSlave.available()) { char c = WireSlave.read(); // receive byte as a character - str += c; + message += c; } - std::string current_eye_asset_name(str.c_str()); - auto it = eye_asset_map.find(current_eye_asset_name); - if (it != eye_asset_map.end()) { - eye.set_emotion(it->first); + logdebug("I2C received : %s", message.c_str()); + + std::string key, value; + splitKeyValue(message, key, value); + if ( key == "eye_status" ) { + std::map& eye_asset_map = eye.eye_asset_map; + eye.set_emotion(value); + } else if ( key == "look_at") { + double x, y; + parseXY(value, x, y); + eye.set_gaze_direction(x, y); + } else { + eye.setup_asset({message}); } + return; } void I2CTask(void *parameter) { @@ -65,33 +52,7 @@ void I2CTask(void *parameter) { } } -void setup_asset(EyeManager& eye) // returns initial status -{ - std::map& eye_asset_map = eye.eye_asset_map; - eye_asset_map["normal"] = EyeAsset(); - eye_asset_map["normal"].name = "normal"; - eye_asset_map["normal"].upperlid_position = {9}; - eye_asset_map["normal"].direction = 1; -#if defined(EYE_RIGHT) - eye_asset_map["normal"].direction = true; -#else - eye_asset_map["normal"].invert_rl = false; -#endif - - eye_asset_map["blink"] = EyeAsset(); - eye_asset_map["blink"].name = "blink"; - eye_asset_map["blink"].upperlid_position = {9, 9, 130, 130, 9, 9}; - eye_asset_map["blink"].direction = 1; -#if defined(EYE_RIGHT) - eye_asset_map["blink"].direction = true; -#else - eye_asset_map["blink"].invert_rl = false; -#endif - - eye.set_emotion("blink"); -} - void setup_i2c() { - xTaskCreatePinnedToCore(I2CTask, "I2C Task", 1024, NULL, 24, NULL, 0); + xTaskCreatePinnedToCore(I2CTask, "I2C Task", 4096, NULL, 24, NULL, 0); } diff --git a/eye_display/include/node_handle_ex.h b/eye_display/include/node_handle_ex.h index 6a788bd6b..cf9ac8dd6 100644 --- a/eye_display/include/node_handle_ex.h +++ b/eye_display/include/node_handle_ex.h @@ -8,8 +8,8 @@ namespace ros template + int INPUT_SIZE = 4096, + int OUTPUT_SIZE = 4096> class NodeHandleEx : public NodeHandle_ { typedef NodeHandle_ super; @@ -18,7 +18,6 @@ class NodeHandleEx : public NodeHandle_ ¶m, int timeout = 1000) + { + if (!super::requestParam(name, timeout)) + return false; + param.resize(super::req_param_resp.floats_length); + // copy it over + for (int i = 0; i < super::req_param_resp.floats_length; i++) + param[i] = super::req_param_resp.floats[i]; + return true; + } #define def_char_log_formatter(funcname) \ void funcname(const char *format, ...) { \ diff --git a/eye_display/include/ros_lib.h b/eye_display/include/ros_lib.h index e1847002c..bc480fddb 100644 --- a/eye_display/include/ros_lib.h +++ b/eye_display/include/ros_lib.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -13,140 +12,199 @@ void callback_emotion(const std_msgs::String &msg); ros::NodeHandleEx nh; -class EyeManagerIO : public EyeManager { -public: - EyeManagerIO() : EyeManager() {} - -#define def_eye_manager_log_func(funcname) \ - void funcname(const char *fmt, ...) override { \ - char *string; \ - va_list args; \ - va_start(args, fmt); \ - if (0 > vasprintf(&string, fmt, args)) string = NULL; \ - va_end(args); \ - if (string) { \ - nh.funcname(string); \ - free(string); \ - } \ - } - def_eye_manager_log_func(logdebug); - def_eye_manager_log_func(loginfo); - def_eye_manager_log_func(logwarn); - def_eye_manager_log_func(logerror); - def_eye_manager_log_func(logfatal); -}; +bool ros_was_connected = false; +bool ros_now_connected = false; +#define def_log_func(funcname) \ +void funcname(const char *fmt, ...) { \ + char *string; \ + va_list args; \ + if (!ros_now_connected) {return;} \ + va_start(args, fmt); \ + if (0 > vasprintf(&string, fmt, args)) string = NULL; \ + va_end(args); \ + if (string) { \ + nh.funcname(string); \ + free(string); \ + } \ +} +def_log_func(logdebug); +def_log_func(loginfo); +def_log_func(logwarn); +def_log_func(logerror); +def_log_func(logfatal); -extern EyeManagerIO eye; +extern EyeManager eye; ros::Subscriber sub_point("~look_at", &callback_look_at); ros::Subscriber sub_eye_status("~eye_status", &callback_emotion); + void callback_look_at(const geometry_msgs::Point &msg) { - eye.set_gaze_direction((float)msg.x, (float)msg.y); + eye.set_gaze_direction((float)msg.x, (float)msg.y, (float)msg.z); } void callback_emotion(const std_msgs::String &msg) { - eye.set_emotion(msg.data); + // eye_status: + // {data : 'emotion name'} for set emotion + // {data : 'eye_asset_default_zoom: normal: iris: 1.2'} changes parameters + if (std::string(msg.data).find(':') == std::string::npos) { + eye.set_emotion(msg.data); + } else { // if message is key-value + eye.setup_asset(msg.data); + } } -void setup_asset(EyeManager& eye) +std::string ros_read_asset() { std::map& eye_asset_map = eye.eye_asset_map; - while (not nh.connected()) - { - nh.spinOnce(); - delay(1000); - } - delay(500); // wait 0.5 sec before reading asset - nh.loginfo("Setup eye asset"); + std::ostringstream oss; bool mode_right; int direction = 1; if (nh.getParam("~mode_right", &mode_right)) { - nh.loginfo(mode_right ? "Read rosparam : mode_right is true" : "mode_right is false"); + nh.logdebug(mode_right ? "Read rosparam : mode_right is true" : "mode_right is false"); + oss << "mode_right: " << (mode_right?"True":"False") << "\n"; } else { - nh.loginfo("Failed to get mode_right parameter"); + nh.logwarn("Failed to get mode_right parameter"); } nh.getParam("~direction", &direction); - nh.loginfo("Read rosparam : direction is %d", direction); + nh.logdebug("Read rosparam : direction is %d", direction); + oss << "direction: " << direction << "\n"; // get eye_asset_names from rosParam std::vector eye_asset_names; nh.getParam("~eye_asset/names", eye_asset_names); - for (int i=0; ipath_upperlid); - // path_outline - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/path_outline", name.c_str()); - nh.getParam(eye_asset_map_key, asset->path_outline); - // path_iris - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/path_iris", name.c_str()); - nh.getParam(eye_asset_map_key, asset->path_iris); - // path_pupil - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/path_pupil", name.c_str()); - nh.getParam(eye_asset_map_key, asset->path_pupil); - // path_reflex - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/path_reflex", name.c_str()); - nh.getParam(eye_asset_map_key, asset->path_reflex); - // upperlid_position - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_position", name.c_str()); - nh.getParam(eye_asset_map_key, asset->upperlid_position); - // upperlid_default_pos_x - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_default_pos_x", name.c_str()); - nh.getParam(eye_asset_map_key, &(asset->upperlid_default_pos_x)); - // upperlid_default_pos_y - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_default_pos_y", name.c_str()); - nh.getParam(eye_asset_map_key, &(asset->upperlid_default_pos_y)); - // upperlid_default_theta - snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_default_theta", name.c_str()); - nh.getParam(eye_asset_map_key, &(asset->upperlid_default_theta)); - } + char eye_asset_map_key[256]; - // display map data - // to show this message, - // call rosservice call eye_display/set_logger_level rosout DEBUG - // or - // roslaunch launch file with debug:=true - for(auto name: eye_asset_names) { - EyeAsset &eye_asset = eye_asset_map[name]; - nh.logdebug("[%s]", name.c_str()); - nh.logdebug(" outline image : %s", eye_asset.path_outline.c_str()); - nh.logdebug(" iris image : %s", eye_asset.path_iris.c_str()); - nh.logdebug(" pupil image : %s", eye_asset.path_pupil.c_str()); - nh.logdebug(" reflex image : %s", eye_asset.path_reflex.c_str()); - nh.logdebug(" upperlid image : %s", eye_asset.path_upperlid.c_str()); - std::ostringstream oss; - std::copy(eye_asset.upperlid_position.begin(), eye_asset.upperlid_position.end(), std::ostream_iterator(oss, ", ")); - std::string result = oss.str(); result.pop_back(); result.pop_back(); // remove last "," - nh.logdebug(" upperlid_positions: %s", result.c_str()); - nh.logdebug(" upperlid_default_pos_x : %d", eye_asset.upperlid_default_pos_x); - nh.logdebug(" upperlid_default_pos_y : %d", eye_asset.upperlid_default_pos_y); - nh.logdebug(" upperlid_default_theta : %d", eye_asset.upperlid_default_theta); - } - // eyeの初期化 - if ( eye_asset_names.size() > 0 ) { - eye.set_emotion(eye_asset_names[0]); - } else { - nh.logwarn("Faile to initialize emotion, use default asset"); + // path + std::vector eye_types; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset_%s_eye_types", name.c_str()); + nh.getParam(eye_asset_map_key, eye_types); + for(const std::string& type: eye_types) { + std::string path; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/path_%s", name.c_str(), type.c_str()); + if (nh.getParam(eye_asset_map_key, path)) { + oss << "eye_asset_image_path: " << name << ": " << type << ": " << path << "\n"; + } + } + + // position + std::vector eye_positions; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset_%s_positions", name.c_str()); + nh.getParam(eye_asset_map_key, eye_positions); + for(const std::string& eye_position: eye_positions) { + if (eye_position == "NONE") continue; + std::vector position; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s", name.c_str(), eye_position.c_str()); + if (nh.getParam(eye_asset_map_key, position)) { + size_t pos = eye_position.find('_'); + if ( pos != std::string::npos ) { + std::string eye_type = eye_position.substr(0, pos); + std::string position_type = eye_position.substr(pos+1); + oss << "eye_asset_" << position_type << ": " << name << ": " << eye_type << ": " << joinVector(position) << "\n"; + } else { + logerror("Invalid param name : %s", eye_position); + } + } + } + + // rotation + std::vector eye_rotations; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset_%s_rotations", name.c_str()); + nh.getParam(eye_asset_map_key, eye_rotations); + for(const std::string& eye_rotation: eye_rotations) { + if (eye_rotation == "NONE") continue; + std::vector rotation; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s", name.c_str(), eye_rotation.c_str()); + if (nh.getParam(eye_asset_map_key, rotation)) { + size_t pos = eye_rotation.find('_'); + if ( pos != std::string::npos ) { + std::string eye_type = eye_rotation.substr(0, pos); + std::string rotation_type = eye_rotation.substr(pos+1); + oss << "eye_asset_" << rotation_type << ": " << name << ": " << eye_type << ": " << joinVector(rotation) << "\n"; + } else { + logerror("Invalid param name : %s", eye_rotation); + } + } + } + + // default + std::vector eye_defaults; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset_%s_defaults", name.c_str()); + nh.getParam(eye_asset_map_key, eye_defaults); + for(const std::string& eye_default: eye_defaults) { + if (eye_default == "NONE") continue; + if (eye_default.find("_default_zoom") != std::string::npos) { + std::vector defaults; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s", name.c_str(), eye_default.c_str()); + if (nh.getParam(eye_asset_map_key, defaults)) { + size_t pos = eye_default.find('_'); + if ( pos != std::string::npos ) { + std::string eye_type = eye_default.substr(0, pos); + std::string default_type = eye_default.substr(pos+1); + oss << "eye_asset_" << default_type << ": " << name << ": " << eye_type << ": " << joinVector(defaults) << "\n"; + } else { + logerror("Invalid param name : %s", eye_default); + } + } + } else { + std::vector defaults; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s", name.c_str(), eye_default.c_str()); + if (nh.getParam(eye_asset_map_key, defaults)) { + size_t pos = eye_default.find('_'); + if ( pos != std::string::npos ) { + std::string eye_type = eye_default.substr(0, pos); + std::string default_type = eye_default.substr(pos+1); + oss << "eye_asset_" << default_type << ": " << name << ": " << eye_type << ": " << joinVector(defaults) << "\n"; + } else { + logerror("Invalid param name : %s", eye_default); + } + } + } + } + + // zoom + std::vector eye_zooms; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset_%s_zooms", name.c_str()); + nh.getParam(eye_asset_map_key, eye_zooms); + for(const std::string& eye_zoom: eye_zooms) { + if (eye_zoom == "NONE") continue; + std::vector zooms; + snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s", name.c_str(), eye_zoom.c_str()); + if (nh.getParam(eye_asset_map_key, zooms)) { + size_t pos = eye_zoom.find('_'); + if ( pos != std::string::npos ) { + std::string eye_type = eye_zoom.substr(0, pos); + std::string zoom_type = eye_zoom.substr(pos+1); + oss << "eye_asset_" << zoom_type << ": " << name << ": " << eye_type << ": " << joinVector(zooms) << "\n"; + } else { + logerror("Invalid param name : %s", eye_zoom); + } + } + } } + + oss << "eye_asset_done:\n"; + std::string eye_asset_text = oss.str(); + nh.logdebug(eye_asset_text.c_str()); + return eye_asset_text; } void setup_ros() { +#if defined(STAMPC3) + nh.getHardware()->setBaud(115200); +#endif nh.initNode(); nh.subscribe(sub_point); nh.subscribe(sub_eye_status); @@ -155,12 +213,11 @@ void setup_ros() void reconnect_ros(EyeManager &eye) { - while (not nh.connected()) - { - nh.spinOnce(); - delay(1000); + ros_now_connected = nh.connected(); + if (ros_now_connected && !ros_was_connected) { + loginfo("ROS reconnected, initializing eye assets"); // when ROS node is re-connected, get rosparam again - setup_asset(eye); - eye.init(); + eye.setup_asset(ros_read_asset()); } + ros_was_connected = ros_now_connected; } diff --git a/eye_display/include/util.h b/eye_display/include/util.h new file mode 100644 index 000000000..6e2d34c43 --- /dev/null +++ b/eye_display/include/util.h @@ -0,0 +1,103 @@ +#ifndef __UTIL_H__ +#define __UTIL_H__ + +#include +#include +#include + +std::string trim(const std::string& s) { + auto start = std::find_if_not(s.begin(), s.end(), ::isspace); + auto end = std::find_if_not(s.rbegin(), s.rend(), ::isspace).base(); + if (start >= end) return ""; + return std::string(start, end); +} + +std::list splitComma(const std::string& input) { + std::list result; + std::stringstream ss(input); + std::string item; + + while (std::getline(ss, item, ',')) { + result.push_back(trim(item)); + } + + // just in case where last element is blank + if (!input.empty() && input.back() == ',') { + result.push_back(""); // add null element + } + + return result; +} + +bool splitKeyValue(const std::string& message, std::string& key, std::string& value) { + size_t colonPos = message.find(':'); + if (colonPos == std::string::npos) { + return false; // failed if ":" is not found + } + + // trim key + size_t keyStart = 0; + size_t keyEnd = colonPos; + while (keyEnd > keyStart && std::isspace(message[keyEnd - 1])) --keyEnd; + + // trim value + size_t valStart = colonPos + 1; + while (valStart < message.size() && std::isspace(message[valStart])) ++valStart; + size_t valEnd = message.size(); + while (valEnd > valStart && std::isspace(message[valEnd - 1])) --valEnd; + + key = message.substr(keyStart, keyEnd - keyStart); + value = message.substr(valStart, valEnd - valStart); + return true; +} + +bool parseXY(const std::string& input, double& x, double& y) { + // remove spaces + std::string cleaned; + for (char c : input) { + // convert camma to spaces + if (c == ',') { + cleaned += ' '; + } else { + cleaned += c; + } + } + + std::istringstream iss(cleaned); + return static_cast(iss >> x >> y); // return true if there are x and y +} + +template +std::string joinVector(const std::vector& vec, const std::string& sep = ", ") { + std::ostringstream oss; + for (size_t i = 0; i < vec.size(); ++i) { + oss << vec[i]; + if (i + 1 < vec.size()) oss << sep; + } + return oss.str(); +} + +#if !defined(USE_ROS) // ROS uses it's own logdebug/loginfo.... +#define def_log_func(funcname) \ +void funcname(const char *fmt, ...) { \ + char *string; \ + va_list args; \ + va_start(args, fmt); \ + if (0 > vasprintf(&string, fmt, args)) string = NULL; \ + va_end(args); \ + if (string) { \ + Serial.print("["); \ + Serial.print(#funcname); \ + Serial.print("] "); \ + Serial.println(string); \ + free(string); \ + } \ +}; +def_log_func(logdebug); +def_log_func(loginfo); +def_log_func(logwarn); +def_log_func(logerror); +def_log_func(logfatal); +#endif + +#endif // __UTIL_H__ diff --git a/eye_display/launch/demo.launch b/eye_display/launch/demo.launch index dc0d8fc8e..1ce8614f8 100644 --- a/eye_display/launch/demo.launch +++ b/eye_display/launch/demo.launch @@ -1,16 +1,20 @@ - + + + + @@ -18,13 +22,40 @@ + + + + + + + + + + diff --git a/eye_display/launch/demo_dual.launch b/eye_display/launch/demo_dual.launch new file mode 100644 index 000000000..bd696fded --- /dev/null +++ b/eye_display/launch/demo_dual.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/eye_display/node_scripts/asset_param_list.py b/eye_display/node_scripts/asset_param_list.py new file mode 100644 index 000000000..ea405bed2 --- /dev/null +++ b/eye_display/node_scripts/asset_param_list.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +import os +import rospy +import rosparam +from rosserial_msgs.srv import RequestParam, RequestParamResponse + +# This node provides a list of parameters related to eye_asset. +# When getParam is called from the ESP32 with non-existent names, it takes a long time to timeout and returns an error. +# To prevent this delay, we first retrieve the list of available parameter names before requesting their values. + +def main(): + rospy.init_node('asset_param_list') + + # get eye_asset param + param_ns = rospy.get_namespace().rstrip('/') + eye_asset_names = rospy.get_param("{}/eye_asset/names".format(param_ns)) + + for name in eye_asset_names: + eye_asset_param_names = rospy.get_param("{}/eye_asset/{}".format(param_ns, name)).keys() + + eye_types = [] + for key in eye_asset_param_names: + # image path : cehck if key start with path_ + if key.startswith("path_"): + eye_type = key[len("path_"):] + eye_types.append(eye_type) + rospy.set_param("{}/eye_asset_{}_eye_types".format(param_ns, name), eye_types) + + positions = [] + for eye_type in eye_types: + positions.extend([n for n in eye_asset_param_names if n.startswith("{}_position".format(eye_type))]) + if not positions: + positions.append('NONE') + rospy.set_param("{}/eye_asset_{}_positions".format(param_ns, name), positions) + + rotations = [] + for eye_type in eye_types: + rotations.extend([n for n in eye_asset_param_names if n.startswith("{}_rotation".format(eye_type))]) + if not rotations: + rotations.append('NONE') + rospy.set_param("{}/eye_asset_{}_rotations".format(param_ns, name), rotations) + + defaults = [] + for eye_type in eye_types: + defaults.extend([n for n in eye_asset_param_names if n.startswith("{}_default".format(eye_type))]) + if not defaults: + defaults.append('NONE') + rospy.set_param("{}/eye_asset_{}_defaults".format(param_ns, name), defaults) + + zooms = [] + for eye_type in eye_types: + zooms.extend([n for n in eye_asset_param_names if n.startswith("{}_zoom".format(eye_type))]) + if not zooms: + zooms.append('NONE') + rospy.set_param("{}/eye_asset_{}_zooms".format(param_ns, name), zooms) + +if __name__ == "__main__": + main() diff --git a/eye_display/node_scripts/demo_move_eye.py b/eye_display/node_scripts/demo_move_eye.py index e6c921cdc..4417c461c 100644 --- a/eye_display/node_scripts/demo_move_eye.py +++ b/eye_display/node_scripts/demo_move_eye.py @@ -1,11 +1,13 @@ #!/usr/bin/env python +import argparse +import math import rospy from geometry_msgs.msg import Point +from std_msgs.msg import String -def main(): - rospy.init_node("pub_eye_status") +def test_look_at(): scale = rospy.get_param('~scale', 10.0) msg = Point() pub = rospy.Publisher("/eye_display/look_at", Point, queue_size=1) @@ -41,6 +43,46 @@ def main(): pub.publish(msg) rate.sleep() +def test_eye_status_command(): + loop = 0 + msg = String() + pub = rospy.Publisher("/eye_display/eye_status", String, queue_size=1) + + # set eye_status to normal + rospy.sleep(0.5) # wait for subscriber + pub.publish(String(data="normal")) + + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + phase = (loop % 100)/100.0 + if (int(loop / 100) % 3) == 0: # circle + msg.data = \ + "eye_asset_default_pos_x: normal: iris: {:.1f}\n".format(30*math.sin(2*math.pi*phase)) + \ + "eye_asset_default_pos_y: normal: iris: {:.1f}\n".format(30*math.cos(2*math.pi*phase)) + \ + "eye_asset_default_theta: normal: iris: {:.1f}\n".format(360*phase) + elif (int(loop / 100) % 3) == 1: + msg.data = "eye_asset_default_zoom: normal: iris: {:.1f}\n".format(math.sin(2*math.pi*phase)+1.0) + elif (int(loop / 100) % 3) == 2: + msg.data = "eye_asset_default_zoom: normal: upperlid: {:.1f}\n".format(30*math.sin(2*math.pi*phase)+7) + #print ((int(loop / 100) % 3), phase) + rospy.loginfo("'{}'".format(msg)) + pub.publish(msg) + rate.sleep() + loop = loop + 1 + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-a", "--advanced", action="store_true", help="test advanced eye status control mode") + + args = parser.parse_args() + + rospy.init_node("pub_eye_status") + + if not args.advanced: + test_look_at() + else: + test_eye_status_command() + if __name__ == "__main__": main() diff --git a/eye_display/node_scripts/dual_serial_logger.py b/eye_display/node_scripts/dual_serial_logger.py new file mode 100755 index 000000000..2cad36161 --- /dev/null +++ b/eye_display/node_scripts/dual_serial_logger.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +import os +import sys +import serial +import threading +import time +import datetime +from colorama import Fore, Style, init + +init(autoreset=True) # 色設定のリセット自動 + +def read_serial(port, baud, color, label=None): + if label is None: + label = os.path.basename(port) + try: + ser = serial.Serial(port, baud, timeout=0.1) + while running: + line = ser.readline() + if line: + text = line.decode(errors='ignore').strip() + timestamp = datetime.datetime.now().strftime("%H:%M:%S.%f") + # 端末出力(色付き) + print(f"{color}[{timestamp}] [{label}] {text}{Style.RESET_ALL}") + except serial.SerialException as e: + print(f"{label} ERROR: {e}") + +if __name__ == "__main__": + if len(sys.argv) < 4: + print("Usage: python dual_serial_logger.py ") + sys.exit(1) + + port1 = sys.argv[1] + port2 = sys.argv[2] + baud = int(sys.argv[3]) + + running = True + + t1 = threading.Thread(target=read_serial, args=(port1, baud, Fore.GREEN)) + t2 = threading.Thread(target=read_serial, args=(port2, baud, Fore.BLUE)) + + t1.start() + t2.start() + + try: + while True: + time.sleep(0.1) + except KeyboardInterrupt: + running = False + print("\nStopping...") + t1.join() + t2.join() diff --git a/eye_display/node_scripts/pub_eye_status.py b/eye_display/node_scripts/pub_eye_status.py index c14314363..de212d8ea 100644 --- a/eye_display/node_scripts/pub_eye_status.py +++ b/eye_display/node_scripts/pub_eye_status.py @@ -1,5 +1,9 @@ #!/usr/bin/env python +import argparse +import signal +import sys + import rospy from std_msgs.msg import String @@ -20,22 +24,50 @@ ] + +def signal_handler(sig, frame): + rospy.loginfo("Ctrl+C detected, shutting down...") + sys.exit(0) + def main(): + signal.signal(signal.SIGINT, signal_handler) + + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--dual", action="store_true", help="Enable dual mode") + parser.add_argument("-n", "--names", nargs="+", help="List of eye status names") + parser.add_argument("-r", "--rate", type=float, default=1.0/3, help="Set the rate") + + args = parser.parse_args() + rospy.init_node("pub_eye_status") - pub = rospy.Publisher("/eye_display/eye_status", String, queue_size=10) - eye_status_names = rospy.get_param("/eye_display/eye_asset/names", NAMES) + eye_status_names = args.names + if args.dual: + pubs = [rospy.Publisher("/right/eye_display/eye_status", String, queue_size=10, latch=True), + rospy.Publisher("/left/eye_display/eye_status", String, queue_size=10, latch=True)] + if eye_status_names is None: + eye_status_names_raw = rospy.get_param("/right/eye_display/eye_asset/names", NAMES) + # remove '_extra' suffix when setting eye_asset_names. + eye_status_names = [s[:-len("_extra")] if s.endswith("_extra") else s for s in eye_status_names_raw] + else: + pubs = [rospy.Publisher("/eye_display/eye_status", String, queue_size=10, latch=True)] + if eye_status_names is None: + eye_status_names = rospy.get_param("/eye_display/eye_asset/names", NAMES) rospy.loginfo("eye status names {}".format(eye_status_names)) - rate = rospy.Rate(1.0/3) + + rate = rospy.Rate(args.rate) index = 0 while not rospy.is_shutdown(): name = eye_status_names[index % len(eye_status_names)] - rospy.loginfo("eye_status: {}".format(name)) - pub.publish(String(name)) + rospy.loginfo("Publishing eye_status: {}".format(name)) + [pub.publish(String(name)) for pub in pubs] rate.sleep() index += 1 if __name__ == "__main__": - main() + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/eye_display/node_scripts/ros_to_i2c.py b/eye_display/node_scripts/ros_to_i2c.py new file mode 100644 index 000000000..b35ae4f3b --- /dev/null +++ b/eye_display/node_scripts/ros_to_i2c.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python + +I2C_SLAVE = 0x0703 + +from i2c_for_esp32 import WirePacker # pip3 install i2c-for-esp32 + +import sys +import io +import fcntl +import time + +import rospy +from std_msgs.msg import String +from geometry_msgs.msg import Point + +if sys.hexversion < 0x03000000: + def _b(x): + return x +else: + def _b(x): + return x.encode('latin-1') + +class I2C: + def __init__(self, device=0x42, bus=0): + self.fr = io.open("/dev/i2c-"+str(bus), "rb", buffering=0) + self.fw = io.open("/dev/i2c-"+str(bus), "wb", buffering=0) + # set device address + fcntl.ioctl(self.fr, I2C_SLAVE, device) + fcntl.ioctl(self.fw, I2C_SLAVE, device) + + def write(self, data): + if type(data) is list: + data = bytearray(data) + elif type(data) is str: + data = _b(data) + self.fw.write(data) + self.fw.flush() + + def read(self, count): + return self.fr.read(count) + + def close(self): + self.fw.close() + self.fr.close() + +def send_data_to_i2c(data): + global i2c + sent_str = str(data) + packer = WirePacker(buffer_size=len(sent_str) + 8) + for s in sent_str: + packer.write(ord(s)) + packer.end() + if packer.available(): + packet = packer.buffer[:packer.available()] + rospy.loginfo("[{}]: {}".format(rospy.get_name(), packet.decode(errors='replace'))) + try: + i2c.write(packet) + except OSError as e: + rospy.logerr(e) + time.sleep(0.02) + +def eye_status_cb(msg): + rospy.loginfo("[{}]: eye_status: {}".format(rospy.get_name(), msg.data)) + send_data_to_i2c("eye_status: {}".format(msg.data)) + +def look_at_cb(msg): + ropsy.loginfo("[{}]: look_at: {} {}".format(rospy.get_name(), msg.x, msg.y)) + send_data_to_i2c("look_at: {} {}".format(msg.x, msg.y)) + +def main(): + global i2c + rospy.init_node('eye_display') + + # setup i2c settings + slave = rospy.get_param("i2c_slave", I2C_SLAVE) + device = int(rospy.get_param("~i2c_device", "0x42"), 16) + bus = rospy.get_param("~i2c_bus", 0) + rospy.loginfo("open i2C slave={:#x}, device={:#x}, bus={}".format(slave, device, bus)) + i2c = I2C(device=device, bus=bus) + + # get mode/direction param + mode_right = rospy.get_param("~mode_right", None) + if mode_right: + if isinstance(mode_right, bool): + send_data_to_i2c("mode_right: {}".format(mode_right)) + else: + rospy.logwarn("Invalid mode_right ({})".format(mode_right)) + + direction = rospy.get_param("~direction", None) + if direction: + if isinstance(direction, int): + send_data_to_i2c("direction: {}".format(direction)) + else: + rospy.logwarn("Invalid direction ({})".format(direction)) + + # get eye_asset param + eye_asset_names = rospy.get_param("~eye_asset/names") + send_data_to_i2c("eye_asset_names: {}".format(', '.join(eye_asset_names))) + + for name in eye_asset_names: + eye_asset_params = rospy.get_param("~eye_asset/{}".format(name)) + for key, value in eye_asset_params.items(): + # image path : cehck if key start with path_ + if key.startswith("path_"): + eye_type = key[len("path_"):] + send_data_to_i2c("eye_asset_image_path: {}: {}: {}".format(name, eye_type, value)) + # position* + if "_position" in key: + eye_type = key[:key.find("_position")] + if isinstance(value, list): + value = ", ".join(map(str, value)) + send_data_to_i2c("eye_asset_{}: {}: {}: {}".format(key[len(eye_type)+1:], name, eye_type, value)) + # default_* + if "_default" in key: + eye_type = key[:key.find("_default")] + send_data_to_i2c("eye_asset_{}: {}: {}: {}".format(key[len(eye_type)+1:], name, eye_type, value)) + send_data_to_i2c("eye_asset_done:\n"); + + # start subscriber + rospy.Subscriber('~eye_status', String, eye_status_cb) + rospy.Subscriber('~look_at', Point, look_at_cb) + rospy.spin() + +if __name__ == "__main__": + main() diff --git a/eye_display/platformio.ini b/eye_display/platformio.ini index 99de68e23..a99562d51 100644 --- a/eye_display/platformio.ini +++ b/eye_display/platformio.ini @@ -11,6 +11,8 @@ [env:stampc3] platform = espressif32 framework = arduino +;; Use https://github.com/espressif/arduino-esp32/pull/8187 +platform_packages = framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#2.0.12 board = esp32dev board_build.mcu = esp32c3 board_build.partitions = huge_app.csv @@ -25,7 +27,7 @@ build_flags = -DCORE_DEBUG_LEVEL=2 -DSTAMPC3 lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 + lovyan03/LovyanGFX @ 1.2.7 bblanchon/ArduinoJson @ ^6.19.4 [env:stamps3] @@ -43,12 +45,13 @@ build_flags = -DARDUINO_USB_MODE=1 -DSTAMPS3 lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 - bblanchon/ArduinoJson @ ^6.19.4 + lovyan03/LovyanGFX @ 1.2.7 [env:stampc3-ros] platform = espressif32 framework = arduino +;; Use https://github.com/espressif/arduino-esp32/pull/8187 +platform_packages = framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#2.0.12 board = esp32dev board_build.mcu = esp32c3 board_build.partitions = huge_app.csv @@ -59,13 +62,12 @@ board_build.flash_mode = dio board_build.arduino.ldscript = esp32c3_out.ld monitor_speed = 115200 monitor_filters = time -build_flags = +build_flags = -DCORE_DEBUG_LEVEL=2 -DSTAMPC3 -DUSE_ROS lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 - bblanchon/ArduinoJson @ ^6.19.4 + lovyan03/LovyanGFX @ 1.2.7 [env:stamps3-ros] platform = espressif32 @@ -83,35 +85,13 @@ build_flags = -DSTAMPS3 -DUSE_ROS lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 - bblanchon/ArduinoJson @ ^6.19.4 - -[env:stampc3-i2c-right] -platform = espressif32 -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 -monitor_speed = 115200 -monitor_filters = time -build_flags = - -DCORE_DEBUG_LEVEL=2 - -DSTAMPC3 - -DUSE_I2C - -DEYE_RIGHT -lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 - iory/i2c-for-esp32 @ 0.4.1 - bblanchon/ArduinoJson @ ^6.19.4 + lovyan03/LovyanGFX @ 1.2.7 -[env:stampc3-i2c-left] +[env:stampc3-i2c] platform = espressif32 framework = arduino +;; Use https://github.com/espressif/arduino-esp32/pull/8187 +platform_packages = framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#2.0.12 board = esp32dev board_build.mcu = esp32c3 board_build.partitions = huge_app.csv @@ -127,11 +107,10 @@ build_flags = -DSTAMPC3 -DUSE_I2C lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 + lovyan03/LovyanGFX @ 1.2.7 iory/i2c-for-esp32 @ 0.4.1 - bblanchon/ArduinoJson @ ^6.19.4 -[env:stamps3-i2c-right] +[env:stamps3-i2c] platform = espressif32 framework = arduino ;; Use https://github.com/espressif/arduino-esp32/pull/8187 @@ -146,30 +125,7 @@ build_flags = -DARDUINO_USB_MODE=1 -DSTAMPS3 -DUSE_I2C - -DEYE_RIGHT lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 + lovyan03/LovyanGFX @ 1.2.7 iory/i2c-for-esp32 @ 0.4.1 - bblanchon/ArduinoJson @ ^6.19.4 - -[env:stamps3-i2c-left] -platform = espressif32 -framework = arduino -;; Use https://github.com/espressif/arduino-esp32/pull/8187 -platform_packages = framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#2.0.12 -board = m5stack-stamps3 -monitor_speed = 115200 -;; USB is HWCDC for ESP32 S3 -;; So we need to enable it -;; See https://lang-ship.com/blog/work/m5stack-atoms3-2/ -build_flags = - -DARDUINO_USB_CDC_ON_BOOT=1 - -DARDUINO_USB_MODE=1 - -DSTAMPS3 - -DUSE_I2C - -DEYE_RIGHT -lib_deps = - lovyan03/LovyanGFX @ ^1.1.2 - iory/i2c-for-esp32 @ 0.4.1 - bblanchon/ArduinoJson @ ^6.19.4 diff --git a/eye_display/src/main.cpp b/eye_display/src/main.cpp index 49cb3e0e8..159d12849 100644 --- a/eye_display/src/main.cpp +++ b/eye_display/src/main.cpp @@ -20,41 +20,18 @@ const int image_width = 139; const int image_height = 139; -EyeManagerIO eye = EyeManagerIO(); +EyeManager eye = EyeManager(); + +std::string eye_asset_text = + "eye_asset_names: normal, blink, happy\n" + "eye_asset_position: normal: upperlid: 9\n" + "eye_asset_position: blink: upperlid: 9, 9, 130, 130, 9, 9\n" + "eye_asset_image_path: happy: iris: /white.jpg\n" + "eye_asset_image_path: happy: pupil: /white.jpg\n" + "eye_asset_image_path: happy: reflex: /white.jpg\n" + "eye_asset_image_path: happy: upperlid: /reflex_happy.jpg\n" + "eye_asset_position: happy: upperlid: 130, 131, 132, 133, 134, 135\n"; -#if defined(USE_ROS) // defien callback -//ros::Subscriber sub_point("~look_at", [&](const geometry_msgs::Point &msg){eye.set_gaze_direction((float)msg.x, (float)msg.y);}); -//ros::Subscriber sub_eye_status("~eye_status", [&](const std_msgs::String &msg){eye.set_emotion(msg.data);}); -#endif -#if !defined(USE_I2C) && !defined(USE_ROS) // sample code for eye asset without ROS/I2C -void setup_asset(EyeManager& eye) // returns initial status -{ - std::map& eye_asset_map = eye.eye_asset_map; - eye_asset_map["normal"] = EyeAsset(); - eye_asset_map["normal"].name = "normal"; - eye_asset_map["normal"].upperlid_position = {9}; - eye_asset_map["normal"].direction = 1; - eye_asset_map["normal"].direction = true; - - eye_asset_map["blink"] = EyeAsset(); - eye_asset_map["blink"].name = "blink"; - eye_asset_map["blink"].upperlid_position = {9, 9, 130, 130, 9, 9}; - eye_asset_map["blink"].direction = 1; - eye_asset_map["blink"].direction = true; - - eye_asset_map["happy"] = EyeAsset(); - eye_asset_map["happy"].name = "happy"; - eye_asset_map["happy"].path_iris = "/white.jpg"; - eye_asset_map["happy"].path_pupil = "/white.jpg"; - eye_asset_map["happy"].path_reflex = "/white.jpg"; - eye_asset_map["happy"].path_upperlid = "/reflex_happy.jpg"; - eye_asset_map["happy"].upperlid_position = {130, 131, 132, 133, 134, 135}; - eye_asset_map["happy"].direction = 1; - eye_asset_map["happy"].direction = true; - - eye.set_emotion("happy"); -} -#endif void setup() { @@ -64,23 +41,22 @@ void setup() SPIFFS.begin(); Serial.begin(115200); - // initialize eye manager with current eye asset - eye.init(); - // draw eye image - eye.update_look(); - #if defined(USE_ROS) // USE_ROS setup_ros(); +#elif defined(USE_I2C) + setup_i2c(); #endif + // initialize eye manager with current eye asset + eye.init(); // initialize eye_asset_map and set current_eye_status - setup_asset(eye); + eye.setup_asset(eye_asset_text); + // draw eye image eye.update_look(); -#if defined(USE_I2C) - setup_i2c(); -#endif + unsigned long next_time = eye.update_next_time(); + loginfo("[%8ld] setup() done: next_time = %ld", next_time); } void loop() @@ -88,15 +64,15 @@ void loop() #if defined(USE_ROS) // USE_ROS reconnect_ros(eye); #endif - delay(100); + long sleep_time = eye.delay_next_time(); // update emotion, this calls update_look to display int frame = eye.update_emotion(); #if defined(USE_ROS) nh.spinOnce(); - nh.loginfo("Eye status: %s (%d)", eye.get_emotion().c_str(), frame); #endif + loginfo("[%8ld] Eye status: %s (%d) (sleep %ld ms)", millis(), eye.get_emotion().c_str(), frame, sleep_time); #if !defined(USE_I2C) && !defined(USE_ROS) // sample code for eye asset static float look_x = 0;