使用xsens控制Turtle移动

使用xsens MTi -28控制Turtle移动

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

 sudo apt-get install ros-kinetic-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

第一条命令为将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