diff --git a/README.md b/README.md index c9622aa..15d9c08 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,6 @@ A high-performance C++ implementation for controlling Unitree GO2 robot with rea - **📝 Comprehensive Logging**: Multi-level logging with file and console output - **⚙️ Configuration Management**: JSON-based configuration with runtime presets - **🧭 Navigation Support**: SLAM and navigation capabilities -- **📊 State Publishing**: Real-time robot state broadcasting - **🎭 Special Actions**: Support for dances, tricks, and custom motions - **🚀 High Performance**: Optimized for real-time control with configurable frequencies - **🔧 Development Tools**: Built-in scripts and utilities for easy deployment @@ -79,31 +78,7 @@ The system supports flexible configuration through compile-time defaults. Config The default configuration is defined in `include/config.hpp`: -```cpp -// Network settings -constexpr std::string_view NETWORK_INTERFACE = "eth0"; -// MQTT settings -constexpr std::string_view MQTT_BROKER = "192.168.2.236"; -constexpr int MQTT_PORT = 1883; -constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client"; -constexpr std::string_view MQTT_USERNAME = "lzwc"; -constexpr std::string_view MQTT_PASSWORD = "Lzwc@4187."; - -// Topic settings -constexpr std::string_view TOPIC_PREFIX = "unitree/go2"; -constexpr std::string_view TOPIC_CMD = "cmd"; -constexpr std::string_view TOPIC_STATE = "state"; - -// Robot control settings -constexpr double CONTROL_FREQUENCY = 200.0; // Hz -constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz - -// Safety settings -constexpr double MAX_LINEAR_VELOCITY = 1.5; // m/s -constexpr double MAX_ANGULAR_VELOCITY = 2.0; // rad/s -constexpr double EMERGENCY_STOP_TIMEOUT = 5.0; // seconds -``` To customize these settings, modify the values in `include/config.hpp` and rebuild the project. @@ -123,13 +98,9 @@ To customize these settings, modify the values in `include/config.hpp` and rebui The robot runs with sensible defaults out of the box: -- **Network Interface**: `eth0` -- **MQTT Broker**: `192.168.2.236:1883` -- **MQTT Credentials**: Username `lzwc`, Password `Lzwc@4187.` -- **Control Frequency**: 200Hz -- **Safety Limits**: Conservative settings for safe operation -- **Logging**: INFO level to console + +Configuration is done at compile time by modifying values in `include/config.hpp`. No configuration files or command-line arguments needed! ## MQTT API @@ -138,24 +109,23 @@ The robot communicates via MQTT using the following topic structure: ### Command Topics -- `unitree/go2/cmd/oac`: Obstacle avoidance commands -- `unitree/go2/cmd/sport`: Sport mode commands -- `unitree/go2/cmd/rsc`: Robot state commands -- `unitree/go2/cmd/nav`: Navigation commands +- `unitree/go2/cmd`: All commands (sport, navigation, robot state, obstacle avoidance) + +Commands are sent to the command topic with a JSON payload specifying the command type and parameters. -### State Topics -- `unitree/go2/state/robot`: Robot state (position, IMU, motors, etc.) -- `unitree/go2/state/heartbeat`: System status and statistics -- `unitree/go2/state/response`: Command execution responses -- `unitree/go2/state/error`: Error messages ### Sport Commands +Sport commands are sent with a JSON payload containing the command type and optional parameters: + ```json { - "request_id": "unique_id", - "cmd": "StandUp" + "type": "sport", + "cmd": "StandUp", + "param": { + // Optional parameters depending on the command + } } ``` @@ -170,36 +140,65 @@ Supported sport commands: - `Dance1`: Dance routine 1 - `Dance2`: Dance routine 2 - `Hello`: Greeting gesture +- `Move`: Move with specified velocities (vx, vy, vyaw) +- `Euler`: Set body orientation (roll, pitch, yaw) +- And many more... ### Navigation Commands +Navigation commands are sent with a JSON payload containing the command type and parameters: + ```json { - "request_id": "unique_id", + "type": "nav", "cmd": "startMapping" } ``` Supported navigation commands: -- `startMapping`: Start SLAM mapping -- `endMapping`: End SLAM mapping -- `pauseNavigation`: Pause navigation -- `resumeNavigation`: Resume navigation -- `closeSlam`: Close SLAM service +- `start_mapping`: Start SLAM mapping +- `end_mapping`: End SLAM mapping with map address parameter +- `initialize_pose`: Set initial pose with position and orientation +- `pose_navigation`: Navigate to a specific pose +- `pause_navigation`: Pause navigation +- `resume_navigation`: Resume navigation +- `close_slam`: Close SLAM service -### System Commands +### Obstacle Avoidance Commands + +Obstacle avoidance commands are sent with a JSON payload containing the command type and parameters: ```json { - "request_id": "unique_id", + "type": "oac", + "cmd": "SwitchSet", + "param": { + "enable": true + } +} +``` + +Supported obstacle avoidance commands: +- `SwitchSet`: Enable/disable obstacle avoidance +- `SwitchGet`: Get obstacle avoidance status +- `UseRemoteCommandFromApi`: Configure remote command source +- `MoveToAbsolutePosition`: Move to absolute position +- `MoveToIncrementPosition`: Move to increment position + +### System Commands + +System commands are sent with a JSON payload containing the command type and parameters: + +```json +{ + "type": "rsc", "cmd": "GetServiceList" } ``` Supported system commands: -- `GetServiceList`: Get list of available services -- `SwitchService`: Enable/disable a service -- `SetReportFreq`: Set state reporting frequency +- `list`: Get list of available services +- `switch`: Enable/disable a service ## Supported Actions @@ -218,11 +217,11 @@ Supported system commands: - `Hello`: Greeting gesture ### Navigation Actions -- `startMapping`: Start SLAM mapping -- `endMapping`: End SLAM mapping and save map -- `pauseNavigation`: Pause navigation -- `resumeNavigation`: Resume navigation -- `closeSlam`: Close SLAM service +- `start_mapping`: Start SLAM mapping +- `end_mapping`: End SLAM mapping and save map +- `pause_navigation`: Pause navigation +- `resume_navigation`: Resume navigation +- `close_slam`: Close SLAM service ## Safety Features @@ -247,21 +246,21 @@ Supported system commands: ### Custom Configurations -To customize the configuration, modify the values in `include/config.hpp`: +The robot's configuration is defined at compile time in `include/config.hpp`. To customize the configuration: -```cpp -// Example: Change MQTT broker address -constexpr std::string_view MQTT_BROKER = "192.168.1.100"; +1. Edit the constants in `include/config.hpp` to change default values +2. Rebuild the project: + ```bash + cd build + make -j$(nproc) + ``` -// Example: Change control frequency -constexpr double CONTROL_FREQUENCY = 400.0; // Hz -``` - -After making changes, rebuild the project: -```bash -cd build -make -j$(nproc) -``` +Common configuration options include: +- Network interface +- MQTT broker settings +- Topic prefixes +- Control frequencies +- Safety limits ## Quick Start Scripts @@ -282,6 +281,7 @@ unitree-go2/ ├── CMakeLists.txt # Build configuration ├── README.md # This file ├── LICENSE # License information +├── .gitignore # Git ignore file ├── config/ # Configuration files (optional) ├── include/ # Header files │ ├── config.hpp # Configuration management @@ -292,13 +292,13 @@ unitree-go2/ │ ├── navigation.hpp # Navigation and SLAM │ └── nlohmann/ # JSON library ├── src/ # Source files -│ ├── config.cpp -│ ├── controller.cpp -│ ├── custom_robot.cpp -│ ├── logger.cpp -│ ├── main.cpp -│ ├── mqtt.cpp -│ └── navigation.cpp +│ ├── config.cpp # Configuration implementation +│ ├── controller.cpp # Robot controller implementation +│ ├── custom_robot.cpp # Main orchestrator implementation +│ ├── logger.cpp # Logging system implementation +│ ├── main.cpp # Entry point +│ ├── mqtt.cpp # MQTT client implementation +│ └── navigation.cpp # Navigation and SLAM implementation ├── scripts/ # Utility scripts │ ├── install_deps.sh # Install dependencies │ └── run_robot.sh # Run with optimal settings @@ -348,9 +348,8 @@ This project is licensed under the MIT License - see the LICENSE file for detail ## Performance Notes - **Control Frequency**: Default 200Hz, configurable up to 400Hz for high-performance applications -- **State Publishing**: Default 50Hz, can be adjusted based on network bandwidth - **Memory Usage**: Optimized for minimal heap allocations during runtime -- **CPU Usage**: Multi-threaded design with separate threads for control, MQTT, and state publishing +- **CPU Usage**: Multi-threaded design with separate threads for control and MQTT ## Contributing diff --git a/include/config.hpp b/include/config.hpp index 1def4a7..51953b2 100644 --- a/include/config.hpp +++ b/include/config.hpp @@ -25,14 +25,13 @@ constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client"; constexpr std::string_view MQTT_USERNAME = "lzwc"; constexpr std::string_view MQTT_PASSWORD = "Lzwc@4187."; -// Topic settings -constexpr std::string_view TOPIC_PREFIX = "unitree/go2"; -constexpr std::string_view TOPIC_CMD = "cmd"; -constexpr std::string_view TOPIC_STATE = "state"; - -// Robot control settings -constexpr double CONTROL_FREQUENCY = 200.0; // Hz -constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz +// MQTT Topics + constexpr const char* TOPIC_PREFIX = "unitree/go2"; + constexpr const char* TOPIC_CMD = "cmd"; + constexpr const char* TOPIC_STATE = "state"; + constexpr const char* TOPIC_HEARTBEAT = "heartbeat"; + constexpr const char* TOPIC_RESPONSE = "response"; + constexpr const char* TOPIC_ERROR = "error"; // Safety settings constexpr double MAX_LINEAR_VELOCITY = 1.5; // m/s @@ -58,7 +57,6 @@ public: std::string topic_state; double control_frequency; - double state_publish_frequency; double max_linear_velocity; double max_angular_velocity; double emergency_stop_timeout; diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index cf29fe6..c7c9075 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -33,7 +33,6 @@ public: bool initializeMqtt(); void onMqttMessage(const std::string& topic, const std::string& payload); void onMqttConnection(bool connected); - void publishStatus(); void processCmd(const nlohmann::json& message); bool processOacCmd(const std::string& cmd, const nlohmann::json& message); diff --git a/scripts/run_robot.sh b/scripts/run_robot.sh index 4430d1c..7c91826 100644 --- a/scripts/run_robot.sh +++ b/scripts/run_robot.sh @@ -105,8 +105,10 @@ if [ ! -f "$CONFIG_FILE" ]; then "interface": "eth0" }, "mqtt": { - "broker": "localhost", + "broker": "192.168.2.236", "port": 1883, + "username": "lzwc", + "password": "Lzwc@4187.", "client_id": "unitree_go2_client" }, "topics": { @@ -115,8 +117,7 @@ if [ ! -f "$CONFIG_FILE" ]; then "state": "state" }, "robot": { - "control_frequency": 200.0, - "state_publish_frequency": 50.0 + "control_frequency": 200.0 }, "safety": { "max_linear_velocity": 1.5, diff --git a/src/config.cpp b/src/config.cpp index 20cfd5f..1259304 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -3,25 +3,29 @@ namespace custom { void CustomConfig::loadDefaults() { - network_interface = std::string(NETWORK_INTERFACE); - mqtt_broker = std::string(MQTT_BROKER); + // Network settings + network_interface = NETWORK_INTERFACE; + mqtt_broker = MQTT_BROKER; mqtt_port = MQTT_PORT; - mqtt_username = std::string(MQTT_USERNAME); - mqtt_password = std::string(MQTT_PASSWORD); - mqtt_client_id = std::string(MQTT_CLIENT_ID); + mqtt_username = MQTT_USERNAME; + mqtt_password = MQTT_PASSWORD; - topic_prefix = std::string(TOPIC_PREFIX); - topic_cmd = std::string(TOPIC_CMD); - topic_state = std::string(TOPIC_STATE); + // Topic settings + topic_prefix = TOPIC_PREFIX; + topic_cmd = TOPIC_CMD; + topic_state = TOPIC_STATE; + topic_heartbeat = TOPIC_HEARTBEAT; + topic_response = TOPIC_RESPONSE; + topic_error = TOPIC_ERROR; + // Robot control settings control_frequency = CONTROL_FREQUENCY; - state_publish_frequency = STATE_PUBLISH_FREQUENCY; + + // Safety settings + command_timeout = COMMAND_TIMEOUT; max_linear_velocity = MAX_LINEAR_VELOCITY; max_angular_velocity = MAX_ANGULAR_VELOCITY; emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; - stand_height = STAND_HEIGHT; - default_gait = static_cast(DEFAULT_GAIT); - } } // namespace custom \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 1923e26..d000570 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -12,7 +12,13 @@ namespace custom { CustomRobot::CustomRobot() - : initialized_(false), running_(false), controller_(nullptr), navigation_(nullptr), rsc_(nullptr), mqttClient_(nullptr) { + : controller_(nullptr) + , navigation_(nullptr) + , rsc_(nullptr) + , mqttClient_(nullptr) + , running_(false) + , initialized_(false) { +} config_.loadDefaults(); try { @@ -224,7 +230,6 @@ void CustomRobot::onMqttMessage(const std::string& topic, const std::string& pay void CustomRobot::onMqttConnection(bool connected) { if (connected) { LOG_INFO("MQTT client connected to broker"); - } else { LOG_WARN("MQTT client disconnected from broker"); }