Merge pull request #3 from iory/selected-modes

Add packet type for select mode

bin/display_information.py

@@ -16,6 +16,7 @@
 from riberry.battery import MP2760BatteryMonitor
 from riberry.battery import PisugarBatteryReader
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 from riberry.network import get_ip_address
 from riberry.network import get_mac_address
 from riberry.network import get_ros_master_ip
@@ -203,9 +204,8 @@ def display_image(self, img):
         jpg_size = len(jpg_img)
 
         header = []
-        header += [0xFF, 0xD8, 0xEA]
+        header += [PacketType.JPEG]
         header += [(jpg_size & 0xFF00) >> 8, (jpg_size & 0x00FF) >> 0]
-
         packer = WirePacker(buffer_size=1000)
         for h in header:
             packer.write(h)
@@ -218,8 +218,7 @@ def display_image(self, img):
 
         for pack in nsplit(jpg_img, n=50):
             packer.reset()
-            for h in [0xFF, 0xD8, 0xEA]:
-                packer.write(h)
+            packer.write(PacketType.JPEG)
             for h in pack:
                 packer.write(h)
             packer.end()
@@ -261,7 +260,8 @@ def display_information(self):
                     battery_str += "-"
                 else:
                     battery_str += "?"
-        sent_str = f"{ip_str}\n{master_str}\n{battery_str}\n"
+        sent_str = [chr(PacketType.TEXT)]
+        sent_str += f"{ip_str}\n{master_str}\n{battery_str}\n"
 
         if ros_available and ros_additional_message:
             sent_str += f"{ros_additional_message}\n"
@@ -270,7 +270,7 @@ def display_information(self):
         self.send_string(sent_str)
 
     def display_qrcode(self, target_url=None):
-        header = [0x02]
+        header = [PacketType.QR_CODE]
         if target_url is None:
             ip = get_ip_address()
             if ip is None:
@@ -284,7 +284,7 @@ def display_qrcode(self, target_url=None):
         self.send_raw_bytes(header)
 
     def force_mode(self, mode_name):
-        header = [0xFF, 0xFE, 0xFD]
+        header = [PacketType.FORCE_MODE]
         forceModebytes = (list (map(ord, mode_name)))
         self.send_raw_bytes(header + forceModebytes)
 

firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.cpp

@@ -38,7 +38,7 @@ int AtomS3I2C::splitString(const String &input, char delimiter, String output[],
   return index; // 分割された部分の数を返す
 }
 
