Programowanie robotów Unitree w C++

Jak programować roboty Unitree w C++

Programowanie robotów Unitree w C++

Wprowadzenie do programowania robotów Unitree w języku C++

Czym jest ROS2 i dlaczego warto go używać?

ROS2 (Robot Operating System 2) to framework do komunikacji międzyprocesowej, który stanowi „układ nerwowy” współczesnych robotów. W odróżnieniu od systemów operacyjnych takich jak Windows czy Linux, ROS2 nie jest systemem w rozumieniu zarządzania plikami czy procesami, lecz warstwą pośredniczącą (middleware) , która umożliwia wymianę danych pomiędzy różnymi komponentami robota.

Wyobraź sobie robota jako organizm: mózg (algorytmy AI), zmysły (lidary, kamery) i mięśnie (silniki). ROS2 to układ nerwowy, który przesyła informacje między tymi elementami w ustandaryzowany sposób.

ROS2 Humble Hawksbill to wersja LTS (Long Term Support) przeznaczona dla Ubuntu 22.04. Jej wsparcie techniczne potrwa do maja 2027 roku, co czyni ją idealnym wyborem dla zastosowań przemysłowych i badawczych.

C++ jako język programowania robotów w przemyśle

W świecie programowania robotów istnieje wyraźny podział: Python króluje w fazie prototypowania, badań i szybkiego tworzenia skryptów, natomiast C++ jest językiem pierwszego wyboru w przemyśle i zastosowaniach produkcyjnych . Dlaczego?

  1. Wydajność i determinizm czasowy: C++ kompiluje się do kodu maszynowego, co zapewnia błyskawiczne wykonanie. W robotyce liczy się każda milisekunda.
  2. Bezpośredni dostęp do sprzętu: Język C++ pozwala na niskopoziomową komunikację z kontrolerami silników i czujnikami.
  3. Ekosystem przemysłowy: Większość fabryk i linii produkcyjnych wykorzystuje systemy czasu rzeczywistego (RTOS), które są naturalnym środowiskiem dla kodu w C++.
  4. Dziedzictwo kodu: Najbardziej zaawansowane biblioteki robotyczne są napisane w C++.

Charakterystyka robotów Unitree

Firma Unitree Robotics oferuje dwie główne linie robotów programowalnych:

  • Czteronożne roboty (quadruped, Robo-psy) znajdujący zastosowanie w inspekcji przemysłowej, monitorowaniu terenu, logistyce oraz jako platforma badawcza.
  • Modele: Unitree Go2 EDU, Go2 ENT (roboty patrolowe) , Unitree A2, B1, B2, B2-W (duże roboty kroczące)
  • Humanoidalne roboty wykorzystywane w zaawansowanych badaniach nad lokomocją dwunożną, manipulacją przedmiotami oraz interakcją człowiek-robot.
  • Modele: Unitree G1 EDU (średni robot humanoidalny), G1-D (robot na platformie kołowej), H2 Duży robot humanoidalny

Kluczowe pojęcia i skróty – słownik pojęć

Skrót/PojęcieRozwinięcie i wyjaśnienie
SDKSoftware Development Kit – zestaw narzędzi, bibliotek i dokumentacji ułatwiających tworzenie oprogramowania dla konkretnej platformy.
DDSData Distribution Service – protokół komunikacyjny używany przez ROS2 do wymiany danych.
Węzeł (Node)Podstawowa jednostka wykonawcza w ROS2.
Temat (Topic)Nazwany kanał, przez który węzły wymieniają dane.
InterfejsDefinicja struktury danych przesyłanych przez tematy.
URDFUnified Robot Description Format – plik XML opisujący model robota.
ColconNarzędzie do kompilacji (budowania) pakietów ROS2.

Architektura komunikacji: jak C++ łączy się z robotem?

