# test

**Repository Path**: gy_luyang/test

## Basic Information

- **Project Name**: test
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-03-09
- **Last Updated**: 2025-05-14

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

#ifndef _PERFECT_DRONE_SIM_HPP_
#define _PERFECT_DRONE_SIM_HPP_

#include "ros/ros.h"
#include "visualization_msgs/Marker.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"
#include "tf2_ros/transform_broadcaster.h"
#include "string"
#include "Eigen/Dense"
#include "nav_msgs/Path.h"
#include "perfect_drone_sim/config.hpp"

typedef Eigen::Matrix<double, 3, 1> Vec3;
typedef Eigen::Matrix<double, 3, 3> Mat33;

namespace perfect_drone {
    class PerfectDrone {
        Config cfg_;

    public:
        PerfectDrone(const ros::NodeHandle &n) : nh_(n) {
            odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/quadrotor/odom", 100);
            pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/quadrotor/pose", 100);
            robot_pub_ = nh_.advertise<visualization_msgs::Marker>("robot", 100);
            path_pub_ = nh_.advertise<nav_msgs::Path>("path", 100);

            float init_x, init_y, init_z;
            nh_.param("init_x", init_x, 0.0f);
            nh_.param("init_y", init_y, 0.0f);
            nh_.param("init_z", init_z, 1.0f);

            position_ = Vec3(init_x, init_y, init_z);
            velocity_.setZero();
            yaw_ = 0.0;
            nh_.param("mesh_resource", mesh_resource_, std::string("package://perfect_drone_sim/meshes/yunque-M.dae"));
            q_ = Eigen::AngleAxisd(yaw_, Vec3::UnitZ());
            odom_.header.frame_id = "world";
            odom_pub_timer_ = nh_.createTimer(ros::Duration(0.01), &PerfectDrone::publishOdom, this);

            path_.poses.clear();
            path_.header.frame_id = "world";
            path_.header.stamp = ros::Time::now();
        }

        ~PerfectDrone() {}

    private:
        nav_msgs::Path path_;
        ros::Publisher odom_pub_, robot_pub_, pose_pub_, path_pub_;
        ros::Timer odom_pub_timer_;
        ros::NodeHandle nh_;
        Vec3 position_, velocity_;
        double yaw_;
        Eigen::Quaterniond q_;
        nav_msgs::Odometry odom_;
        std::string mesh_resource_;

        void publishOdom(const ros::TimerEvent &e) {
            static double t = 0.0;
            const double radius = 2.0; // 圆的半径
            const double omega = 0.5; // 角速度
            const double z0 = 1.0; // 高度

            // 计算当前时刻在圆形轨迹上的位置
            double x = radius * cos(omega * t);
            double y = radius * sin(omega * t);

            // 计算当前时刻的速度
            double vx = -radius * omega * sin(omega * t);
            double vy = radius * omega * cos(omega * t);

            // 更新时间
            t += 0.01; // 假设时间步长为 0.01 秒

            // 设置位置和速度
            position_ = Vec3(x, y, z0); // 高度固定为 z0
            velocity_ = Vec3(vx, vy, 0.0); // 速度始终在 xy 平面内
            yaw_ = atan2(vy, vx); // 偏航角
            updateFlatness(position_, velocity_, Vec3(0, 0, 0), yaw_);

            odom_.pose.pose.position.x = position_.x();
            odom_.pose.pose.position.y = position_.y();
            odom_.pose.pose.position.z = position_.z();

            odom_.pose.pose.orientation.x = q_.x();
            odom_.pose.pose.orientation.y = q_.y();
            odom_.pose.pose.orientation.z = q_.z();
            odom_.pose.pose.orientation.w = q_.w();

            odom_.twist.twist.linear.x = velocity_.x();
            odom_.twist.twist.linear.y = velocity_.y();
            odom_.twist.twist.linear.z = velocity_.z();

            odom_.header.stamp = ros::Time::now();

            odom_pub_.publish(odom_);

            geometry_msgs::PoseStamped pose;
            pose.pose = odom_.pose.pose;
            pose.header = odom_.header;
            pose_pub_.publish(pose);

            visualization_msgs::Marker meshROS;
            meshROS.header.frame_id = "world";
            meshROS.header.stamp = odom_.header.stamp;
            meshROS.ns = "mesh";
            meshROS.id = 0;
            meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
            meshROS.action = visualization_msgs::Marker::ADD;
            meshROS.pose.position = odom_.pose.pose.position;
            meshROS.pose.orientation = odom_.pose.pose.orientation;
            meshROS.scale.x = 1;
            meshROS.scale.y = 1;
            meshROS.scale.z = 1;
            meshROS.mesh_resource = mesh_resource_;
            meshROS.mesh_use_embedded_materials = true;
            meshROS.color.a = 1.0;
            meshROS.color.r = 1.0;
            meshROS.color.g = 1.0;
            meshROS.color.b = 1.0;
            robot_pub_.publish(meshROS);
            static int slow_down = 0;
            if (slow_down++ % 10 == 0) {
                path_.poses.push_back(pose);
                path_.header = odom_.header;
                path_pub_.publish(path_);
            }
        }

        void updateFlatness(const Vec3 &pos, const Vec3 &vel,
                            const Vec3 &acc, const double yaw) {
            Vec3 gravity_ = 9.80 * Eigen::Vector3d(0, 0, 1);
            position_ = pos;
            velocity_ = vel;
            double a_T = (gravity_ + acc).norm();
            Eigen::Vector3d xB, yB, zB;
            Eigen::Vector3d xC(cos(yaw), sin(yaw), 0);

            zB = (gravity_ + acc).normalized();
            yB = ((zB).cross(xC)).normalized();
            xB = yB.cross(zB);
            Eigen::Matrix3d R;
            R << xB, yB, zB;
            q_ = Eigen::Quaterniond(R);
        }
    };
}

#endif