feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/r1/audio/r1_audio_client_example.cpp

@@ -0,0 +1,175 @@
+#include <fstream>
+#include <iostream>
+#include <thread>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/idl/ros2/String_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/r1/audio/audio_client.hpp>
+
+#include "wav.hpp"
+
+#define AUDIO_FILE_PATH "../example/r1/audio/test.wav"
+#define AUDIO_SUBSCRIBE_TOPIC "rt/audio_msg"
+#define GROUP_IP "239.168.123.161"
+#define PORT 5555
+
+#define WAV_SECOND 5  // record seconds
+#define WAV_LEN (16000 * 2 * WAV_SECOND)
+#define WAV_LEN_ONCE (16000 * 2 * 160 / 1000)
+#define CHUNK_SIZE 96000  // 3 seconds
+int sock;
+
+void asr_handler(const void *msg) {
+  std_msgs::msg::dds_::String_ *resMsg = (std_msgs::msg::dds_::String_ *)msg;
+  std::cout << "Topic:\"rt/audio_msg\" recv: " << resMsg->data() << std::endl;
+}
+
+std::string get_local_ip_for_multicast() {
+  struct ifaddrs *ifaddr, *ifa;
+  char host[NI_MAXHOST];
+  std::string result = "";
+
+  getifaddrs(&ifaddr);
+  for (ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) {
+    if (!ifa->ifa_addr || ifa->ifa_addr->sa_family != AF_INET) continue;
+    getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST,
+                NULL, 0, NI_NUMERICHOST);
+    std::string ip(host);
+    if (ip.find("192.168.123.") == 0) {
+      result = ip;
+      break;
+    }
+  }
+  freeifaddrs(ifaddr);
+  return result;
+}
+
+void thread_mic(void) {
+  sock = socket(AF_INET, SOCK_DGRAM, 0);
+  sockaddr_in local_addr{};
+  local_addr.sin_family = AF_INET;
+  local_addr.sin_port = htons(PORT);
+  local_addr.sin_addr.s_addr = INADDR_ANY;
+  bind(sock, (sockaddr *)&local_addr, sizeof(local_addr));
+
+  ip_mreq mreq{};
+  inet_pton(AF_INET, GROUP_IP, &mreq.imr_multiaddr);
+  std::string local_ip = get_local_ip_for_multicast();
+  std::cout << "local ip: " << local_ip << std::endl;
+  mreq.imr_interface.s_addr = inet_addr(local_ip.c_str());
+  setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
+
+  int total_bytes = 0;
+  std::vector<int16_t> pcm_data;
+  pcm_data.reserve(WAV_LEN / 2);
+  std::cout << "start record!" << std::endl;
+  while (total_bytes < WAV_LEN) {
+    char buffer[WAV_LEN_ONCE];
+    ssize_t len = recvfrom(sock, buffer, sizeof(buffer), 0, nullptr, nullptr);
+    if (len > 0) {
+      size_t sample_count = len / 2;
+      const int16_t *samples = reinterpret_cast<const int16_t *>(buffer);
+      pcm_data.insert(pcm_data.end(), samples, samples + sample_count);
+      total_bytes += len;
+    }
+  }
+
+  WriteWave("record.wav", 16000, pcm_data.data(), pcm_data.size(), 1);
+  std::cout << "record finish! save to record.wav " << std::endl;
+}
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: audio_client_example [NetWorkInterface(eth0)]"
+              << std::endl;
+    exit(0);
+  }
+  int32_t ret;
+  /*
+   * Initilaize ChannelFactory
+   */
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+  unitree::robot::r1::AudioClient client;
+  client.Init();
+  client.SetTimeout(10.0f);
+
+  /*ASR message Example*/
+  unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_> subscriber(
+      AUDIO_SUBSCRIBE_TOPIC);
+  subscriber.InitChannel(asr_handler);
+
+  /*Volume Example*/
+  uint8_t volume;
+  ret = client.GetVolume(volume);
+  std::cout << "GetVolume API ret:" << ret
+            << "  volume = " << std::to_string(volume) << std::endl;
+  ret = client.SetVolume(100);
+  std::cout << "SetVolume to 100% , API ret:" << ret << std::endl;
+
+  /*TTS Example*/
+  ret = client.TtsMaker("你好。我是宇树科技的机器人。例程启动成功",
+                        0);  // Auto play
+  std::cout << "TtsMaker API ret:" << ret << std::endl;
+  unitree::common::Sleep(5);
+
+  ret = client.TtsMaker(
+      "Hello. I'm a robot from Unitree Robotics. The example has started "
+      "successfully. ",
+      1);  // Engilsh TTS
+  std::cout << "TtsMaker API ret:" << ret << std::endl;
+  unitree::common::Sleep(8);
+
+  /*Audio Play Example*/
+  int32_t sample_rate = -1;
+  int8_t num_channels = 0;
+  bool filestate = false;
+  std::vector<uint8_t> pcm =
+      ReadWave(AUDIO_FILE_PATH, &sample_rate, &num_channels, &filestate);
+
+  std::cout << "wav file sample_rate = " << sample_rate
+            << " num_channels =  " << std::to_string(num_channels)
+            << " filestate =" << filestate << "filesize = " << pcm.size()
+            << std::endl;
+
+  if (filestate && sample_rate == 16000 && num_channels == 1) {
+    size_t total_size = pcm.size();
+    size_t offset = 0;
+    int chunk_index = 0;
+    std::string stream_id =
+        std::to_string(unitree::common::GetCurrentTimeMillisecond());
+
+    while (offset < total_size) {
+      size_t remaining = total_size - offset;
+      size_t current_chunk_size =
+          std::min(static_cast<size_t>(CHUNK_SIZE), remaining);
+      std::vector<uint8_t> chunk(pcm.begin() + offset,
+                                 pcm.begin() + offset + current_chunk_size);
+      client.PlayStream("example", stream_id, chunk);
+      unitree::common::Sleep(1);
+      std::cout << "Playing size: " << offset << std::endl;
+      offset += current_chunk_size;
+    }
+
+    ret = client.PlayStop(stream_id);  // stop playback after transmission ends
+
+  } else {
+    std::cout << "audio file format error, please check!" << std::endl;
+  }
+
+  /*LED Control Example*/
+  client.LedControl(0, 255, 0);
+  unitree::common::Sleep(1);
+  client.LedControl(0, 0, 0);
+  unitree::common::Sleep(1);
+  client.LedControl(0, 0, 255);
+
+  std::cout << "AudioClient api test finish , asr start..." << std::endl;
+
+  std::thread mic_t(thread_mic);
+
+  while (1) {
+    sleep(1);  // wait for asr message
+  }
+  mic_t.join();
+  return 0;
+}

