main.cpp - master - tracie/Sirius_projects

main.cpp
152 строки5 KB
Ksenia Vasileva
done
13 окт 2025, 07:51
13 окт 2025, 07:517ae8d58
#include <iostream>
#include <vector>
#include <cmath>
#include <chrono>
#include <thread>
#include <boost/asio.hpp>
#include <Eigen/Dense>
#include "egm.pb.h"
using namespace abb;
using namespace std;
using namespace boost::asio;
using namespace Eigen;
using ip::udp;
using namespace std::chrono;

// Настройки UDP
const string ROBOT_IP = "169.254.127.243";
const int ROBOT_PORT = 1025;

// Функция для вычисления расстояния между двумя точками
double distance(const Vector3d& p1, const Vector3d& p2) {
    return (p1 - p2).norm();
}

// Функция линейной интерполяции между двумя точками
vector<VectorXd> linearInterpolation(const VectorXd& start, const VectorXd& end) {
    vector<VectorXd> interpolated_points;

    Vector3d start_coords = start.head<3>();
    Vector3d end_coords = end.head<3>();
    Vector4d start_quat = start.tail<4>();

    double total_distance = distance(start_coords, end_coords);
    double max_distance = 1.5;
    int num_points = static_cast<int>(ceil(total_distance / max_distance)) + 1;

    vector<double> t_values(num_points);
    for (int i = 0; i < num_points; ++i) {
        t_values[i] = sin((i / double(num_points - 1)) * M_PI / 2);
    }

    for (double t : t_values) {
        VectorXd point(7);
        point.head<3>() = (1 - t) * start_coords + t * end_coords;
        point.tail<4>() = start_quat;
        interpolated_points.push_back(point);
    }

    return interpolated_points;
}

// Функция для генерации точек по окружности
vector<VectorXd> generateCirclePoints(const VectorXd& start, const VectorXd& end, int num_points) {
    vector<VectorXd> points;

    Vector3d start_coords = start.head<3>();
    Vector3d end_coords = end.head<3>();
    Vector3d center = (start_coords + end_coords) / 2;
    double radius = (start_coords - center).norm();

    for (int i = 0; i < num_points; ++i) {
        double theta = (i / double(num_points - 1)) * 2 * M_PI;
        VectorXd point(7);
        point << center[0] + radius * sin(theta),
                 center[1] + radius * cos(theta),
                 start_coords[2],
                 0, 0, 0, 1;  // Кватернион (без вращения)
        points.push_back(point);
    }

    return points;
}

// Создание EGM-сообщения
egm::EgmSensor createSensorMessage(const VectorXd& point) {
    egm::EgmSensor sensor_message;

    // Позиция
    egm::EgmCartesian* cart = new egm::EgmCartesian();
    cart->set_x(point[0]);
    cart->set_y(point[1]);
    cart->set_z(point[2]);

    // Кватернион
    egm::EgmQuaternion* quat = new egm::EgmQuaternion();
    quat->set_u0(point[3]);
    quat->set_u1(point[4]);
    quat->set_u2(point[5]);
    quat->set_u3(point[6]);

    egm::EgmPose* pose = new egm::EgmPose();
    pose->set_allocated_pos(cart);
    pose->set_allocated_orient(quat);

    egm::EgmPlanned* planned = new egm::EgmPlanned();
    planned->set_allocated_cartesian(pose);

    sensor_message.set_allocated_planned(planned);

    // Заголовок
    egm::EgmHeader* header = new egm::EgmHeader();
    header->set_mtype(egm::EgmHeader::MSGTYPE_CORRECTION);
    header->set_seqno(0);
    header->set_tm(std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()));
    sensor_message.set_allocated_header(header);

    return sensor_message;
}

// Отправка сообщений через UDP
void sendMotion(io_service& io, udp::socket& socket, const vector<VectorXd>& points) {
    udp::endpoint robot_endpoint(ip::address::from_string(ROBOT_IP), ROBOT_PORT);

    for (const auto& point : points) {
        egm::EgmSensor sensor_message = createSensorMessage(point);

        string serialized_message;
        sensor_message.SerializeToString(&serialized_message);

        socket.send_to(buffer(serialized_message), robot_endpoint);
        this_thread::sleep_for(chrono::milliseconds(5));
    }
}

int main() {
    // Исходные точки
    VectorXd robtarget_start(7);
    robtarget_start << 800, 0, 1000, 0, 0, 0, 1;

    VectorXd robtarget_end(7);
    robtarget_end << 800, -200, 1000, 0, 0, 0, 1;

    // Инициализация Boost::Asio
    io_service io;
    udp::socket socket(io, udp::endpoint(udp::v4(), 1025));

    // Движение по прямой
    vector<VectorXd> path1 = linearInterpolation(robtarget_start, robtarget_end);
    sendMotion(io, socket, path1);
    this_thread::sleep_for(chrono::seconds(3));

    vector<VectorXd> path2 = linearInterpolation(robtarget_end, robtarget_start);
    sendMotion(io, socket, path2);
    this_thread::sleep_for(chrono::seconds(3));

    // Движение по окружности
    int num_circle_points = 600;
    vector<VectorXd> circle_points = generateCirclePoints(robtarget_start, robtarget_end, num_circle_points);
    sendMotion(io, socket, circle_points);

    return 0;
}