Roboty Unitree nowej generacji (Go2, G1, H1) wykorzystują Cyclone DDS jako domyślny protokół komunikacyjny . ROS2 Humble również używa Cyclone DDS. Oznacza to, że:

  1. Węzły ROS2 napisane w C++ mogą bezpośrednio komunikować się z oprogramowaniem pokładowym robota.
  2. SDK Unitree (unitree_sdk2) dostarcza biblioteki C++, które abstrahują szczegóły komunikacji DDS.

Istnieją dwa główne tryby łączenia się z robotem:

  • Tryb przewodowy (Ethernet): Zapewnia najniższe opóźnienia.
  • Tryb bezprzewodowy (Wi-Fi / WebRTC): Wygodny, ale może wprowadzać dodatkowe opóźnienia. W tym trybie tylko jeden klient może być połączony z robotem w danym momencie .
ROS C++ Unitree

Przygotowanie środowiska programistycznego

Krok 1: Instalacja ROS2 Humble

Postępuj zgodnie z oficjalną dokumentacją ROS2 dla Ubuntu 22.04. Zainstaluj wersję ros-humble-desktop.

Krok 2: Instalacja zależności dla C++

bash

sudo apt install g++ cmake python3-colcon-common-extensions

Krok 3: Pobranie SDK Unitree dla C++

Odwiedź oficjalne repozytorium Unitree na GitHubie (unitreerobotics/unitree_sdk2) i postępuj zgodnie z instrukcjami w pliku README, aby skompilować i zainstalować SDK.

Krok 4: Utworzenie przestrzeni roboczej ROS2

bash

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

Krok 5: Konfiguracja sieci

Dla połączenia przewodowego skonfiguruj interfejs sieciowy komputera na adres z tej samej podsieci co robot (domyślnie 192.168.123.161). Sprawdź połączenie komendą ping.

Pierwszy projekt: Tworzenie pakietu C++ w ROS2

Utwórz pakiet z zależnościami:

bash

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake unitree_cpp_tutorials --dependencies rclcpp std_msgs geometry_msgs

Struktura pakietu:

  • src/ – pliki źródłowe C++
  • include/ – pliki nagłówkowe
  • CMakeLists.txt – konfiguracja kompilacji
  • package.xml – metadane i zależności

Komunikacja z robotem: diagnostyka i eksploracja tematów

Zanim napiszesz kod, poznaj interfejs robota:

bash

# Lista dostępnych tematów
ros2 topic list

# Sprawdzenie struktury wiadomości dla konkretnego tematu
ros2 interface show $(ros2 topic list -t | grep /imu | cut -d' ' -f2)

# Podgląd danych na żywo
ros2 topic echo /imu

Implementacja prostego wydawcy (publisher) w C++

Węzeł publikujący na temat /cmd_vel powinien:

  1. Dziedziczyć po rclcpp::Node.
  2. Utworzyć wydawcę (create_publisher).
  3. Utworzyć timer wywołujący funkcję okresowo.
  4. W funkcji timera utworzyć wiadomość typu geometry_msgs::msg::Twist i opublikować ją.

Kluczowe elementy kodu:

cpp

// Tworzenie wydawcy
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

// Tworzenie timera
timer_ = this->create_wall_timer(500ms, std::bind(&ClassName::timer_callback, this));

// Publikowanie wiadomości
auto message = geometry_msgs::msg::Twist();
message.linear.x = 0.2;
publisher_->publish(message);

W CMakeLists.txt należy dodać:

cmake

add_executable(node_name src/node_name.cpp)
ament_target_dependencies(node_name rclcpp geometry_msgs)
install(TARGETS node_name DESTINATION lib/${PROJECT_NAME})

Po kompilacji (colcon build) uruchamiamy węzeł:

bash

ros2 run unitree_cpp_tutorials node_name

Implementacja prostego prenumeratora (subscriber) w C++

Węzeł subskrybujący temat /imu:

  1. Tworzy prenumeratora (create_subscription).
  2. Definiuje funkcję callback wywoływaną przy każdej wiadomości.

Kluczowe elementy:

cpp

subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
    "/imu", 10, std::bind(&ClassName::topic_callback, this, std::placeholders::_1));

