Method G1Control

HomieDeploy/unitree_sdk2/g1_control.cpp:197–238

 public:
  G1Control(std::string networkInterface)
      : time_(0.0),
        control_dt_(0.005),
        duration_(3.0),
        mode_(PR),
        mode_machine_(0) {
    ChannelFactory::Instance()->Init(0, networkInterface);

    // create publisher
    lowcmd_publisher_.reset(
        new ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(HG_CMD_TOPIC));
    lowcmd_publisher_->InitChannel();

    // create subscriber
    lowstate_subscriber_.reset(
        new ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
            HG_STATE_TOPIC));
    lowstate_subscriber_->InitChannel(
        std::bind(&G1Control::LowStateHandler, this, std::placeholders::_1), 1);

    // create gamepad
    joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
    joystick_subscriber->InitChannel(std::bind(&G1Control::JoystickHandler, this, std::placeholders::_1), 1);


    // create threads
    command_writer_ptr_ =
        CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 5000,
                                &G1Control::LowCommandWriter, this);
    control_thread_ptr_ = CreateRecurrentThreadEx(
        "control", UT_CPU_ID_NONE, 5000, &G1Control::Control, this);
    joystick_thread_ptr_ = CreateRecurrentThreadEx(
        "nn_ctrl", UT_CPU_ID_NONE, 4000, &G1Control::JoystickStep, this);

    // add lcm subscriber
    _simpleLCM.subscribe("pd_plustau_targets", &G1Control::handleActionLCM, this);
    _simpleLCM.subscribe("arm_action", &G1Control::handleArmLCM, this);
    _simple_LCM_thread = std::thread(&G1Control::_simpleLCMThread, this);
    _firstCommandReceived = false;

    // todo: set nominal pose
  }

  void JoystickHandler(const void *message)
    {