ROS使用Xsens传感器

最新推荐文章于 2024-09-25 14:34:33 发布

我要和你讲道理

最新推荐文章于 2024-09-25 14:34:33 发布

阅读量1w

点赞数 4

分类专栏： ROS系统

本文链接：https://blog.csdn.net/qq_31356389/article/details/79216765

版权

ROS系统专栏收录该内容

7 篇文章 7 订阅

订阅专栏

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

1. 安装驱动（Hydro版本）

笔者使用的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

3. Xsens控制Turtle移动

在功能包下的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;
}

修改CMakeLists.txt

add_executable(IMU_Turtle_Test src/IMU_Turtle_Test.cpp)

add_dependencies(IMU_Turtle_Test ROS_Test2_generate_message_cpp)

target_link_libraries(IMU_Turtle_Test ${catkin_LIBRARIES})

编译运行

rosrun turtlesim turtlesim_node

rosrun ROS_Test2 IMU_Turtle_Test