- Removed the need for _extra in eye_asset/name. Introduced asset_param_list.py to provide a list of parameters, since rosserial_client does not support param_list.

k-okada · k-okada · commit de4a44457a96 · 2025-08-03T16:49:48.000+09:00
- Added support for iris_zoom (controls iris and pupil reflex), upperlid_zoom, and extra_zoom.
diff --git a/eye_display/CMakeLists.txt b/eye_display/CMakeLists.txt
@@ -9,5 +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/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
@@ -146,10 +146,12 @@ If you need more than the standard images (outline, iris, pupil, reflex, upperli
   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
@@ -183,6 +185,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
diff --git a/eye_display/include/node_handle_ex.h b/eye_display/include/node_handle_ex.h
@@ -18,7 +18,6 @@ class NodeHandleEx : public NodeHandle_<Hardware, MAX_SUBSCRIBERS, MAX_PUBLISHER
     return super::getParam(name, param, length, timeout);
   }
   bool getParam(const char* name, float* param, int length = 1, int timeout = 1000) {
-    logwarn("Failed to get param: length mismatch float");
     return super::getParam(name, param, length, timeout);
   }
   bool getParam(const char* name, char** param, int length = 1, int timeout = 1000) {
@@ -56,6 +55,16 @@ class NodeHandleEx : public NodeHandle_<Hardware, MAX_SUBSCRIBERS, MAX_PUBLISHER
           param[i] = super::req_param_resp.ints[i];
       return true;
   }
+  bool getParam(const char *name, std::vector<float> &param, 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
@@ -37,6 +37,7 @@ extern EyeManager eye;
 
 ros::Subscriber<geometry_msgs::Point> sub_point("~look_at", &callback_look_at);
 ros::Subscriber<std_msgs::String> 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);
@@ -49,13 +50,6 @@ void callback_emotion(const std_msgs::String &msg)
 std::string ros_read_asset()
 {
   std::map<std::string, EyeAsset>& eye_asset_map = eye.eye_asset_map;
-  while (not nh.connected())
-  {
-    nh.spinOnce();
-    delay(1000);
-  }
-  delay(500);  // wait 0.5 sec before reading asset
-
   std::ostringstream oss;
 
   bool mode_right;
@@ -77,94 +71,102 @@ std::string ros_read_asset()
   oss << "eye_asset_names: ";
   for (auto it = eye_asset_names.begin(); it != eye_asset_names.end(); ++it) {
     std::string name = *it;
-    std::string suffix = "_extra";
-    if (name.size() >= suffix.size() &&
-        name.rfind(suffix) == name.size() - suffix.size()) {
-        name = name.substr(0, name.size() - suffix.size());
-    }
     oss << name;
     if (std::next(it) != eye_asset_names.end()) oss << ", ";
   }
   oss << "\n";
   //
   for(auto name: eye_asset_names) {
     char eye_asset_map_key[256];
-    bool have_extra = false;
-    std::string suffix = "_extra";
-    if (name.size() >= suffix.size() &&
-        name.rfind(suffix) == name.size() - suffix.size()) {
-        name = name.substr(0, name.size() - suffix.size());
-        have_extra = true;
-    }
-    // path_upperlid
-    for(const std::string& type: {"upperlid", "outline", "iris", "pupil", "reflex"}) {
+
+    // path
+    std::vector<std::string> 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";
       }
     }
-    // upperlid_position, upperlid_default_pos_x, upperlid_default_pos_y, upperlid_default_theta
-    std::vector<int> position;
-    snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_position", name.c_str());
-    if (nh.getParam(eye_asset_map_key, position, 300)) {  // timeout == 300
-      oss << "eye_asset_position: " << name << ": upperlid: " << joinVector(position) << "\n";
-    } else { // v2 API, upperlid_position_x, upperlid_position_y, upperlid_rotate_theta instead of upperlid_position
-      nh.logwarn("Using v2 API (position_x, position_y, rotation_theta). Please ignore the above 'Parameter %sn does not exist' message", eye_asset_map_key);
-      std::vector<int> position_x, position_y, rotation_theta;
-      snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_position_x", name.c_str());
-      if (nh.getParam(eye_asset_map_key, position_x)) {
-        oss << "eye_asset_position_x: " << name << ": upperlid: " << joinVector(position_x) << "\n";
-      }
-      snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_position_y", name.c_str());
-      if (nh.getParam(eye_asset_map_key, position_y)) {
-        oss << "eye_asset_position_y: " << name << ": upperlid: " << joinVector(position_y) << "\n";
-      }
-      snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_rotation_theta", name.c_str());
-      if (nh.getParam(eye_asset_map_key, rotation_theta)) {
-        oss << "eye_asset_rotation_theta: " << name << ": upperlid: " << joinVector(rotation_theta) << "\n";
-      }
-    } //v2 API
-    for(const std::string& pos: {"default_pos_x", "default_pos_y", "default_theta"}) {
-      int data;
-      snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/upperlid_%s", name.c_str(), pos.c_str());
-      if (nh.getParam(eye_asset_map_key, &data)) {
-        oss << "eye_asset_" << pos << ": " << name << ": upperlid: " << data << "\n";
+
+    // position
+    std::vector<std::string> 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<int> 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);
+        }
       }
     }
