Feature/sdp uwb waypoint

.github/workflows/clang_format.yml

@@ -20,5 +20,5 @@ jobs:
         git diff --exit-code
     - name: Check Clang Format for sketches
       run: |
-        find arduino_lib -name '*.cpp' -o -name '*.h' | xargs clang-format -i
+        find sketchbooks -name '*.cpp' -o -name '*.h' | xargs clang-format -i
         git diff --exit-code

.github/workflows/platformio.yml

@@ -28,10 +28,13 @@ jobs:
           - sdp_elevator_status_broadcaster
           - sdp_example
           - sdp_landmark_information
+          - sdp_rpc_server_example
+          - sdp_rpc_client_example
           - sdp_sesami_host
           - sdp_stickv2_interface
           - sdp_switchbot_light
           - sdp_switchbot_lock
+          - sdp_uwb_waypoint
           - smart_device_protocol_interface
     env:
       CATKIN_WS: /home/runner/work/catkin_ws

.github/workflows/platformio_full.yml

@@ -24,10 +24,13 @@ jobs:
           - sdp_elevator_status_broadcaster
           - sdp_example
           - sdp_landmark_information
+          - sdp_rpc_server_example
+          - sdp_rpc_client_example
           - sdp_sesami_host
           - sdp_stickv2_interface
           - sdp_switchbot_light
           - sdp_switchbot_lock
+          - sdp_uwb_waypoint
           - smart_device_protocol_interface
     env:
       CATKIN_WS: /home/runner/work/catkin_ws

.github/workflows/python_black.yml

@@ -12,5 +12,3 @@ jobs:
     steps:
       - uses: actions/checkout@v2
       - uses: psf/black@stable
-        with:
-          src: "./python ./node_scripts"

CMakeLists.txt

@@ -15,6 +15,7 @@ catkin_python_setup()
 add_message_files(
   FILES
   Packet.msg
+  PacketType.msg
   UWBDistance.msg
 )
 

arduino_lib/devices/m5stack_servo_module.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <Arduino.h>
+#include <M5Stack.h>
+#include <Wire.h>
+
+#define SERVO_ADDR 0x53
+
+void init_servo_module(bool init_wire = true) {
+  if (init_wire) {
+    Wire.begin(21, 22, 100000UL);
+  }
+}
+
+// addr 0x01 mean control the number 1 servo by us
+void Servo_write_us(uint8_t number, uint16_t us) {
+  Wire.beginTransmission(SERVO_ADDR);
+  Wire.write(0x00 | number);
+  Wire.write(us & 0x00ff);
+  Wire.write(us >> 8 & 0x00ff);
+  Wire.endTransmission();
+}
+
+// addr 0x11 mean control the number 1 servo by angle
+void Servo_write_angle(uint8_t number, float servo_angle) {
+  uint8_t angle;
+  if (servo_angle < 0) {
+    angle = 0;
+  } else if (servo_angle > 180) {
+    angle = 180;
+  } else {
+    angle = (uint8_t)servo_angle;
+  }
+  Wire.beginTransmission(SERVO_ADDR);
+  Wire.write(0x10 | number);
+  Wire.write(angle);
+  Wire.endTransmission();
+}

arduino_lib/devices/m5stack_tof_unit.h

