lx/Go2_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化项目，包含fast_lio_ws和unitree_sdk2

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-12 15:01:52 +08:00
commit 8e16ecceba
1490 changed files with 232338 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
unitree_sdk2/example/a2/CMakeLists.txt Normal file
View File

@@ -0,0 +1,8 @@
add_executable(a2_sport_client sport/a2_sport_client.cpp)
target_link_libraries(a2_sport_client unitree_sdk2)
add_executable(a2_sport_state sport/a2_sport_state.cpp)
target_link_libraries(a2_sport_state unitree_sdk2)
add_executable(a2_audio_client_example audio/a2_audio_client_example.cpp)
target_link_libraries(a2_audio_client_example unitree_sdk2)

175
unitree_sdk2/example/a2/audio/a2_audio_client_example.cpp Normal file
View File

@@ -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/a2/audio/audio_client.hpp>
#include "wav.hpp"
#define AUDIO_FILE_PATH "../example/a2/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::a2::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;
}

BIN
unitree_sdk2/example/a2/audio/test.wav Normal file
View File

Binary file not shown.

232
unitree_sdk2/example/a2/audio/wav.hpp Normal file
View File

@@ -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

315
unitree_sdk2/example/a2/sport/a2_sport_client.cpp Normal file
View File

@@ -0,0 +1,315 @@
#include <iostream>
#include <string>
#include <chrono>
#include <mutex>
#include <thread>
#include <pthread.h>
#include <stdexcept>
#include <cmath>
#include <unitree/robot/a2/sport/sport_client.hpp>
using namespace std;
struct TestOption
{
std::string name;
int id;
};
const vector<TestOption> option_list =
{
{"damp", 0},
{"balance_stand", 1},
{"stop_move", 2},
{"stand_down", 3},
{"recovery_stand", 4},
{"move", 5},
{"switch_gait", 6},
{"speed_level", 7},
{"get_state", 8},
{"recovery_switch", 9},
{"body_height", 10},
{"stand_up", 11},
// Caution: test in open area
{"enter_leftside_gait", 12},
{"exit_leftside_gait", 13},
{"enter_handstand", 14},
{"exit_handstand", 15},
{"front_flip", 16},
{"back_flip", 17},
{"pose", 18},
{"euler", 19},
{"reset_estimator", 20},
{"square_trajectory", 21},
{"circle_trajectory", 22},
};
int ConvertToInt(const std::string &str)
{
try
{
std::stoi(str);
return std::stoi(str);
}
catch (const std::invalid_argument &)
{
return -1;
}
catch (const std::out_of_range &)
{
return -1;
}
}
class UserInterface
{
public:
UserInterface() {};
~UserInterface() {};
void terminalHandle()
{
std::string input;
std::getline(std::cin, input);
if (input.compare("list") == 0)
{
for (TestOption option : option_list)
{
std::cout << option.name << ", id: " << option.id << std::endl;
}
}
for (TestOption option : option_list)
{
if (input.compare(option.name) == 0 || ConvertToInt(input) == option.id)
{
test_option_->id = option.id;
test_option_->name = option.name;
std::cout << "Test: " << test_option_->name << ", test_id: " << test_option_->id << std::endl;
}
}
};
TestOption *test_option_;
};
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
TestOption test_option;
test_option.id = 1;
unitree::robot::a2::SportClient sport_client;
sport_client.SetTimeout(25.0f);
sport_client.Init();
UserInterface user_interface;
user_interface.test_option_ = &test_option;
std::cout << "Input \"list \" to list all test option ..." << std::endl;
long res_count = 0;
while (1)
{
auto time_start_trick = std::chrono::high_resolution_clock::now();
static const constexpr auto dt = std::chrono::microseconds(20000); // 50Hz
user_interface.terminalHandle();
int res = 1;
if (test_option.id == 0)
{
res = sport_client.Damp();
}
else if (test_option.id == 1)
{
res = sport_client.BalanceStand();
}
else if (test_option.id == 2)
{
res = sport_client.StopMove();
}
else if (test_option.id == 3)
{
res = sport_client.StandDown();
}
else if (test_option.id == 4)
{
res = sport_client.RecoveryStand();
}
else if (test_option.id == 5)
{
res = sport_client.Move(0.0, 0.0, 0.5);
}
else if (test_option.id == 6)
{
res = sport_client.SwitchGait(0);
}
else if (test_option.id == 7)
{
res = sport_client.SpeedLevel(1);
}
else if (test_option.id == 8)
{
std::map<std::string, std::string> state_map;
res = sport_client.GetState(state_map);
std::cout << "fsm_id: " << state_map["fsm_id"] << std::endl;
std::cout << "fsm_name: " << state_map["fsm_name"] << std::endl;
std::cout << "speed_level: " << state_map["speed_level"] << std::endl;
std::cout << "auto_recovery_switch: " << state_map["auto_recovery_switch"] << std::endl;
std::cout << "process_state: " << state_map["process_state"] << std::endl;
}
else if (test_option.id == 9)
{
res = sport_client.SetAutoRecovery(0);
}
else if (test_option.id == 10)
{
res = sport_client.BodyHeight(0.3f);
}
else if (test_option.id == 11)
{
res = sport_client.StandUp();
}
else if (test_option.id == 12)
{
res = sport_client.LeftSideGait(1);
}
else if (test_option.id == 13)
{
res = sport_client.LeftSideGait(0);
}
else if (test_option.id == 14)
{
res = sport_client.HandStand(1);
}
else if (test_option.id == 15)
{
res = sport_client.HandStand(0);
}
else if (test_option.id == 16)
{
res = sport_client.FrontFlip();
}
else if (test_option.id == 17)
{
res = sport_client.BackFlip();
}
else if (test_option.id == 18)
{
res = sport_client.BodyPosition(0.2f, 0.2f, -0.2f, 0.2f);
}
else if (test_option.id == 19)
{
res = sport_client.Euler(0.2f, 0.3f, 0.3f);
}
else if (test_option.id == 20)
{
res = sport_client.ResetEstimator();
}
else if (test_option.id == 21)
{
// 矩形轨迹: 1.0m x 1.0m, 带加减速过程的密集路径点
std::vector<unitree::robot::a2::PathPoint> path;
unitree::robot::a2::PathPoint pt;
// ===== 边1: (0,0)→(1,0), 向前 1m, 0-4s =====
pt.t_from_start = 0.0f; pt.x = 0.00f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 0.5f; pt.x = 0.03f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 1.0f; pt.x = 0.12f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 1.5f; pt.x = 0.28f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 2.0f; pt.x = 0.45f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 2.5f; pt.x = 0.62f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 3.0f; pt.x = 0.78f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 3.5f; pt.x = 0.94f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 4.0f; pt.x = 1.00f; pt.y = 0.0f; pt.yaw = 0.0f; path.push_back(pt);
// ===== 边2: (1,0)→(1,1), 左移 1m, 4-8s =====
pt.t_from_start = 4.5f; pt.x = 1.00f; pt.y = 0.03f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 5.0f; pt.x = 1.00f; pt.y = 0.12f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 5.5f; pt.x = 1.00f; pt.y = 0.28f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 6.0f; pt.x = 1.00f; pt.y = 0.45f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 6.5f; pt.x = 1.00f; pt.y = 0.62f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 7.0f; pt.x = 1.00f; pt.y = 0.78f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 7.5f; pt.x = 1.00f; pt.y = 0.94f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 8.0f; pt.x = 1.00f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
// ===== 边3: (1,1)→(0,1), 后退 1m, 8-13s =====
pt.t_from_start = 8.5f; pt.x = 0.97f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 9.0f; pt.x = 0.90f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 9.5f; pt.x = 0.78f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 10.0f; pt.x = 0.62f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 10.5f; pt.x = 0.45f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 11.0f; pt.x = 0.28f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 11.5f; pt.x = 0.12f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 12.0f; pt.x = 0.03f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 13.0f; pt.x = 0.00f; pt.y = 1.00f; pt.yaw = 0.0f; path.push_back(pt);
// ===== 边4: (0,1)→(0,0), 右移 1m, 13-16s =====
pt.t_from_start = 13.5f; pt.x = 0.00f; pt.y = 0.94f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 14.0f; pt.x = 0.00f; pt.y = 0.78f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 14.5f; pt.x = 0.00f; pt.y = 0.55f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 15.0f; pt.x = 0.00f; pt.y = 0.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 15.5f; pt.x = 0.00f; pt.y = 0.00f; pt.yaw = 0.0f; path.push_back(pt);
pt.t_from_start = 16.0f; pt.x = 0.00f; pt.y = 0.00f; pt.yaw = 0.0f; path.push_back(pt);
res = sport_client.Trajectory(path, 1);
}
else if (test_option.id == 22)
{
// 圆形轨迹: 半径1.0m, 圆心(0.0,1.0), 逆时针
const float R = 1.0f, cx = 0.0f, cy = 1.0f;
const float TOTAL_TIME = 20.0f; // <-- 修改此处调整总时长(s)
const float T_ACCEL = 0.25f * TOTAL_TIME; // 加速段
const float T_CRUISE = 0.50f * TOTAL_TIME; // 匀速段
const float T_DECEL = 0.25f * TOTAL_TIME; // 减速段
const int N = 36;
std::vector<unitree::robot::a2::PathPoint> path;
for (int i = 0; i <= N; ++i)
{
float frac = (float)i / N;
float t;
if (frac <= 0.25f)
t = T_ACCEL * (frac / 0.25f);
else if (frac <= 0.75f)
t = T_ACCEL + T_CRUISE * ((frac - 0.25f) / 0.5f);
else
t = T_ACCEL + T_CRUISE + T_DECEL * ((frac - 0.75f) / 0.25f);
float angle = -M_PI_2 + 2.0f * M_PI * frac;
unitree::robot::a2::PathPoint pt;
pt.t_from_start = t;
pt.x = cx + R * cosf(angle);
pt.y = cy + R * sinf(angle);
pt.yaw = 0.0f;
path.push_back(pt);
}
res = sport_client.Trajectory(path, 1);
}
if (res < 0)
{
res_count += 1;
std::cout << "Request error for: " << option_list[test_option.id].name << ", code: " << res << ", count: " << res_count << std::endl;
}
else
{
res_count = 0;
std::cout << "Request successed: " << option_list[test_option.id].name << ", code: " << res << std::endl;
}
std::this_thread::sleep_until(time_start_trick + dt);
}
return 0;
}