void topic_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    // Odczyt danych z msg
    double linear_acc_z = msg->linear_acceleration.z;
    RCLCPP_INFO(this->get_logger(), "Przyspieszenie Z: %f", linear_acc_z);
}

Symulacja jako bezpieczne środowisko testowe

Zanim uruchomisz kod na fizycznym robocie, przetestuj go w symulacji.

Symulacja w MuJoCo: Istnieje repozytorium quadruped_ros2_control, które integruje MuJoCo z ROS2 dla robotów Unitree. Po sklonowaniu i kompilacji uruchamiamy:

bash

ros2 launch unitree_guide_controller mujoco.launch.py

Symulacja w Gazebo:

bash

sudo apt install ros-humble-gazebo-ros
ros2 launch unitree_guide_controller gazebo_classic.launch.py

Symulacja w Isaac Sim: NVIDIA Isaac Sim oferuje zaawansowane możliwości. W repozytorium go2_omniverse znajdziesz gotowe konfiguracje dla Go2 i G1.

Praca z sensorami w C++

Integracja z lidarem Unitree L2

SDK lidaru L2 (unilidar_sdk2) udostępnia funkcje do odczytu chmury punktów. Węzeł ROS2 powinien:

  1. Zainicjalizować SDK lidaru.
  2. W pętli lub timerze pobierać dane (getPointCloud()).
  3. Konwertować je do formatu sensor_msgs::msg::PointCloud2 i publikować.

Integracja z kamerą Intel RealSense

Sterownik realsense2_camera publikuje obrazy na znanych tematach. Aby przetwarzać obraz w C++, subskrybujemy temat /camera/color/image_raw i używamy cv_bridge do konwersji do formatu OpenCV.

Kluczowe elementy:

cpp

#include <cv_bridge/cv_bridge.h>

void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    cv::Mat image = cv_ptr->image;
    // Przetwarzanie w OpenCV
}

Zaawansowane sterowanie w C++

Implementacja kontrolera prędkości z regulatorem PID

Koncepcja: węzeł subskrybuje odometrię, oblicza błąd położenia, a następnie za pomocą regulatora PID wyznacza prędkości liniową i kątową, które publikuje na /cmd_vel.

Główne kroki:

  1. W callbacku odometrii zapamiętujemy aktualną pozycję i orientację.
  2. W timerze (np. 100 Hz) obliczamy błąd w układzie robota.
  3. Stosujemy wzór PID: u = Kp * error + Ki * integral + Kd * derivative.
  4. Publikujemy wynik na /cmd_vel.

Sterowanie z wykorzystaniem WebRTC (dla Go2)

WebRTC w Go2 pozwala na bezprzewodowe sterowanie. W C++ można użyć klienta usługi ROS2 do wysłania komendy. Przykładowo, aby wykonać „BackFlip”, wysyłamy odpowiednie żądanie do usługi /webrtc_command.

Nawigacja autonomiczna z Nav2

Nav2 to stos oprogramowania do nawigacji. Aby użyć go z robotem Unitree:

  1. Zbuduj mapę za pomocą SLAM (slam_toolbox).
  2. Zapisz mapę.
  3. Uruchom Nav2 z wczytaną mapą.
  4. Wyślij cel nawigacji.

W C++ możemy wysłać cel za pomocą klienta akcji navigate_to_pose. Klient tworzy cel, wysyła go i oczekuje na wynik.

Debugowanie i rozwiązywanie problemów

Konflikt CycloneDDS z SDK Unitree: Jeśli wystąpią problemy z komunikacją, spróbuj:

  • Zmienić ROS_DOMAIN_ID: export ROS_DOMAIN_ID=42
  • Przełączyć się na Fast-DDS: export RMW_IMPLEMENTATION=rmw_fastrtps_cpp

Sprawdzanie połączenia:

bash

ping 192.168.123.161
ros2 topic list
ros2 topic echo /imu --once

