SLAM从入门到入土：维特惯性导航传感器，ROS程序

以WTGAHRS2为例。

代码为测试版，数据位置和转换不一定正确（比如经纬只能输出整数，四元数放在了GPS数据位置）。由于本人时间条件，来不及修改，请急需的自行修改。

#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "ImuGpsRaw/IMU_Data.h"
#include "ImuGpsRaw/GPS_Data.h"
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>using namespace std;
using namespace Eigen;Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  Eigen::Quaterniond q = rollAngle *yawAngle *pitchAngle;  return q;
}  int main(int argc, char** argv)
{ros::init(argc, argv, "Integrated_Navigation");ros::NodeHandle n;//创建imu数据话题发布ros::Publisher Pub_IMU_Data = n.advertise<ImuGpsRaw::IMU_Data>("Wit/IMU",100);ros::Publisher Pub_GPS_Data = n.advertise<ImuGpsRaw::GPS_Data>("Wit/GPS",100);//创建一个serial对象serial::Serial sp;//创建timeoutserial::Timeout to = serial::Timeout::simpleTimeout(100);//设置要打开的串口名称sp.setPort("/dev/ttyUSB0");//设置串口通信的波特率sp.setBaudrate(9600);//串口设置timeoutsp.setTimeout(to);double ax,ay,az;double wx,wy,wz;double angle_roll,angle_pitch,angle_yaw;double longitude, latitude, altitude;double GPS_Yaw, GPSV;double q0, q1, q2, q3;int satellite;double PDOP, HDOP, VDOP;Eigen::Quaterniond q;ImuGpsRaw::IMU_Data imu_data;ImuGpsRaw::GPS_Data GPS_data;try{//打开串口sp.open();}catch(serial::IOException& e){ROS_ERROR_STREAM("Unable to open port.");return -1;}//判断串口是否打开成功if(sp.isOpen()){ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");}else{return -1;}ros::Rate loop_rate(100);while(ros::ok()){//获取缓冲区内的字节数size_t n = sp.available();if(n!=0){uint8_t buffer[1024];//读出数据n = sp.read(buffer, n);for(int i=0; i<n; i++){if(buffer[i]==0X55)            //IMU地址{if(buffer[i+1]==0x51) //加速度帧{signed short int temp;temp=buffer[i+3];temp=temp<<8;temp=(temp|buffer[i+2]);ax=(temp/32768.0)*16*9.8051;temp=buffer[i+5];         //加速度八位temp=temp<<8;temp=(temp|buffer[i+4]);  //加速度高八位ay=(temp/32768.0)*16*9.8051;temp=buffer[i+7];         //加速度低八位temp=temp<<8;temp=(temp|buffer[i+6]);  //加速度高八位az=(temp/32768.0)*16*9.8051;// std::cout<<"ax:"<<ax<<std::endl;// std::cout<<"ay:"<<ay<<std::endl;// std::cout<<"az:"<<az<<std::endl;i+=11;}if(buffer[i+1]==0x52) //角速度帧{signed short int temp;temp=buffer[i+3];temp=temp<<8;temp=(temp|buffer[i+2]);wx=(temp/32768.0)*2000;temp=buffer[i+5];         //角速度低八位temp=temp<<8;temp=(temp|buffer[i+4]);  //角速度高八位wy=(temp/32768.0)*2000;temp=buffer[i+7];         //角速度低八位temp=temp<<8;temp=(temp|buffer[i+6]);  //角速度高八位wz=(temp/32768.0)*2000;// std::cout<<"wx:"<<wx<<std::endl;// std::cout<<"wy:"<<wy<<std::endl;// std::cout<<"wz:"<<wz<<std::endl;i+=11;}if(buffer[i+1]==0x53) //角度帧{unsigned short int temp;temp=buffer[i+3];         //角度低八位temp=temp<<8;temp=(temp|buffer[i+2]);  //角度高八位angle_roll=(temp/32768.0)*180;temp=buffer[i+5];         //角度低八位temp=temp<<8;temp=(temp|buffer[i+4]);  //角度高八位angle_pitch=(temp/32768.0)*180;temp=buffer[i+7];         //角度低八位temp=temp<<8;temp=(temp|buffer[i+6]);  //角度高八位angle_yaw=(temp/32768.0)*180;q = euler2Quaternion(angle_roll, angle_pitch, angle_yaw) ;// std::cout<<"angle_roll:"<<angle_roll<<std::endl;// std::cout<<"angle_pitch:"<<angle_pitch<<std::endl;// std::cout<<"angle_yaw:"<<angle_yaw<<std::endl;i+=11;}if(buffer[i+1]==0x57) //GPS帧{unsigned long int temp1;unsigned long int temp2;unsigned long int temp3;unsigned long int temp4;temp1=buffer[i+3];         //temp1=temp1<<8;temp2=buffer[i+4];         //temp2=temp2<<16;temp3=buffer[i+5];         //temp3=temp3<<24;temp4=(temp3|temp2|temp1|buffer[i+2]);  //longitude = int(temp4 /10000000)*100 + (temp4 % 10000000) / 100000;temp1=buffer[i+7];         //temp1=temp1<<8;temp2=buffer[i+8];         //temp2=temp2<<16;temp3=buffer[i+9];         //temp3=temp3<<24;temp4=(temp3|temp2|temp1|buffer[i+6]);  //latitude = int(temp4 /10000000)*100 + (temp4 % 10000000) / 100000;// std::cout<<"longitude :"<<longitude <<std::endl;// std::cout<<"latitude :"<<latitude <<std::endl;i+=11;}if(buffer[i+1]==0x58) //GPS帧{unsigned short int temp;unsigned long int temp1;unsigned long int temp2;unsigned long int temp3;unsigned long int temp4;temp=buffer[i+3];         //temp=temp<<8;temp=(temp|buffer[i+2]);  //altitude = temp / 10;temp=buffer[i+5];         //temp=temp<<8;temp=(temp|buffer[i+4]);  //GPS_Yaw = temp / 100;temp1=buffer[i+7];         //temp1=temp1<<8;temp2=buffer[i+8];         //temp2=temp2<<16;temp3=buffer[i+9];         //temp3=temp3<<24;temp4=(temp3|temp2|temp1|buffer[i+6]);  //GPSV = temp4 / 1000;// std::cout<<"altitude:"<<altitude<<std::endl;// std::cout<<"GPS_Yaw:"<<GPS_Yaw<<std::endl;// std::cout<<"GPSV:"<<GPSV<<std::endl;i+=11;}if(buffer[i+1]==0x59)  //四元数{signed short int temp;temp=buffer[i+3];temp=temp<<8;temp=(temp|buffer[i+2]);q0 = temp / 32768;temp=buffer[i+5];        temp=temp<<8;temp=(temp|buffer[i+4]);  q1 = temp / 32768;temp=buffer[i+7];       temp=temp<<8;temp=(temp|buffer[i+6]);  q2 = temp / 32768;temp=buffer[i+9];         temp=temp<<8;temp=(temp|buffer[i+8]);  q3 = temp / 32768;i+=11;}if(buffer[i+1]==0x5A) //GPS精度{signed short int temp;temp=buffer[i+3];temp=temp<<8;temp=(temp|buffer[i+2]);satellite = temp;temp=buffer[i+5];         temp=temp<<8;temp=(temp|buffer[i+4]);  PDOP = temp / 100;temp=buffer[i+7];         temp=temp<<8;temp=(temp|buffer[i+6]);  HDOP = temp / 100;temp=buffer[i+9];         temp=temp<<8;temp=(temp|buffer[i+8]);  VDOP = temp / 100;i+=11;}imu_data.IMU_Data.header.stamp = ros::Time::now();imu_data.IMU_Data.header.frame_id = "base_link";imu_data.IMU_Data.linear_acceleration.x = ax;imu_data.IMU_Data.linear_acceleration.y = ay;imu_data.IMU_Data.linear_acceleration.z = az;imu_data.IMU_Data.angular_velocity.x = wx; imu_data.IMU_Data.angular_velocity.y = wy; imu_data.IMU_Data.angular_velocity.z = wz;imu_data.IMU_Data.orientation.x = q.x();imu_data.IMU_Data.orientation.y = q.y();imu_data.IMU_Data.orientation.z = q.z();imu_data.IMU_Data.orientation.w = q.w();imu_data.Roll = angle_roll;imu_data.Pitch = angle_pitch;imu_data.Yaw = angle_yaw;Pub_IMU_Data.publish(imu_data);GPS_data.GPS_Data.header.stamp =ros::Time::now();GPS_data.GPS_Data.header.frame_id = "base_link";GPS_data.GPS_Data.latitude = latitude;GPS_data.GPS_Data.longitude = longitude;GPS_data.GPS_Data.altitude = altitude;GPS_data.DOPP = PDOP;GPS_data.DOPH = HDOP;GPS_data.DOPV = VDOP;GPS_data.satellite = satellite;GPS_data.GPS_Yaw = GPS_Yaw;GPS_data.q0 = q0;GPS_data.q1 = q1;GPS_data.q2 = q2;GPS_data.q3 = q3;Pub_GPS_Data.publish(GPS_data);}}// std::cout << std::endl;//sp.write(buffer, n);}loop_rate.sleep();}//关闭串口sp.close();return 0;
}