@@ -0,0 +1,150 @@
+/*
+ * From https://github.com/m5stack/M5Stack/blob/master/examples/Unit/ToF_VL53L0X/ToF_VL53L0X.ino
+ */
+
+#include <Arduino.h>
+#if defined(M5STACK_FIRE)
+#include <M5Stack.h>
+#elif defined(M5STACK_CORE2)
+#include <M5Core2.h>
+#endif
+
+#include <optional>
+
+#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0xc0
+#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0xc2
+#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
+#define VL53L0X_REG_SYSRANGE_START 0x00
+#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x13
+#define VL53L0X_REG_RESULT_RANGE_STATUS 0x14
+#define VL53L0X_I2C_address 0x29  // I2C VL53L0X_I2C_address
+
+byte gbuf[16];
+
+uint16_t _bswap(byte b[]) {
+  // Big Endian unsigned short to little endian unsigned short
+  uint16_t val = ((b[0] << 8) & b[1]);
+  return val;
+}
+
+uint16_t _makeuint16(int lsb, int msb) {
+  return ((msb & 0xFF) << 8) | (lsb & 0xFF);
+}
+
+void _write_byte_data(byte data) {
+  Wire.beginTransmission(VL53L0X_I2C_address);
+  Wire.write(data);
+  Wire.endTransmission();
+}
+
+void _write_byte_data_at(byte reg, byte data) {
+  // write data word at VL53L0X_I2C_address and register
+  Wire.beginTransmission(VL53L0X_I2C_address);
+  Wire.write(reg);
+  Wire.write(data);
+  Wire.endTransmission();
+}
+
+void _write_word_data_at(byte reg, uint16_t data) {
+  // write data word at VL53L0X_I2C_address and register
+  byte b0 = (data & 0xFF);
+  byte b1 = ((data >> 8) && 0xFF);
+
+  Wire.beginTransmission(VL53L0X_I2C_address);
+  Wire.write(reg);
+  Wire.write(b0);
+  Wire.write(b1);
+  Wire.endTransmission();
+}
+
+byte _read_byte_data() {
+  Wire.requestFrom(VL53L0X_I2C_address, 1);
+  while (Wire.available() < 1) delay(1);
+  byte b = Wire.read();
+  return b;
+}
+
+byte _read_byte_data_at(byte reg) {
+  // _write_byte_data((byte)0x00);
+  _write_byte_data(reg);
+  Wire.requestFrom(VL53L0X_I2C_address, 1);
+  while (Wire.available() < 1) delay(1);
+  byte b = Wire.read();
+  return b;
+}
+
+uint16_t _read_word_data_at(byte reg) {
+  _write_byte_data(reg);
+  Wire.requestFrom(VL53L0X_I2C_address, 2);
+  while (Wire.available() < 2) delay(1);
+  gbuf[0] = Wire.read();
+  gbuf[1] = Wire.read();
+  return _bswap(gbuf);
+}
+
+void _read_block_data_at(byte reg, int sz) {
+  int i = 0;
+  _write_byte_data(reg);
+  Wire.requestFrom(VL53L0X_I2C_address, sz);
+  for (i = 0; i < sz; i++) {
+    while (Wire.available() < 1) delay(1);
+    gbuf[i] = Wire.read();
+  }
+}
+
+uint16_t _VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
+  // Converts the encoded VCSEL period register value into the real
+  // period in PLL clocks
+  uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
+  return vcsel_period_pclks;
+}
+
+bool init_m5stack_tof_unit() {
+  Wire.begin();
+  return true;
+}
+
+std::optional<std::tuple<uint16_t, uint16_t, uint16_t, byte>> get_m5stack_tof_unit_data() {
+  _write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
+
+  byte val = 0;
+  int cnt = 0;
+  while (cnt < 100) {  // 1 second waiting time max
+    delay(10);
+    val = _read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
+    if (val & 0x01) break;
+    cnt++;
+  }
+  if (val & 0x01) {
+    // Serial.println("ready");
+  } else {
+    // Serial.println("not ready");
+    return std::nullopt;
+  }
+  _read_block_data_at(0x14, 12);
+  uint16_t acnt = _makeuint16(gbuf[7], gbuf[6]);
+  uint16_t scnt = _makeuint16(gbuf[9], gbuf[8]);
+  uint16_t dist = _makeuint16(gbuf[11], gbuf[10]);
+  byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);
+  return std::make_tuple(acnt, scnt, dist, DeviceRangeStatusInternal);
+}
+
+// void loop() {
+//   _read_block_data_at(0x14, 12);
+//   uint16_t acnt = _makeuint16(gbuf[7], gbuf[6]);
+//   uint16_t scnt = _makeuint16(gbuf[9], gbuf[8]);
+//   uint16_t dist = _makeuint16(gbuf[11], gbuf[10]);
+//   byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);
+//   M5.Lcd.fillRect(0, 35, 319, 239, BLACK);
+//   M5.Lcd.setCursor(0, 35, 4);
+//   M5.Lcd.print("ambient count: ");
+//   M5.Lcd.println(acnt);
+//   M5.Lcd.print("signal count: ");
+//   M5.Lcd.println(scnt);
+//   M5.Lcd.print("distance: ");
+//   M5.Lcd.println(dist);
+//   M5.Lcd.print("status: ");
+//   M5.Lcd.println(DeviceRangeStatusInternal);
+//   delay(1000);
+// }

arduino_lib/devices/stickv2_util.h

@@ -10,6 +10,7 @@ bool send_data_to_serial(HardwareSerial &serial,
                          StaticJsonDocument<BUFSIZE> &doc) {
   String request;
   serializeJson(doc, request);
+  Serial.printf("Send data: %s\n", request.c_str());
   serial.println(request);
   return true;
 }
@@ -32,4 +33,51 @@ bool set_object_recognition_model(HardwareSerial &serial,
   return send_data_to_serial(serial, doc);
 }
 
