92 lines
3.2 KiB
C++
92 lines
3.2 KiB
C++
#include "line_laser_modbus/host.hpp"
|
|
#include "line_laser_modbus/motion_adapter.hpp"
|
|
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
|
|
namespace {
|
|
|
|
void print_frame(const char* title, const line_laser_modbus::ByteVector& frame) {
|
|
std::cout << title;
|
|
for (const std::uint8_t byte : frame) {
|
|
std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
|
|
<< static_cast<int>(byte) << ' ';
|
|
}
|
|
std::cout << std::dec << '\n';
|
|
}
|
|
|
|
class DemoBoardMotion final : public line_laser_modbus::MotionControlAdapter {
|
|
public:
|
|
void on_mode_changed(const line_laser_modbus::WorkMode mode) override {
|
|
mode_ = mode;
|
|
std::cout << "adapter: mode changed to "
|
|
<< static_cast<std::uint16_t>(mode_) << '\n';
|
|
}
|
|
|
|
void on_target_pose(const line_laser_modbus::Pose6D& pose) override {
|
|
target_pose_ = pose;
|
|
std::cout << "adapter: target pose timestamp "
|
|
<< target_pose_.timestamp_ms << '\n';
|
|
}
|
|
|
|
void on_correction(const line_laser_modbus::Pose6D& correction) override {
|
|
correction_ = correction;
|
|
current_pose_.x += correction_.x;
|
|
current_pose_.y += correction_.y;
|
|
current_pose_.z += correction_.z;
|
|
std::cout << "adapter: correction applied dx=" << correction_.x
|
|
<< ", dy=" << correction_.y << ", dz=" << correction_.z << '\n';
|
|
}
|
|
|
|
[[nodiscard]] line_laser_modbus::Pose6D current_pose() const override {
|
|
return current_pose_;
|
|
}
|
|
|
|
[[nodiscard]] line_laser_modbus::DeviceState current_state() const override {
|
|
if (mode_ == line_laser_modbus::WorkMode::OnlineTracking) {
|
|
return line_laser_modbus::DeviceState::OnlineTrackingNormal;
|
|
}
|
|
return line_laser_modbus::DeviceState::StandbyReady;
|
|
}
|
|
|
|
private:
|
|
line_laser_modbus::WorkMode mode_ =
|
|
line_laser_modbus::WorkMode::StandbyReset;
|
|
line_laser_modbus::Pose6D current_pose_{100U, 10.0F, 20.0F, 30.0F,
|
|
1.0F, 2.0F, 3.0F};
|
|
line_laser_modbus::Pose6D target_pose_{};
|
|
line_laser_modbus::Pose6D correction_{};
|
|
};
|
|
|
|
} // 匿名命名空间
|
|
|
|
int main() {
|
|
line_laser_modbus::HostClient host;
|
|
line_laser_modbus::DeviceServer server;
|
|
DemoBoardMotion board;
|
|
line_laser_modbus::MotionControlBridge bridge(server, board);
|
|
|
|
const auto mode_request =
|
|
host.make_write_mode_request(line_laser_modbus::WorkMode::OnlineTracking);
|
|
const auto mode_response = bridge.process_request(mode_request);
|
|
print_frame("write mode response: ", mode_response);
|
|
|
|
const line_laser_modbus::Pose6D target{200U, 100.0F, 200.0F, 300.0F,
|
|
10.0F, 20.0F, 30.0F};
|
|
const auto target_response =
|
|
bridge.process_request(host.make_write_target_pose_request(target));
|
|
print_frame("write target response: ", target_response);
|
|
|
|
const line_laser_modbus::Pose6D correction{250U, 0.5F, -0.25F, 1.0F,
|
|
0.0F, 0.0F, 0.0F};
|
|
const auto correction_response =
|
|
bridge.process_request(host.make_write_correction_request(correction));
|
|
print_frame("write correction response: ", correction_response);
|
|
|
|
const auto pose_response =
|
|
bridge.process_request(host.make_read_current_pose_request());
|
|
print_frame("read pose response: ", pose_response);
|
|
|
|
return 0;
|
|
}
|