ROS使用Xsens传感器

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

3. Xsens控制Turtle移动

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

1. #include <ros/ros.h>

2. #include <geometry_msgs/Twist.h>

3. #include <sensor_msgs/Imu.h>

4. #include <iostream>

5. #include <tf/LinearMath/Matrix3x3.h>

6. #include <tf/LinearMath/Quaternion.h>

7.  

8. using namespace std;

9.  

10. class Imu_Test

11. {

12. public:

13. Imu_Test();

14. private:

15. void CallBack(const sensor_msgs::Imu::ConstPtr& imu);

16. ros::NodeHandle n;

17. ros::Publisher pub;

18. ros::Subscriber sub;

19. };

20.  
21.  

22. Imu_Test::Imu_Test()

23. {

24. pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);

25. sub = n.subscribe<sensor_msgs::Imu>("imu/data", 10, &Imu_Test::CallBack, this);

26. }

27.  

28. void Imu_Test::CallBack(const sensor_msgs::Imu::ConstPtr& imu)

29. {

30. geometry_msgs::Twist vel;

31. tf::Quaternion bq(imu->orientation.x, imu->orientation.y, imu->orientation.z, imu->orientation.w);

32. double roll, pitch, yaw;

33. tf::Matrix3x3(bq).getRPY(roll, pitch, yaw);

34. ROS_INFO("%lf %lf %lf", roll, pitch, yaw);

35. vel.angular.z = roll;

36. vel.linear.x = pitch;

37. pub.publish(vel);

38. }

39.  

40. int main(int argc, char **argv)

41. {

42. ros::init(argc, argv, "IMU_Turtle");

43. Imu_Test imu_test;

44. ros::spin();

45. return 0;

46. }

 修改CMakeLists.txt

1. add_executable(IMU_Turtle_Test src/IMU_Turtle_Test.cpp)

2.  

3. add_dependencies(IMU_Turtle_Test ROS_Test2_generate_message_cpp)

4.  

5. target_link_libraries(IMU_Turtle_Test ${catkin_LIBRARIES})

 编译运行

 rosrun turtlesim turtlesim_node

 rosrun ROS_Test2 IMU_Turtle_Test

 

个人分类： ROS系统

相关热词： ros 句柄ros 古月ros 图灵机器人ros 单片机ros