本文主要是介绍ROS学习记录:用C++实现IMU航向锁定,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
前言
获取IMU数据的C++节点
在了解了如何获取到IMU的姿态信息(链接在上面)后,接下来尝试实现让一个节点在订阅IMU数据的时候,还能发布运动控制指令,使机器人能对姿态变化做出反应,达到一个航向锁定的效果。
一、实现步骤
二、开始操作
1、打开一个终端,输入cd ~/catkin_ws1/src,进入工作空间
2、输入 code . 打开VScode
3、在VScode中打开上一篇文章编写的imu_node.cpp, 上一篇文章链接已经放在开头
4、在imu_node.cpp中写入如下代码
#include "ros/ros.h" //包含ros头文件
#include "sensor_msgs/Imu.h" //包含sensor_msgs/Imu消息类型头文件
#include "tf/tf.h"//用于使用TF工具,将四元素转换为欧拉角
#include "geometry_msgs/Twist.h"//引入速度消息包的头文件ros::Publisher vel_pub;//定义一个发布对象vel_pubvoid IMUCallback(sensor_msgs::Imu msg) //IMU消息回调函数
{if(msg.orientation_covariance[0]<0) //检查协方差,确保数据有效性return; //如果协方差小于0,数据无效,直接返回tf::Quaternion quaternion( //创建四元素msg.orientation.x, //从IMU消息中获取四元素数据msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll,pitch,yaw; //定义滚转、俯仰、偏航角tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);// 利用TF库将四元数转换为欧拉角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;// 创建Twist类型消息对象用于发布速度指令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节点ros::NodeHandle n; // 创建节点句柄ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback); // 创建imu_sub订阅者,订阅IMU数据消息vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);//创建速度指令发布器ros::spin();// 进入自发循环,阻塞程序直至节点关闭return 0;
}
5、按CTRL+S进行保存,再按CTRL+SHIFT进行编译,编译成功
6、在终端中输入cd ~/catkin_ws1,进入工作空间
7、再输入source ./devel/setup.bash,设置ROS的环境变量,以便在当前的终端窗口中正确运行ROS软件包。
8、再输入roslaunch wpr_simulation wpb_simple.launch,启动机器人仿真环境
9、再打开一个终端,进入工作空间后输入source ./devel/setup.bash,设置环境变量
10、输入rosrun imu_pkg imu_node运行刚刚我们更新的节点
11、可以看到机器人偏航角锁定在了90度
12、这时因为在代码中,我们设定了目标偏航角为90度,如果想要机器人朝向其它角度可以自行修改
13、点这个旋转按钮,对机器人进行旋转,拖动这个蓝色的圈圈,机器人转动后,一松开鼠标,可以发现机器人会自行转回去直至目标角度90度,这便是偏航角锁定。
ROS机器人偏航角锁定演示
14、回到节点代码,在这里加上这一句代码,给机器一个前进的速度,可以预想到,机器人会一边前进一边转弯。
#include "ros/ros.h" //包含ros头文件
#include "sensor_msgs/Imu.h" //包含sensor_msgs/Imu消息类型头文件
#include "tf/tf.h"//用于使用TF工具,将四元素转换为欧拉角
#include "geometry_msgs/Twist.h"//引入速度消息包的头文件ros::Publisher vel_pub;//定义一个发布对象vel_pubvoid IMUCallback(sensor_msgs::Imu msg) //IMU消息回调函数
{if(msg.orientation_covariance[0]<0) //检查协方差,确保数据有效性return; //如果协方差小于0,数据无效,直接返回tf::Quaternion quaternion( //创建四元素msg.orientation.x, //从IMU消息中获取四元素数据msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll,pitch,yaw; //定义滚转、俯仰、偏航角tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);// 利用TF库将四元数转换为欧拉角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;// 创建Twist类型消息对象用于发布速度指令vel_cmd.angular.z = diff_angle*0.01;// 计算角速度vel_cmd.linear.x = 0.1;//给机器人x轴方向一个0.1m/s的线速度vel_pub.publish(vel_cmd);// 发布速度指令
}int main(int argc, char *argv[]) // 主函数
{setlocale(LC_ALL,""); // 设置本地区域选项ros::init(argc,argv,"imu_node"); // 初始化ROS节点ros::NodeHandle n; // 创建节点句柄ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback); // 创建imu_sub订阅者,订阅IMU数据消息vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);//创建速度指令发布器ros::spin();// 进入自发循环,阻塞程序直至节点关闭return 0;
}
15、CTRL+S保存后,在按CTRL+SHIFT编译,编译成功
16、先按CTRL+Z停止运行刚刚的节点,在重新运行节点
17、可以看到当拉动蓝色的圈圈后,机器人一边转一边前进。
这篇关于ROS学习记录:用C++实现IMU航向锁定的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!