代码上传在github里

GitHub - casso1993/wit_ros: This is the ROS code for wit sensor

SLAM从入门到入土：维特惯性导航传感器，ROS程序相关推荐

移动SLAM从入门到入土（二）环境安装
最近在环境上折腾了好久,最终还是用上了双系统,记录一下沙雕的历程吧想知道双系统情况的,可以直接跳转到第三阶段第一阶段: 为了更好的调试软件,本不富裕的家庭花重金将电脑内存升级到了16G,并在虚拟机 ...
SLAM从入门到入土：TF转换中，odom与base_footprint没有连接上。
问题: map中status:Error报错,no transform from map to base_footprint. 有时候会延迟几分钟后显示正常.如图1所示打开rqt,显示base_fo ...
Excel VBA设计测绘工程电子计算表格从入门到入土（待完结）- 开发方法举例
Excel VBA设计测绘工程表格从入门到入土从第一个宏程序入门如何设计电子表格如何使用VBA开发应用实际如何编辑VBA代码一.了解界面二.组织结构三.如何添加代码和窗口四.编辑代码 ...
维特WT931——制作支持ROS的IMU惯性导航传感器
维特WT931--制作支持ROS的IMU惯性导航传感器本来自己想玩一个ros小车进行slam建图和导航,结果发现由于地形原因,导致建图不稳定.这是因为在某些情况下,机器人可能出现轮子在动,本体却不动 ...
重磅！ SLAM从入门到精通系统教程汇总
3D视觉工坊相继推出了<透彻刨析室内.室外激光SLAM关键算法原理.代码与实战>.<激光-视觉-IMU-GPS融合SLAM算法:理论推导.代码讲解和实战>.<彻底搞懂基于 ...
Flink 教程 gitbook 从入门到入土（详细教程）
Flink从入门到入土(详细教程) 和其他所有的计算框架一样,flink也有一些基础的开发步骤以及基础,核心的API,从开发步骤的角度来讲,主要分为四大部分 1.Environment Flink J ...
一起自学SLAM算法：第4章-机器人传感器
连载文章,长期更新,欢迎关注: 写在前面第1章-ROS入门必备知识第2章-C++编程范式第3章-OpenCV图像处理第4章-机器人传感器 4.1 惯性测量单元 4.2 激光雷达 4.3 相机 ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）
贝叶斯滤波.卡尔曼滤波.粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.histo ...
rocketmq怎么保证消息一致性_从入门到入土（三）RocketMQ 怎么保证的消息不丢失？...
精彩推荐一百期Java面试题汇总SpringBoot内容聚合IntelliJ IDEA内容聚合Mybatis内容聚合接上一篇:RocketMQ入门到入土(二)事务消息&顺序消息面试官常常 ...

SLAM从入门到入土：维特惯性导航传感器，ROS程序

SLAM从入门到入土：维特惯性导航传感器，ROS程序相关推荐

最新文章

热门文章