unitree_sdk2/example/r1/audio/wav.hpp

@@ -0,0 +1,232 @@
+#ifndef __UT_ROBOT_WAV_READER_HPP__
+#define __UT_ROBOT_WAV_READER_HPP__
+
+struct WaveHeader {
+  void SeekToDataChunk(std::istream &is) {
+    while (is && subchunk2_id != 0x61746164) {
+      is.seekg(subchunk2_size, std::istream::cur);
+      is.read(reinterpret_cast<char *>(&subchunk2_id), sizeof(int32_t));
+      is.read(reinterpret_cast<char *>(&subchunk2_size), sizeof(int32_t));
+    }
+  }
+
+  int32_t chunk_id;
+  int32_t chunk_size;
+  int32_t format;
+  int32_t subchunk1_id;
+  int32_t subchunk1_size;
+  int16_t audio_format;
+  int16_t num_channels;
+  int32_t sample_rate;
+  int32_t byte_rate;
+  int16_t block_align;
+  int16_t bits_per_sample;
+  int32_t subchunk2_id;    // a tag of this chunk
+  int32_t subchunk2_size;  // size of subchunk2
+};
+
+static_assert(sizeof(WaveHeader) == 44);
+
+std::vector<uint8_t> ReadWaveImpl(std::istream &is, int32_t *sampling_rate,
+                                  int8_t *channelCount, bool *is_ok) {
+  WaveHeader header{};
+  is.read(reinterpret_cast<char *>(&header.chunk_id), sizeof(header.chunk_id));
+
+  //                        F F I R
+  if (header.chunk_id != 0x46464952) {
+    printf("Expected chunk_id RIFF. Given: 0x%08x\n", header.chunk_id);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.chunk_size),
+          sizeof(header.chunk_size));
+
+  is.read(reinterpret_cast<char *>(&header.format), sizeof(header.format));
+
+  //                      E V A W
+  if (header.format != 0x45564157) {
+    printf("Expected format WAVE. Given: 0x%08x\n", header.format);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.subchunk1_id),
+          sizeof(header.subchunk1_id));
+
+  is.read(reinterpret_cast<char *>(&header.subchunk1_size),
+          sizeof(header.subchunk1_size));
+
+  if (header.subchunk1_id == 0x4b4e554a) {
+    // skip junk padding
+    is.seekg(header.subchunk1_size, std::istream::cur);
+
+    is.read(reinterpret_cast<char *>(&header.subchunk1_id),
+            sizeof(header.subchunk1_id));
+
+    is.read(reinterpret_cast<char *>(&header.subchunk1_size),
+            sizeof(header.subchunk1_size));
+  }
+
+  if (header.subchunk1_id != 0x20746d66) {
+    printf("Expected subchunk1_id 0x20746d66. Given: 0x%08x\n",
+           header.subchunk1_id);
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.subchunk1_size != 16 &&
+      header.subchunk1_size != 18) {  // 16 for PCM
+    printf("Expected subchunk1_size 16. Given: %d\n", header.subchunk1_size);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.audio_format),
+          sizeof(header.audio_format));
+
+  if (header.audio_format != 1) {  // 1 for PCM
+    printf("Expected audio_format 1. Given: %d\n", header.audio_format);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.num_channels),
+          sizeof(header.num_channels));
+
+  *channelCount = (int8_t)header.num_channels;
+
+  is.read(reinterpret_cast<char *>(&header.sample_rate),
+          sizeof(header.sample_rate));
+
+  is.read(reinterpret_cast<char *>(&header.byte_rate),
+          sizeof(header.byte_rate));
+
+  is.read(reinterpret_cast<char *>(&header.block_align),
+          sizeof(header.block_align));
+
+  is.read(reinterpret_cast<char *>(&header.bits_per_sample),
+          sizeof(header.bits_per_sample));
+
+  if (header.byte_rate !=
+      (header.sample_rate * header.num_channels * header.bits_per_sample / 8)) {
+    printf("Incorrect byte rate: %d. Expected: %d", header.byte_rate,
+           (header.sample_rate * header.num_channels * header.bits_per_sample /
+            8));
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.block_align !=
+      (header.num_channels * header.bits_per_sample / 8)) {
+    printf("Incorrect block align: %d. Expected: %d\n", header.block_align,
+           (header.num_channels * header.bits_per_sample / 8));
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.bits_per_sample != 16) {  // we support only 16 bits per sample
+    printf("Expected bits_per_sample 16. Given: %d\n", header.bits_per_sample);
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.subchunk1_size == 18) {
+    int16_t extra_size = -1;
+    is.read(reinterpret_cast<char *>(&extra_size), sizeof(int16_t));
+    if (extra_size != 0) {
+      printf(
+          "Extra size should be 0 for wave from NAudio. Current extra size "
+          "%d\n",
+          extra_size);
+      *is_ok = false;
+      return {};
+    }
+  }
+
+  is.read(reinterpret_cast<char *>(&header.subchunk2_id),
+          sizeof(header.subchunk2_id));
+
+  is.read(reinterpret_cast<char *>(&header.subchunk2_size),
+          sizeof(header.subchunk2_size));
+
+  header.SeekToDataChunk(is);
+  if (!is) {
+    *is_ok = false;
+    return {};
+  }
+
+  *sampling_rate = header.sample_rate;
+
+  // header.subchunk2_size contains the number of bytes in the data.
+  // As we assume each sample contains two bytes, so it is divided by 2 here
+  std::vector<int16_t> samples(header.subchunk2_size / 2);
+
+  is.read(reinterpret_cast<char *>(samples.data()), header.subchunk2_size);
+  if (!is) {
+    *is_ok = false;
+    return {};
+  }
+
+  std::vector<uint8_t> ans(samples.size() * 2);
+  for (int32_t i = 0; i != static_cast<int32_t>(samples.size()); ++i) {
+    ans[i * 2] = samples[i] & 0xFF;
+    ans[i * 2 + 1] = (samples[i] >> 8) & 0xFF;
+  }
+
+  *is_ok = true;
+  return ans;
+}
+
+std::vector<uint8_t> ReadWave(const std::string &filename,
+                              int32_t *sampling_rate, int8_t *channelCount,
+                              bool *is_ok) {
+  std::ifstream is(filename, std::ifstream::binary);
+  auto samples = ReadWaveImpl(is, sampling_rate, channelCount, is_ok);
+  return samples;
+}
+
+bool WriteWave(const std::string &filename, int32_t sampling_rate,
+               const int16_t *samples, int32_t n, uint8_t num_channels) {
+  WaveHeader header{};
+  header.chunk_id = 0x46464952;      // FFIR
+  header.format = 0x45564157;        // EVAW
+  header.subchunk1_id = 0x20746d66;  // "fmt "
+  header.subchunk1_size = 16;        // 16 for PCM
+  header.audio_format = 1;           // PCM =1
+
+  int32_t bits_per_sample = 16;  // int16_t
+  header.num_channels = num_channels;
+  header.sample_rate = sampling_rate;
+  header.byte_rate = sampling_rate * num_channels * bits_per_sample / 8;
+  header.block_align = num_channels * bits_per_sample / 8;
+  header.bits_per_sample = bits_per_sample;
+  header.subchunk2_id = 0x61746164;  // atad
+  header.subchunk2_size = n * num_channels * bits_per_sample / 8;
+
+  header.chunk_size = 36 + header.subchunk2_size;
+
+  std::vector<int16_t> samples_int16(n * num_channels);
+  for (int32_t i = 0; i != n * num_channels; ++i) {
+    samples_int16[i] = samples[i];
+  }
+
+  std::ofstream os(filename, std::ios::binary);
+  if (!os) {
+    printf("Failed to create %s", filename.c_str());
+    return false;
+  }
+
+  os.write(reinterpret_cast<const char *>(&header), sizeof(header));
+  os.write(reinterpret_cast<const char *>(samples_int16.data()),
+           samples_int16.size() * sizeof(int16_t));
+
+  if (!os) {
+    printf("Write %s failed", filename.c_str());
+    return false;
+  }
+
+  return true;
+}
+
+#endif