ros入门（10）IMU航向锁定

啊不卷

已于 2024-05-16 14:56:17 修改

阅读量374

点赞数 5

分类专栏： ros入门文章标签：机器人

于 2024-03-09 21:14:57 首次发布

本文链接：https://blog.csdn.net/tingwods/article/details/136591070

版权

ros入门专栏收录该内容

12 篇文章 0 订阅

订阅专栏

本文介绍了如何在ROS（RobotOperatingSystem）环境中，通过解析IMU传感器数据，计算并实时调整姿态（包括滚转、俯仰和朝向），实现一个简单的控制节点，以便在`imu_node`中驱动`cmd_vel`话题进行导航。

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

#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::Quaternion quaternion(

msg.orientation.x,

msg.orientation.y,

msg.orientation.z,

msg.orientation.w

);

double 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("滚转= %.0f 俯仰= %.0f 朝向= %.0f",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,"");

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;

}