ROS使用Xsens传感器

最新推荐文章于 2022-10-10 09:19:03 发布

~AI钢铁侠~

最新推荐文章于 2022-10-10 09:19:03 发布

阅读量976

点赞数 1

分类专栏：机器人 ROS

47 篇文章 13 订阅

订阅专栏

25 篇文章 0 订阅

订阅专栏

Xsens传感器广泛应用于机器人，在机器人姿态量测或SLAM中重要的器件。本篇文章主要讲述Xsens传感器在ROS下的使用。

笔者使用的Xsens型号为MTI-30。首先安装Xsens的驱动（Hydro）版本。

sudo apt-get install ros-hydro-xsens-driver

rosstack profile

rospack profile

2. 测试Xsens工作

sudo chmod 777 /dev/ttyUSB*

roslaunch xsens_driver xsens_driver.launch

rostopic list

rostopic echo /imu/data

在功能包下的src文件夹下创建cpp文件（工作空间与功能包创建请参考博文【ROS节点订阅与发布】）

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include <iostream>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Quaternion.h>
using namespace std;
class Imu_Test
{
public:
Imu_Test();
private:
void CallBack(const sensor_msgs::Imu::ConstPtr& imu);
ros::NodeHandle n;
ros::Publisher pub;
ros::Subscriber sub;
};
Imu_Test::Imu_Test()
{
pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);
sub = n.subscribe<sensor_msgs::Imu>("imu/data", 10, &Imu_Test::CallBack, this);
}
void Imu_Test::CallBack(const sensor_msgs::Imu::ConstPtr& imu)
{
geometry_msgs::Twist vel;
tf::Quaternion bq(imu->orientation.x, imu->orientation.y, imu->orientation.z, imu->orientation.w);
double roll, pitch, yaw;
tf::Matrix3x3(bq).getRPY(roll, pitch, yaw);
ROS_INFO("%lf %lf %lf", roll, pitch, yaw);
vel.angular.z = roll;
vel.linear.x = pitch;
pub.publish(vel);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "IMU_Turtle");
Imu_Test imu_test;
ros::spin();
return 0;
}