IMU航向锁定

3 篇文章 0 订阅


一、IMU消息包格式

示例:pandas 是基于NumPy 的一种工具,该工具是为了解决数据分析任务而创建的。

二、节点发布+订阅的应用:IMU航向锁定

在这里插入图片描述实现步骤

1、构建一个新的软件包,包名叫做imu_pkg
2、在软件包中新建一个节点,节点名叫做imu_node。
3、在节点中,向ROS大管家NodeHandle申请订阅话题/imu/data并设置回调函数为IMUCallback0)。
4、构建回调函数IMUCallback() ,用来接收和处理IMU数据
5、使用TF工具将四元数转换成欧拉角。
6、调用ROS_INFOO显示转换后的欧拉角数值
7、让大管家 NodeHandle 发布速度控制话题 /cmd_ve
8、设定一个目标朝向角,当姿态信息中的朝向角和目标朝向角不致时,控制机器人转向目标向角。

进入catkin_ws/src目录:

catkin_create_pkg imu_pkg rospy roscpp sensor_msgs

进入src目录创建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;

// IMU 回调函数
void IMUCallback(const 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);
    
    // 速度消息包
    geometry_msgs::Twist vel_cmd;
    // 目标朝向角
    double target_yaw = 90;
    // 计算速度
    double diff_angle = target_yaw - yaw;
    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, "demo_imu_behavior"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 发布速度控制话题
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();

    return 0;
}

修改CMakeLists.txt

add_executable(imu_nodesrc/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

此时机器人会保持yaw = 90°前进
在这里插入图片描述

  • 4
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值