ROS使用Xsens传感器

 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 
#include 
#include 
#include 
#include 
#include 

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("/turtle1/cmd_vel", 1);
	sub = n.subscribe("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