ros入门（9）IMU消息获取

啊不卷

已于 2024-05-16 14:55:50 修改

阅读量402

点赞数 8

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

于 2024-03-09 20:57:06 首次发布

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

版权

ros入门专栏收录该内容

12 篇文章 0 订阅

订阅专栏

本文介绍了如何在ROS（RobotOperatingSystem）环境中创建一个名为imu_node的节点，该节点从imu_data话题接收传感器数据，计算并调整姿态，然后发布cmd_vel指令进行控制。着重展示了IMU回调函数和姿态转换的过程。

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

cd catkin_ws2/src/

catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

#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_cmd.linear.x=0.1;

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;

}