Program Listing for File unitree_driver.hpp

↰ Return to documentation for file (include/unitree_ros/unitree_driver.hpp)

#ifndef UNITREE_DRIVER_H
#define UNITREE_DRIVER_H

#include <FaceLightClient.h>
#include <unitree_legged_sdk/unitree_legged_sdk.h>

#include <atomic>
#include <memory>
#include <thread>
#include <unitree_ros/common_defines.hpp>

#include "unitree_legged_sdk/comm.h"

/*
 * @brief Class Unitree driver which is use for the main interaction with
 * the Unitree Go1 robot.
 */
class UnitreeDriver {
    // Unitree SDK related
   private:
    std::string ip_addr_ = "192.168.123.161";
    int target_port_ = 8082;
    int local_port_ = 8090;
    UNITREE_LEGGED_SDK::UDP udp_connection_;

    UNITREE_LEGGED_SDK::HighCmd high_cmd_ = {};
    UNITREE_LEGGED_SDK::HighState high_state_ = {};

    // Robot settings
    mode_enum curr_mode_ = mode_enum::MODE_IDDLE;
    gaitype_enum curr_gait_type_ = gaitype_enum::GAITYPE_IDDLE;
    velocity_t curr_velocity_cmd_ = {0, 0, 0};
    uint8_t speed_level_ = speed_level_enum::LOW_SPEED;
    bool use_obstacle_avoidance_ = false;

    // Faceled
    FaceLightClient light_client_;

    // Robot status
    robot_status_e robot_status = robot_status_e::IDDLE;

    // Threads
    std::thread face_led_thread_;
    std::atomic<bool> face_led_thread_stop_flag_;
    std::thread recv_state_thread_;
    std::atomic<bool> recv_state_thread_stop_flag_;

   public:
    UnitreeDriver(std::string ip_addr = "192.168.123.161", int target_port = 8082);
    /*
     * @brief Deconstructor for the class UnitreeDriver. When called, it makes
     * the robot stand down and deactivate the motors.
     */
    ~UnitreeDriver();

    void stand_up();

    void stand_down();

    void damping_mode();

    void walk_w_vel(float x, float y, float yaw);

    void walk_w_pos(position_t position, quarternion_t orientation);

    odom_t get_odom();

    UNITREE_LEGGED_SDK::IMU get_imu();

    UNITREE_LEGGED_SDK::BmsState get_bms();

    sensor_ranges_t get_radar_ranges();

    uint8_t get_battery_percentage();

    void set_mode(mode_enum mode);

    void set_gaitype(gaitype_enum gaitype);

    void enable_obstacle_avoidance(bool flag);

    void stop();

    void set_head_led(uint8_t r, uint8_t g, uint8_t b);

    void set_head_led(UNITREE_LEGGED_SDK::LED led);

    void update_robot_status();

   private:
    void send_high_cmd_();

    void recv_high_state_();

    bool is_connection_established_();

    position_t get_position_();

    quarternion_t get_quaternion_();

    velocity_t get_velocity_();

    void blink_face_led(uint8_t r, uint8_t g, uint8_t b);
    void blink_face_led(UNITREE_LEGGED_SDK::LED led);
};

#endif  // !#ifndef UNITREE_DRIVER_H