Dobre praktyki programowania w C++ dla ROS2

  1. Zarządzanie pamięcią: Używaj std::shared_ptr.
  2. Obsługa błędów: Sprawdzaj dostępność usług przed wysłaniem żądania.
  3. Konfiguracja: Przechowuj parametry w plikach YAML.
  4. Bezpieczeństwo: Testuj w symulacji, używaj podwieszenia dla humanoidów.

Przydatne repozytoria i zasoby

  • unitreerobotics/unitree_sdk2 – oficjalne SDK
  • unitreerobotics/unilidar_sdk2 – SDK lidaru L2
  • legubiao/quadruped_ros2_control – kontrolery dla robotów czworonożnych
  • abizovnuralem/go2_omniverse – integracja z Isaac Sim

Podsumowanie i mapa drogowa

  1. Faza 1: Podstawy – instalacja, pierwszy pakiet, wydawca/prenumerator.
  2. Faza 2: Eksploracja – połączenie z robotem, diagnostyka tematów.
  3. Faza 3: Symulacja – testy w MuJoCo/Gazebo.
  4. Faza 4: Sensory – integracja lidaru i kamery.
  5. Faza 5: Zaawansowane sterowanie – PID, Nav2.
  6. Faza 6: Wdrożenie – testy na fizycznym robocie.

Bezpieczeństwo przede wszystkim – ważne wskazówki przed uruchomieniem robota

Praca z robotami mobilnymi, zwłaszcza humanoidalnymi (Unitree G1) czy czworonożnymi (Unitree Go2), niesie ze sobą ryzyko uszkodzenia sprzętu oraz potencjalne zagrożenie dla osób w otoczeniu. Poniższe zasady pomogą Ci uniknąć najczęstszych błędów i pracować odpowiedzialnie.

1. Symulacja – Twój pierwszy i najważniejszy krok

Zanim uruchomisz jakikolwiek kod na fizycznym robocie, przetestuj go w symulacji.
W poradniku dla C++ wspomnieliśmy o symulatorach takich jak MuJoCo (z integracją w repozytorium quadruped_ros2_control), Gazebo oraz NVIDIA Isaac Sim (z gotowymi konfiguracjami dla Go2 i G1). Symulacja pozwala bezpiecznie sprawdzić, czy Twój skompilowany węzeł C++ poprawnie publikuje komendy na /cmd_vel, czy odbiera dane z lidaru, zanim robot wykona gwałtowny ruch w rzeczywistości. Traktuj symulację jako obowiązkowy etap – nigdy nie pomijaj go w pośpiechu.

2. Zawsze rozumiesz kod, który uruchamiasz

C++ daje ogromną kontrolę, ale też wymaga szczególnej uwagi. Nie kopiuj fragmentów kodu (w tym z tego poradnika) bez zrozumienia, co robią. Przeanalizuj każdą linijkę:

  • Do jakiego tematu ROS2 się odwołujesz? (/cmd_vel, /joint_states, /lowstate)
  • Jakie wartości wysyłasz? Zbyt gwałtowna komenda prędkości (np. linear.x = 1.0) może przewrócić robota.
  • Czy Twój kod uwzględnia ograniczenia sprzętowe (maksymalne prędkości, zakresy ruchu, limity momentów obrotowych)?
  • Sprawdź, czy używasz odpowiednich typów wiadomości i czy dane są prawidłowo konwertowane (np. przy odczycie z lidaru).

3. Fizyczne środki bezpieczeństwa – to nie są fanaberie

  • Dla robota Go2 (czworonożny): Upewnij się, że podłoże jest równe, a w zasięgu robota nie ma przeszkód ani osób. Zawsze miej robota w polu widzenia. Startuj od niskich prędkości (np. 0,1 m/s).
  • Dla robota G1 (humanoidalny): Bezwzględnie stosuj uprząż lub podwieszenie podczas pierwszych testów. Upadek humanoida z wysokości może go trwale uszkodzić i stanowić zagrożenie dla otoczenia. Nawet w symulacji humanoidy bywają niestabilne – w rzeczywistości ryzyko jest jeszcze większe.
  • Awaryjne zatrzymanie: Zawsze miej pod ręką fizyczny przycisk STOP (emergency stop) i wiedz, jak go użyć. W przypadku robota Go2 możesz też użyć aplikacji mobilnej do natychmiastowego zatrzymania. W C++ warto zaimplementować węzeł nasłuchujący na osobny temat awaryjnego zatrzymania.

