MSRSIM/quadrotor_dynamics_node.cpp

最新推荐文章于 2025-12-31 14:11:39 发布

原创

最新推荐文章于 2025-12-31 14:11:39 发布 · 181 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#cocos2d #游戏引擎

博客介绍了一段代码的主要作用。该代码从订阅主题‘cmd_RPM’获取四个旋翼电机转速命令并存储，通过转速前向积分等方法更新四旋翼无人机的位置、速度等状态，还会发布无人机状态信息，如里程计数据到‘odom’主题、IMU数据到‘imu’主题。

1.代码的主要作用

从订阅的主题 "cmd_RPM" 中获取四个旋翼电机的转速命令，存储在 RPM_input 中。
根据获取的转速命令，从转速前向积分等办法更新四旋翼无人机的状态，包括位置、速度、加速度、姿态等。
发布无人机的状态信息，包括里程计数据到 "odom" 主题和IMU数据到 "imu" 主题

2.代码

#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <quadrotor_dynamics.hpp>
#include <std_msgs/Float32MultiArray.h>

using namespace std;
using namespace Eigen;

std_msgs::Float32MultiArray RPM_msg;
Vector4d RPM_input;
void RPMCallbck(const std_msgs::Float32MultiArray& msg)
{
    RPM_msg = msg;
    RPM_input<< RPM_msg.data[0], RPM_msg.data[1],RPM_msg.data[2],RPM_msg.data[3];
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "quadrotor_dynamics");

    ros::NodeHandle n("~");


    double init_x, init_y, init_z,mass;
    double simulation_rate;
    std::string quad_name;
    n.param("mass", mass, 0.9);
    n.param("init_state_x", init_x