-    // v3 API (extra images)
-    if (have_extra) {
-      for(const std::string& type: {"extra1", "extra2"}) {
-        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, 300)) {  // timeout == 300
-          oss << "eye_asset_image_path: " << name << ": " << type << ": " << path << "\n";
-        }
-        std::vector<int> position_x, position_y, rotation_theta;
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_position_x", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, position_x)) {
-          oss << "eye_asset_position_x: " << name << ": " << type << ": " << joinVector(position_x) << "\n";
-        }
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_position_y", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, position_y)) {
-          oss << "eye_asset_position_y: " << name << ": " << type << ": " << joinVector(position_y) << "\n";
-        }
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_rotation_theta", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, rotation_theta)) {
-          oss << "eye_asset_rotation_theta: " << name << ": " << type << ": " << joinVector(rotation_theta) << "\n";
-        }
-        int data;
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_default_pos_x", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, &data)) {
-          oss << "eye_asset_default_pos_x: " << name << ": " << type << ": " << data << "\n";
+
+    // rotation
+    std::vector<std::string> 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<int> 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);
         }
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_default_pos_y", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, &data)) {
-          oss << "eye_asset_default_pos_y: " << name << ": " << type << ": " << data << "\n";
+      }
+    }
+
+    // default
+    std::vector<std::string> 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;
+      std::vector<int> 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);
         }
-        snprintf(eye_asset_map_key, sizeof(eye_asset_map_key), "~eye_asset/%s/%s_default_theta", name.c_str(), type.c_str());
-        if (nh.getParam(eye_asset_map_key, &data)) {
-          oss << "eye_asset_default_theta: " << name << ": " << type << ": " << data << "\n";
+      }
+    }
+
+    // zoom
+    std::vector<std::string> 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<float> 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);
         }
       }
     }
diff --git a/eye_display/launch/demo.launch b/eye_display/launch/demo.launch
@@ -14,21 +14,34 @@
         type="serial_node.py"
         name="eye_display"
         output="screen"
-	unless="$(arg use_i2c)"
+        unless="$(arg use_i2c)"
         >
         <param name="port" value="$(arg port)" />
         <param name="baud" value="$(arg baud)" />
         <param name="mode_right" value="$(arg mode_right)" />
         <param name="direction" value="$(arg direction)" />
         <rosparam command="load" ns="eye_asset" file="$(arg eye_asset_file)" />
-        <param name="eye_asset_text" command="cat $(arg eye_asset_file)" />
+    </node>
+    <!-- The namespace (ns) used by asset_param_list must match the
+         node name of serial_node.py. If you rename node name of
+         serial_node.py, make sure to update the namespace in
+         asset_param_list.py accordingly to ensure proper parameter
+         retrieval. -->
+    <node
+        ns="eye_display"
+        pkg="eye_display"
+        type="asset_param_list.py"
+        name="asset_param_list"
+        output="screen"
+        unless="$(arg use_i2c)"
+        >
     </node>
     <node
         pkg="eye_display"
         type="ros_to_i2c.py"
         name="eye_display"
         output="screen"
-	if="$(arg use_i2c)"
+        if="$(arg use_i2c)"
         >
         <param name="i2c_device" value="$(arg i2c_device)" />
         <param name="i2c_bus" value="$(arg i2c_bus)" />
diff --git a/eye_display/node_scripts/asset_param_list.py 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/ros_to_i2c.py b/eye_display/node_scripts/ros_to_i2c.py
@@ -94,9 +94,7 @@ def main():
          rospy.logwarn("Invalid direction ({})".format(direction))
 
    # get eye_asset param
-   eye_asset_names_raw = rospy.get_param("~eye_asset/names")
-   # remove '_extra' suffix when setting eye_asset_names.
-   eye_asset_names = [s[:-len("_extra")] if s.endswith("_extra") else s for s in eye_asset_names_raw]
+   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:
@@ -125,6 +123,3 @@ def main():
 
 if __name__ == "__main__":
     main()
-
-
-