68
unitree_sdk2/example/a2/sport/a2_sport_state.cpp Normal file
View File

@@ -0,0 +1,68 @@
#include <cmath>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/SportModeState_.hpp>
#define TOPIC_HIGHSTATE "rt/lf/sportmodestate"
using namespace unitree::common;
const std::vector<std::string> FSM_STATE_STR = {
"PASSIVE",
"STAND_DOWN",
"STAND_UP",
"DEFAULT_MODE",
"RUNNING_MODE",
"CLIMB_MODE",
"LEFT_SIDE_GAIT",
"RIGHT_SIDE_GAIT",
"HANDSTAND",
"BIPED_STAND",
"FRONT_FLIP",
"BACK_FLIP",
"RECOVERY",
"BASE_HEIGHT_CTRL"};
class Custom
{
public:
Custom()
{
suber.reset(new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>(TOPIC_HIGHSTATE));
suber->InitChannel(std::bind(&Custom::HighStateHandler, this, std::placeholders::_1), 1);
};
void HighStateHandler(const void *message)
{
state = *(unitree_go::msg::dds_::SportModeState_ *)message;
std::cout << "Position: " << state.position()[0] << ", " << state.position()[1] << ", " << state.position()[2] << std::endl;
std::cout << "Velocity: " << state.velocity()[0] << ", " << state.velocity()[1] << ", " << state.velocity()[2] << std::endl;
std::cout << "Mode: " << FSM_STATE_STR[int(state.mode())] << std::endl;
std::cout << "Progress: " << state.progress() << std::endl;
// std::cout << "IMU rpy: " << state.imu_state().rpy()[0] << ", " << state.imu_state().rpy()[1] << ", " << state.imu_state().rpy()[2] << std::endl;
};
unitree_go::msg::dds_::SportModeState_ state;
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> suber;
float dt = 0.05; // 控制步长0.001~0.01
};
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
Custom custom;
while (1)
{
sleep(10);
}
return 0;
}