4. Uwaga na połączenie sieciowe i konflikty DDS

  • W trybie bezprzewodowym (WebRTC) tylko jeden klient może być połączony z robotem w danej chwili. Przed uruchomieniem swojego programu upewnij się, że aplikacja mobilna i fizyczny kontroler są rozłączone.
  • Pamiętaj o potencjalnym konflikcie między CycloneDDS (domyślnym w ROS2 Humble) a SDK Unitree. Jeśli wystąpią problemy z komunikacją, rozważ zmianę ROS_DOMAIN_ID lub przełączenie na Fast-DDS (zgodnie ze wskazówkami w rozdziale o debugowaniu).
  • Zawsze sprawdzaj połączenie komendą ping 192.168.123.161 przed wysłaniem pierwszej komendy.

5. Testuj w izolacji, potem w symulacji, na końcu na robocie

  1. Test jednostkowy: Uruchom sam węzeł C++ (np. publikujący na /cmd_vel) bez robota, obserwując komunikaty w konsoli. Sprawdź, czy timer działa, czy wiadomości są publikowane z oczekiwaną częstotliwością.
  2. Test w symulacji: Uruchom symulator (MuJoCo/Gazebo) i swój węzeł. Obserwuj zachowanie modelu robota. Sprawdź, czy odometria, IMU i inne sensory dostarczają danych w oczekiwanym formacie.
  3. Test na fizycznym robocie: Dopiero gdy symulacja działa stabilnie, możesz przejść do testów na fizycznym robocie – zaczynając od małych, bezpiecznych wartości i mając palec na przycisku STOP.

6. Bądź na bieżąco z dokumentacją i wersjami

Środowisko ROS2 i SDK Unitree ewoluują. To, co działa dziś, może być niekompatybilne z jutrzejszą aktualizacją oprogramowania robota. Zawsze sprawdzaj oficjalne repozytoria GitHub Unitree (unitreerobotics) i daty publikacji przykładów. W przypadku C++ kluczowe jest repozytorium unitree_sdk2 – upewnij się, że używasz wersji zgodnej z firmware Twojego robota (np. dla Go2 wersja ≥ V1.1.6 wymaga uwzględnienia zmian w interfejsie usług sterowania ruchem).

Pamiętaj: W robotyce nie ma miejsca na pochopne eksperymenty. Każda komenda wysłana do robota ma fizyczne konsekwencje. Bezpieczeństwo zależy od Ciebie – od Twojej wiedzy, rozwagi i przestrzegania procedur

FAQ – Programowanie robotów Unitree w C++ (ROS2 Humble)

1. Podstawy i pierwsze kroki

P: Czy muszę mieć fizycznego robota, aby rozpocząć naukę?
O: Zdecydowanie zalecamy rozpoczęcie od symulacji (MuJoCo, Gazebo, Isaac Sim). W C++ błędy w zarządzaniu pamięcią lub wycieki mogą być trudne do wychwycenia i mogą uszkodzić sprzęt. Symulacja pozwala na bezpieczne testowanie i debugowanie.

P: Jaka jest różnica między ROS2 Humble a ROS (ROS1)?
O: ROS2 (w tym Humble) został zaprojektowany od nowa z myślą o zastosowaniach produkcyjnych i systemach czasu rzeczywistego. Kluczowe różnice:

  • Komunikacja oparta na DDS (zamiast niestandardowego protokołu ROS1).
  • Wsparcie dla systemów wielowątkowych i embedded.
  • Lepsze bezpieczeństwo i szyfrowanie.
  • Wersje LTS, takie jak Humble, gwarantują długoterminową stabilność.
    Dla nowych projektów zawsze wybieraj ROS2.

