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 <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;
}
 修改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






评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值