io_gripper_sdk/main.cpp at master · ioai-tech/io_gripper_sdk · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
#include <yaml-cpp/yaml.h>

#include <chrono>
#include <iostream>
#include <thread>

#include "include/io_gripper.hpp"

using namespace io::gripper;

DeviceProfile create_gripper_driver(const std::string& config_file_path) {
  DeviceProfile profile;
  YAML::Node config = YAML::LoadFile(config_file_path);
  auto DeviceProfile_Node = config["DeviceProfile"];
  // 基础信息
  profile.model_name =
      DeviceProfile_Node["device_info"]["model_name"].as<std::string>();
  profile.servo_id = static_cast<uint8_t>(
      DeviceProfile_Node["device_info"]["servo_id"].as<int>());
  profile.recommended_baudrate =
      DeviceProfile_Node["device_info"]["recommended_baudrate"].as<int>();

  // 安全限制
  profile.min_voltage_V =
      DeviceProfile_Node["safety_limits"]["min_voltage_v"].as<float>();
  profile.max_voltage_V =
      DeviceProfile_Node["safety_limits"]["max_voltage_v"].as<float>();
  profile.max_temperature_C =
      DeviceProfile_Node["safety_limits"]["max_temperature_c"].as<float>();
  profile.start_power =
      DeviceProfile_Node["safety_limits"]["start_pow"].as<uint8_t>(8);
  profile.release_torque_on_disconnect =
      DeviceProfile_Node["safety_limits"]["release_torque_on_disconnect"]
          .as<bool>(true);
  profile.max_servo_velocity =
      DeviceProfile_Node["safety_limits"]["max_servo_velocity"].as<uint16_t>(
          2000);

  // 标定参数
  profile.calib_max_position_raw =
      DeviceProfile_Node["calibration"]["calib_max_position_raw"]
          .as<uint16_t>();
  profile.calib_min_position_raw =
      DeviceProfile_Node["calibration"]["calib_min_position_raw"]
          .as<uint16_t>();
  profile.calib_max_width_mm =
      DeviceProfile_Node["calibration"]["calib_max_width_mm"].as<float>();
  profile.calib_min_width_mm =
      DeviceProfile_Node["calibration"]["calib_min_width_mm"].as<float>();

  return profile;
}

// 用夹爪语义控制
GripperCommand cmd_gripper_move(std::string config_path) {
  try {
    // 重新加载配置
    YAML::Node config_root = YAML::LoadFile("../gripper_config.yaml");
    auto gripper_node = config_root["gripper"]["use_width_normalized"];

    GripperCommand cmd;
    cmd.use_width_mm = gripper_node["use_width_mm"].as<bool>();
    cmd.use_normalized_opening =
        gripper_node["use_normalized_opening"].as<bool>();
    cmd.width_mm = gripper_node["width_mm"].as<float>();
    cmd.normalized_opening = gripper_node["normalized_opening"].as<float>();
    cmd.max_effort = gripper_node["max_effort"].as<float>();
    cmd.speed = gripper_node["speed"].as<float>();

    return cmd;
  } catch (const std::exception& e) {
    std::cerr << "加载动作配置失败: " << e.what() << std::endl;
    throw;
  }
}

int main() {
  try {
    std::cout << "正在尝试初始化库..." << std::endl;

    // 查找端口
    std::unique_ptr<find_port> port_;
    std::vector<CameraInfo> res_caminfo = port_->get_usb_cameras_info();
    std::string first_port = port_->find_by_path_from_tty(
        port_->resolve_gripper_by_camera_serial(res_caminfo[0].serial));

    std::cout << "first_port: " << first_port << std::endl;

    std::string path = "../gripper_config.yaml";

    // 配置profile文件
    DeviceProfile profile = create_gripper_driver(path);
    auto driver = std::make_unique<GripperDriver>(first_port, profile, path);

    if (driver->connect() && driver->ping(profile.servo_id)) {
      std::cout << "初始化" << std::endl;
      driver->initialize({profile.servo_id});

      std::cout << "执行标定" << std::endl;
      driver->calibrate();

      std::this_thread::sleep_for(std::chrono::milliseconds(2000));

      std::cout << "位置的最大弧度：" << driver->getmaxpos_rad() << std::endl;
      std::cout << "速度-位置控制" << std::endl;
      driver->commandPosition(profile.servo_id, std::nullopt, 0.689);

      std::this_thread::sleep_for(std::chrono::milliseconds(3000));

      driver->commandVelocity(profile.servo_id, 1000, std::nullopt,
                              std::nullopt);
      driver->commandPosition(profile.servo_id, 2700, std::nullopt);
      std::this_thread::sleep_for(std::chrono::milliseconds(2000));

      // 加载配置文件，使用夹爪语义
      std::cout << "---夹爪语义控制---" << std::endl;
      driver->commandGripper(cmd_gripper_move(path));
      std::this_thread::sleep_for(std::chrono::milliseconds(3000));

      // 抓取物体
      std::cout << "----抓取物体----" << std::endl;
      driver->pickObject(profile.servo_id, 20, 150, 500, 1000);
      std::this_thread::sleep_for(std::chrono::milliseconds(2000));

      driver->disconnect();
    }
  } catch (const std::exception& e) {
    std::cerr << "发生异常: " << e.what() << std::endl;
  }
  return 0;
}