P: Czy SDK Unitree dla C++ różni się od tego dla Pythona?
O: Funkcjonalnie są podobne, ale w C++ masz bezpośredni dostęp do niskopoziomowych struktur DDS i większą kontrolę nad wydajnością. SDK dla C++ (unitree_sdk2) jest biblioteką, którą kompilujesz i linkujesz ze swoim kodem, podczas gdy w Pythonie jest to pakiet instalowany przez pip.

2. Instalacja i konfiguracja

P: Podczas kompilacji pojawiają się błędy linker’a (undefined reference). Jak je rozwiązać?
O: To typowy problem w C++.

  1. Sprawdź, czy w CMakeLists.txt poprawnie dodano zależności: ament_target_dependencies(NAZWA_WĘZŁA rclcpp geometry_msgs ...).
  2. Upewnij się, że ścieżki do bibliotek SDK są poprawne. Może być konieczne dodanie ich ręcznie poprzez target_link_libraries.
  3. Skorzystaj z find_package, aby odnaleźć zainstalowane pakiety ROS2 i SDK.
  4. Sprawdź kolejność linkowania – biblioteki zależne muszą występować po tych, które z nich korzystają.

P: Mój program nie widzi robota, mimo że ping działa.
O: To często problem konfiguracji DDS.

  1. Ustaw zmienną środowiskową: export ROS_DOMAIN_ID=42 (wybierz liczbę od 0 do 232, różną od domyślnej 0).
  2. Jeśli to nie pomoże, spróbuj przełączyć implementację DDS na Fast-DDS: export RMW_IMPLEMENTATION=rmw_fastrtps_cpp.
  3. Sprawdź, czy nie masz uruchomionych innych procesów blokujących komunikację (np. aplikacja mobilna).

P: Gdzie znaleźć pliki nagłówkowe (*.hpp) SDK Unitree?
O: Po skompilowaniu i zainstalowaniu SDK (sudo make install), pliki nagłówkowe powinny trafić do standardowych lokalizacji, np. /usr/local/include/unitree/. W swoim kodzie dołączasz je przez #include <unitree/robot/...>.

3. Programowanie i debugowanie

P: Jaka jest różnica między rclcpp::spin a rclcpp::spin_some?
O:

  • rclcpp::spin(node): Blokuje główny wątek i przetwarza wszystkie przychodzące callbacki w nieskończonej pętli. Używane dla węzłów, które mają pracować ciągle (np. sterowniki, kontrolery).
  • rclcpp::spin_some(node): Przetwarza jedną porcję oczekujących callbacki i natychmiast zwraca kontrolę. Przydatne, gdy musisz wykonać własną pętlę z określoną częstotliwością lub gdy łączysz ROS2 z inną biblioteką (np. GUI).

P: Jak uniknąć wycieków pamięci w węźle C++?
O: Stosuj dobre praktyki:

  • Używaj inteligentnych wskaźników (std::shared_ptr, std::unique_ptr) zamiast surowych (*).
  • Dla publikatorów, subskrybentów i timerów używaj typów SharedPtr dostarczanych przez ROS2 (np. rclcpp::Publisher::SharedPtr).
  • Wszystkie alokacje wewnątrz callbacków (jeśli konieczne) staraj się ograniczać do minimum i alokuj na stosie, nie na stercie.
  • Używaj narzędzi takich jak valgrind do wykrywania wycieków.

P: Mój węzeł działa, ale sporadycznie występują „segmentation faults”. Jak to debugować?
O:

  1. Skompiluj swój kod w trybie debug (w CMakeLists.txt: set(CMAKE_BUILD_TYPE Debug)).
  2. Uruchom program przez gdb: gdb --args ros2 run ... , a następnie wewnątrz gdb wpisz run.
  3. Gdy program się wyłoży, wpisz bt (backtrace), aby zobaczyć, która linia kodu spowodowała błąd.
  4. Częste przyczyny to: dereferencja wskaźnika nullptr, dostęp poza zakres tablicy, używanie obiektu po jego usunięciu.

4. Sensory i zaawansowane funkcje

