替别人做设计的网站,网站如何被搜索到,做搞笑图片的网站,中关村手机网前言
获取IMU数据的C节点 在了解了如何获取到IMU的姿态信息#xff08;链接在上面#xff09;后#xff0c;接下来尝试实现让一个节点在订阅IMU数据的时候#xff0c;还能发布运动控制指令#xff0c;使机器人能对姿态变化做出反应#xff0c;达到一个航向锁定的效果。 …前言
获取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.advertisegeometry_msgs::Twist(/cmd_vel,10);//创建速度指令发布器ros::spin();// 进入自发循环阻塞程序直至节点关闭return 0;
}5、按CTRLS进行保存再按CTRLSHIFT进行编译编译成功 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.advertisegeometry_msgs::Twist(/cmd_vel,10);//创建速度指令发布器ros::spin();// 进入自发循环阻塞程序直至节点关闭return 0;
}15、CTRLS保存后在按CTRLSHIFT编译编译成功 16、先按CTRLZ停止运行刚刚的节点在重新运行节点 17、可以看到当拉动蓝色的圈圈后机器人一边转一边前进。