-// Receive data over I2C and store it in the atoms3lcd instance (color_str, jpegBuf, qrCodeData)
+// Receive data over I2C and store it in the atoms3lcd instance
 // No rendering is done; lcd update is handled by other tasks
 void AtomS3I2C::receiveEvent(int howMany) {
     if (instance->receiveEventEnabled == false)
@@ -49,51 +49,98 @@ void AtomS3I2C::receiveEvent(int howMany) {
     String str;
     // Read data from the I2C bus
     while (0 < WireSlave.available()) {
-        char c = WireSlave.read();  // receive byte as a character
-        // If currently loading JPEG, store the data in jpegBuf
-        if (instance->atoms3lcd.loadingJpeg && str.length() >= 3) {
-            instance->atoms3lcd.jpegBuf[instance->atoms3lcd.currentJpegIndex + str.length() - 3] = c;
-        }
+        char c = WireSlave.read();  // receive byte as a character;
         str += c;
     }
-    // Check for JPEG packet header and update length if found
-    if (instance->atoms3lcd.readyJpeg == false
-        && str.length() == 5 && (str[0] == jpegPacketHeader[0]) && (str[1] == jpegPacketHeader[1]) && (str[2] == jpegPacketHeader[2])) {
-        instance->atoms3lcd.jpegLength = (uint32_t)(str[3] << 8) | str[4];
+
+    // Parse the first byte as PacketType
+    if (str.length() < 1) {
+        return;  // Invalid packet
+    }
+
+    PacketType packetType = static_cast<PacketType>(str[0]);
+    switch (packetType) {
+        case JPEG:
+            handleJpegPacket(str.substring(1));
+            break;
+
+        case QR_CODE:
+            handleQrCodePacket(str);
+            break;
+
+        case FORCE_MODE:
+            handleForceModePacket(str);
+            break;
+
+        case SELECTED_MODE:
+            handleSelectedModePacket(str);
+            break;
+
+        case DISPLAY_BATTERY_GRAPH_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case SERVO_CONTROL_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case PRESSURE_CONTROL_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case TEACHING_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        default:
+            // Handle TEXT or unknown packets
+            instance->atoms3lcd.color_str = str;
+            break;
+    }
+}
+
+void AtomS3I2C::handleJpegPacket(const String& str) {
+    if (str.length() == 2 && !instance->atoms3lcd.readyJpeg) {
+        // Initialize JPEG loading
+        instance->atoms3lcd.jpegLength = (static_cast<uint32_t>(str[0]) << 8) | static_cast<uint8_t>(str[1]);
         instance->atoms3lcd.currentJpegIndex = 0;
         instance->atoms3lcd.loadingJpeg = true;
-        return;
-    }
-    // Continue receiving JPEG data if already in loading state
-    else if (instance->atoms3lcd.loadingJpeg) {
-        if ((str[0] == jpegPacketHeader[0]) && (str[1] == jpegPacketHeader[1]) && (str[2] == jpegPacketHeader[2])) {
-            instance->atoms3lcd.currentJpegIndex += str.length() - 3;
-            // End loading if the entire JPEG is received
-            if (instance->atoms3lcd.currentJpegIndex >= instance->atoms3lcd.jpegLength) {
-                instance->atoms3lcd.loadingJpeg = false;
-                instance->atoms3lcd.readyJpeg = true;
-            }
-            return;
+    } else if (instance->atoms3lcd.loadingJpeg) {
+        size_t index = instance->atoms3lcd.currentJpegIndex;
+        size_t strLength = str.length();
+        // Continue loading JPEG data
+        if (index + strLength <= sizeof(instance->atoms3lcd.jpegBuf)) {
+          memcpy(instance->atoms3lcd.jpegBuf + index, str.c_str(), strLength);
+            instance->atoms3lcd.currentJpegIndex += strLength;
         } else {
-          // Stop loading if a wrong packet is received
-          instance->atoms3lcd.loadingJpeg = false;
+            instance->atoms3lcd.loadingJpeg = false;
+        }
+        if (instance->atoms3lcd.currentJpegIndex >= instance->atoms3lcd.jpegLength) {
+            instance->atoms3lcd.loadingJpeg = false;
+            instance->atoms3lcd.readyJpeg = true;
         }
+    } else {
+        instance->atoms3lcd.loadingJpeg = false;  // Reset if invalid packet received
     }
-    // Check for QR code header and store data if found
-    if (str.length() > 1 && str[0] == qrCodeHeader) {
-        uint8_t qrCodeLength = str[1];  // Assuming the length of the QR code data is in the second byte
+}
+
+void AtomS3I2C::handleQrCodePacket(const String& str) {
+    if (str.length() > 1) {
+        uint8_t qrCodeLength = static_cast<uint8_t>(str[1]);
         instance->atoms3lcd.qrCodeData = str.substring(2, 2 + qrCodeLength);
-        return;
     }
-    // Check for force mode change
-    if (str.length() > 1 && str[0] == forceModeHeader[0] && str[1] == forceModeHeader[1] && str[2] == forceModeHeader[2]) {
-      forcedMode = str.substring(3);
+}
+
+void AtomS3I2C::handleForceModePacket(const String& str) {
+    if (str.length() > 1) {
+        forcedMode = str.substring(1);
     }
-    // Check for selected modes
-    if (str.length() > 1 && str[0] == selectedModesHeader[0] && str[1] == selectedModesHeader[1] && str[2] == selectedModesHeader[2]) {
-      selectedModesStr = str.substring(3);
+}
+
+void AtomS3I2C::handleSelectedModePacket(const String& str) {
+    if (str.length() > 1) {
+        selectedModesStr = str.substring(1);
     }
-    instance->atoms3lcd.color_str = str;
 }
 
 void AtomS3I2C::stopReceiveEvent() {

firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.h

@@ -6,6 +6,8 @@
 #include <atom_s3_lcd.h>
 #include <atom_s3_button.h>
 
+#include "packet.h"
+
 /**
  * @brief Handles I2C communication for AtomS3, including receiving and sending data via I2C bus.
  */
@@ -82,6 +84,10 @@ class AtomS3I2C {
    * @param howMany Number of bytes received.
    */
   static void receiveEvent(int howMany);
+  static void handleJpegPacket(const String& str);
+  static void handleQrCodePacket(const String& str);
+  static void handleForceModePacket(const String& str);
+  static void handleSelectedModePacket(const String& str);
 
   /**
    * @brief Called when the master requests data from the I2C slave.

firmware/atom_s3_i2c_display/lib/atom_s3_i2c/packet.h

@@ -0,0 +1,11 @@
+enum PacketType : uint8_t {
+    TEXT = 0x00,
+    JPEG = 0x01,
+    QR_CODE = 0x02,
+    FORCE_MODE = 0x03,
+    SELECTED_MODE = 0x04,
+    DISPLAY_BATTERY_GRAPH_MODE = 0x05,
+    SERVO_CONTROL_MODE = 0x06,
+    PRESSURE_CONTROL_MODE = 0x07,
+    TEACHING_MODE = 0x08,
+};

riberry/i2c_base.py

@@ -1,3 +1,4 @@
+from enum import IntEnum
 import fcntl
 import sys
 
@@ -114,3 +115,15 @@ def identify_device():
             return "Unknown Device"
         except FileNotFoundError:
             return "Unknown Device"
+
+
+class PacketType(IntEnum):
+    TEXT = 0x00
+    JPEG = 0x01
+    QR_CODE = 0x02
+    FORCE_MODE = 0x03
+    SELECTED_MODE = 0x04
+    DISPLAY_BATTERY_GRAPH_MODE = 0x05
+    SERVO_CONTROL_MODE = 0x06
+    PRESSURE_CONTROL_MODE = 0x07
+    TEACHING_MODE = 0x08

ros/riberry_startup/node_scripts/display_battery_graph_mode.py

@@ -4,11 +4,11 @@
 from std_msgs.msg import String
 
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 
 
 class DisplayBatteryGraphMode(I2CBase):
-    def __init__(self, i2c_addr,
-                 lock_path="/tmp/i2c_display_battery_graph_mode.lock"):
+    def __init__(self, i2c_addr):
         super().__init__(i2c_addr)
 
         self.mode = None
@@ -60,7 +60,8 @@ def timer_callback(self, event):
         self.send_data()
 
     def send_data(self):
-        sent_str = f'{self.charge_status},{self.charge_current},{self.display_duration},'
+        sent_str = [chr(PacketType.DISPLAY_BATTERY_GRAPH_MODE)]
+        sent_str += f'{self.charge_status},{self.charge_current},{self.display_duration},'
         for i in range(self.display_bins):
             percentage = self.battery_percentages[
                 int((i+1)*self.display_duration/self.display_bins)-1]

ros/riberry_startup/node_scripts/pressure_control_mode.py

@@ -9,10 +9,11 @@
 from std_msgs.msg import String
 
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 
 
 class PressureControlMode(I2CBase):
-    def __init__(self, i2c_addr, lock_path="/tmp/i2c_pressure_control_mode.lock"):
+    def __init__(self, i2c_addr):
         super().__init__(i2c_addr)
         # Create robot model to control pressure
         robot_model = RobotModel()
@@ -100,7 +101,8 @@ def read_pressure(self, msg, idx):
     def timer_callback(self, event):
         if self.mode != "PressureControlMode":
             return
-        sent_str = "pressure [kPa]\n"
+        sent_str = chr(PacketType.PRESSURE_CONTROL_MODE)
+        sent_str += "pressure [kPa]\n"
         for idx, value in self.pressures.items():
             if value is None:
                 continue

ros/riberry_startup/node_scripts/servo_control_mode.py

@@ -8,10 +8,11 @@
 from std_msgs.msg import String
 
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 
 
 class ServoControlMode(I2CBase):
-    def __init__(self, i2c_addr, lock_path="/tmp/i2c_servo_control_mode.lock"):
+    def __init__(self, i2c_addr):
         super().__init__(i2c_addr)
         # Create robot model to control servo
         robot_model = RobotModel()
@@ -80,7 +81,8 @@ def timer_callback(self, event):
         if self.mode != "ServoControlMode":
             return
         # Send message on AtomS3 LCD
-        sent_str = "Servo control mode\n\nSingle Click:\nServo on off"
+        sent_str = chr(PacketType.SERVO_CONTROL_MODE)
+        sent_str += "Servo control mode\n\nSingle Click:\nServo on off"
         self.send_string(sent_str)
 
 

ros/riberry_startup/node_scripts/set_mode.py

@@ -3,6 +3,7 @@
 import rospy
 
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 
 
 class SetMode(I2CBase):
@@ -20,7 +21,7 @@ def get_mode_names_from_rosparam(self):
             return ""
 
     def timer_callback(self, event):
-        header = [0xFC, 0xFB, 0xFA]
+        header = [PacketType.SELECTED_MODE]
         forceModebytes = (list (map(ord, self.mode_names)))
         rospy.loginfo_throttle(60, f"Mode names: {self.mode_names}")
         self.send_raw_bytes(header + forceModebytes)

ros/riberry_startup/node_scripts/teaching_mode.py

@@ -9,6 +9,7 @@
 from std_msgs.msg import String
 
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 from riberry.motion_manager import MotionManager
 from riberry.select_list import SelectList
 
@@ -20,7 +21,7 @@ class State(Enum):
 
 
 class TeachingMode(I2CBase):
-    def __init__(self, i2c_addr, lock_path="/tmp/teaching_mode.lock"):
+    def __init__(self, i2c_addr):
         super().__init__(i2c_addr)
         self.motion_manager = MotionManager()
         self.json_dir = os.path.join(os.environ["HOME"], ".ros/riberry")
@@ -98,7 +99,7 @@ def button_cb(self, msg):
     def timer_callback(self, event):
         if self.mode != "TeachingMode":
             return
-        sent_str = ''
+        sent_str = chr(PacketType.TEACHING_MODE)
         if self.state == State.WAIT:
             sent_str += 'Teaching mode\n\n'\
                 + '1tap:\n record\n\n'\