P: Jak w C++ połączyć dane z lidaru i kamery?
O: To zadanie dla tzw. fuzji sensorów. Możesz podejść do tego na kilka sposobów:

  1. Utwórz jeden węzeł, który subskrybuje zarówno temat lidaru (/points), jak i kamery (/camera/color/image_raw). W callbackach synchronizuj wiadomości po znacznikach czasu.
  2. Użyj time synchronizera z ROS2 (message_filters::Synchronizer), który pozwala wywołać jeden callback, gdy dostępne są skorelowane czasowo wiadomości z obu tematów.
  3. Biblioteki takie jak PCL (Point Cloud Library) i OpenCV mają funkcje do łączenia chmur punktów z obrazami (teksturowanie chmury).

P: Jak wysłać cel do Nav2 z poziomu C++?
O: Użyj klienta akcji dla nav2_msgs/action/NavigateToPose. Proces wygląda następująco:

  1. Utwórz klienta akcji: rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this, "navigate_to_pose").
  2. Poczekaj, aż serwer akcji będzie dostępny: client->wait_for_action_server().
  3. Utwórz wiadomość celu (NavigateToPose::Goal), wypełnij współrzędne i orientację.
  4. Wyślij cel asynchronicznie: client->async_send_goal(goal_msg).
  5. Zdefiniuj funkcje callback dla wyniku i ewentualnego feedbacku.

P: Czy mogę sterować robotem G1 (humanoid) przez C++?
O: Tak, G1 używa tego samego SDK2 co Go2. Interfejsy są analogiczne. Różnica polega na większej liczbie stopni swobody i konieczności sterowania nie tylko ruchem bazy, ale także ramionami i głową. Będziesz musiał publikować na tematy związane z pozycjami stawów (/joint_trajectory_controller/joint_trajectory), a nie tylko /cmd_vel.

5. Bezpieczeństwo i dobre praktyki

P: Jak zaimplementować awaryjne zatrzymanie w kodzie C++?
O: Możesz to zrobić na kilka sposobów:

  1. Subskrypcja tematu alarmowego: Jeśli robot publikuje temat z sygnałem STOP (np. /emergency_stop), utwórz subskrybenta, który w callbacku natychmiast publikuje zerową prędkość na /cmd_vel i kończy działanie węzła.
  2. Obsługa sygnałów systemowych (SIGINT): Zarejestruj handler dla Ctrl+C, który bezpiecznie zatrzymuje robota przed zamknięciem programu.
  3. Watchdog: W pętli sterowania sprawdzaj, czy od jakiegoś czasu nie otrzymano nowych danych z czujników bezpieczeństwa; jeśli tak, przejdź do stanu awaryjnego.

P: Czy kod z tego poradnika jest bezpieczny do skopiowania i uruchomienia?
O: Przedstawione fragmenty kodu mają charakter edukacyjny i poglądowy. C++ daje ogromną moc, ale też większe pole do popełnienia błędów. Zawsze przed uruchomieniem na fizycznym robocie:

  • Przeanalizuj i zrozum każdą linijkę.
  • Skompiluj z ostrzeżeniami (-Wall -Wextra) i popraw wszystkie warningi.
  • Przetestuj w symulacji, używając tych samych narzędzi debugowania.
  • Zacznij od małych wartości i bądź gotowy do natychmiastowego zatrzymania.
    Odpowiedzialność za kod i jego konsekwencje spoczywa na Tobie.

P: Gdzie szukać pomocy przy problemach z C++ i ROS2?
O: Najlepsze źródła to:

  1. Oficjalna dokumentacja ROS2 w C++: https://docs.ros.org/en/humble/ – Tutoriale i API reference.
  2. Repozytoria GitHub Unitree: https://github.com/unitreerobotics – Przykłady w C++ są w unitree_sdk2.
  3. Stack Overflow: Taguj pytania ros2 i c++.
  4. Discord i fora społeczności ROS: Linki często dostępne na oficjalnej stronie ROS.
  5. Książki: „Programming Robots with ROS2” (wersja dla C++).