+bool set_face_recognition(HardwareSerial &serial) {
+  StaticJsonDocument<BUFSIZE> doc;
+  doc["function"] = "face_recognition";
+  JsonArray args = doc.createNestedArray("args");
+  return send_data_to_serial(serial, doc);
+}
+
+//
+// Data Parser
+//
+int parse_object_recognition_response(const StaticJsonDocument<BUFSIZE> &doc,
+                                      const String &target_class,
+                                      float threashold) {
+  if (doc.containsKey("num") and doc.containsKey("obj")) {
+    int num_of_target = 0;
+    long num_of_objects = doc["num"];
+    for (int i = 0; i < num_of_objects; i++) {
+      // Serial.printf(" %d th object: %s, %f\n",
+      //               i,
+      //               doc["obj"][i]["type"].as<String>().c_str(),
+      //               doc["obj"][i]["prob"].as<float>());
+      if (doc["obj"][i]["type"] == target_class and doc["obj"][i]["prob"].as<float>() > threashold) {
+        ++num_of_target;
+      }
+    }
+    return num_of_target;
+  } else {
+    return -1;
+  }
+}
+
+bool parse_face_recognition_response(const StaticJsonDocument<BUFSIZE> &doc,
+                                     float threshold_prob, float threshold_match, std::vector<String> &names) {
+  if (doc.containsKey("num") and doc.containsKey("face")) {
+    names.clear();
+    long num_of_faces = doc["num"];
+    for (int i = 0; i < num_of_faces; i++) {
+      if (doc["face"][i]["prob"].as<float>() > threshold_prob and doc["face"][i]["match_prob"].as<float>() > threshold_match) {
+        names.push_back(doc["face"][i]["name"].as<String>());
+      }
+    }
+    return true;
+  } else {
+    return false;
+  }
+}
+
 #endif

arduino_lib/m5stack_utils/common.h

@@ -0,0 +1,12 @@
+#pragma once
+
+enum M5StackPort {
+  PORT_A,
+  PORT_B,
+  PORT_C,
+};
+
+struct M5StackSerialPortInfo {
+  int rx;
+  int tx;
+};

arduino_lib/m5stack_utils/m5atomlite.h

@@ -0,0 +1,9 @@
+#pragma once
+#include "m5stack_utils/common.h"
+
+#define PORT_A_SERIAL_RX 32
+#define PORT_A_SERIAL_TX 26
+
+const M5StackSerialPortInfo M5StackSerialPortInfoList[] = {
+    {PORT_A_SERIAL_RX, PORT_A_SERIAL_TX},
+};

arduino_lib/m5stack_utils/m5atoms3.h

@@ -0,0 +1,9 @@
+#pragma once
+#include "m5stack_utils/common.h"
+
+#define PORT_A_SERIAL_RX 1
+#define PORT_A_SERIAL_TX 2
+
+const M5StackSerialPortInfo M5StackSerialPortInfoList[] = {
+    {PORT_A_SERIAL_RX, PORT_A_SERIAL_TX},
+};

arduino_lib/m5stack_utils/m5core2.h

@@ -0,0 +1,17 @@
+#pragma once
+#include "m5stack_utils/common.h"
+
+#define PORT_A_SERIAL_RX 33
+#define PORT_A_SERIAL_TX 32
+
+#define PORT_B_SERIAL_RX 26
+#define PORT_B_SERIAL_TX 36
+
+#define PORT_C_SERIAL_RX 13
+#define PORT_C_SERIAL_TX 14
+
+const M5StackSerialPortInfo M5StackSerialPortInfoList[] = {
+    {PORT_A_SERIAL_RX, PORT_A_SERIAL_TX},
+    {PORT_B_SERIAL_RX, PORT_B_SERIAL_TX},
+    {PORT_C_SERIAL_RX, PORT_C_SERIAL_TX},
+};

arduino_lib/m5stack_utils/m5paper.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "m5stack_utils/common.h"
+
+#define PORT_A_SERIAL_RX 32
+#define PORT_A_SERIAL_TX 25
+
+#define PORT_B_SERIAL_RX 33
+#define PORT_B_SERIAL_TX 26
+
+#define PORT_C_SERIAL_RX 19
+#define PORT_C_SERIAL_TX 18
+
+const M5StackSerialPortInfo M5StackSerialPortInfoList[] = {
+    {PORT_A_SERIAL_RX, PORT_A_SERIAL_TX},
+    {PORT_B_SERIAL_RX, PORT_B_SERIAL_TX},
+    {PORT_C_SERIAL_RX, PORT_C_SERIAL_TX},
+};

arduino_lib/m5stack_utils/m5stack.h

@@ -0,0 +1,17 @@
+#pragma once
+#include "m5stack_utils/common.h"
+
+#define PORT_A_SERIAL_RX 22
+#define PORT_A_SERIAL_TX 21
+
+#define PORT_B_SERIAL_RX 36
+#define PORT_B_SERIAL_TX 26
+
+#define PORT_C_SERIAL_RX 16
+#define PORT_C_SERIAL_TX 17
+
+const M5StackSerialPortInfo M5StackSerialPortInfoList[] = {
+    {PORT_A_SERIAL_RX, PORT_A_SERIAL_TX},
+    {PORT_B_SERIAL_RX, PORT_B_SERIAL_TX},
+    {PORT_C_SERIAL_RX, PORT_C_SERIAL_TX},
+};