使用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