ROS基础学习（实现一个IMU航向锁定）（三）_ros使用imu数据计算航向-CSDN博客

本文链接：https://blog.csdn.net/kenneth666888/article/details/138142925

本文详细描述了如何在ROS环境中创建一个IMU节点，用于接收传感器数据并实现航向锁定，通过TF转换四元数到角度，根据目标角度调整并发布控制指令。

摘要由CSDN通过智能技术生成

实现一个IMU航向锁定

创建功能包

cd catkin_make/src
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

创建节点

imu_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;

void IMUCallback(sensor_msgs::Imu msg){
    if(msg.orientation_covariance[0]<0)
        return;
    //将消息中的四元素转换为tf的四元素对象
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll,pitch,yaw;
    //这个时候的roll,pitch,yaw是弧度
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);
    //下面转换成角度，更直观
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转 = %f, 俯仰 = %f, 朝向 = %f",roll,pitch,yaw);

    double target_yaw = 90;
    double diff_angle = target_yaw - yaw;
    geometry_msgs::Twist vel_cmd;
    vel_cmd.angular.z = diff_angle*0.01;
    vel_pub.publish(vel_cmd);


}

int main(int argc,char **argv)
{
    setlocale(LC_ALL,"zh_CN.UTF-8");
    ros::init(argc,argv,"imu_node");
    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();
    
    return 0;
}

创建编译规则

CMakeLists.txt

${PROJECT_NAME}_node 改为节点名字
cpp文件也改为对应文件

add_executable(imu_node src/imu_node.cpp)
add_dependencies(imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node
  ${catkin_LIBRARIES}
)

运行仿真环境并运行节点

roslaunch wpr_simulation wpb_simple.launch
//另开一个终端
rosrun imu_pkg imu_node