使用xsens控制Turtle移动

秒速五厘米 2022-03-11 01:30 219阅读 0赞

# **使用xsens MTi -28控制Turtle移动** #

## 1.首先安装Xsens的驱动（kinetic）版本 ##

sudo apt-get install ros-kinetic-xsens-driver
     rosstack profile
     rospack profile

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70][]

## 2. 启动Xsens，查看输出数据 ##

sudo chmod 777 /dev/ttyUSB*
     roslaunch xsens_driver xsens_driver.launch
     rostopic list
     rostopic echo /imu/data

第一条命令为将USB设置为最高权限，防止因权限不够而无法驱动

有时发现执行第二条语句后终端出现：Got MTi data packet: 'RAW', ignored!

请将MTi的设备输出格式设置下（或者复位下），利用MT Manager软件。

## 3. 编程实现Xsens控制Turtle移动 ##

### 3.1创建包(添加依赖roscpp sensor\_msgs std\_msgs tf geometry\_msgs) ###

catkin_create_pkg IMU_Turtle_Test std_msgs rospy roscpp tf geometry_msgs sensor_msgs

### 3.2在此包下的src文件夹下创建IMU\_Turtle\_Test.cpp ###

#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;
    }

### 3.3修改CMakeLists.txt(添加以下内容) ###

add_executable(IMU_Turtle_Test src/IMU_Turtle_Test.cpp)
    add_dependencies(IMU_Turtle_Test IMU_Turtle_Test_generate_message_cpp)
    target_link_libraries(IMU_Turtle_Test ${catkin_LIBRARIES})

3.4编译运行

roslaunch xsens_driver xsens_driver.launch
    rosrun turtlesim turtlesim_node
    source ~/catkin_ws/devel/setup.bash
    rosrun IMU_Turtle_Test IMU_Turtle_Test

![网速差劲，刷新页面试试][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]: /images/20220311/810bd3da25cb460b9030a844e27f7214.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]: /images/20220311/4babee4341cc410283f5bb3fa757861e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2]: /images/20220311/0a20482d261342ffbc688fef2fa36b7d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3]: /images/20220311/69789163161843d4966ab2b5c22112dd.png