ROS IMU topic 发布


#include <string>
#include <ros/ros.h>                           // 包含ROS的头文件
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>                  //包含boost库函数
#include <boost/bind.hpp>
#include <math.h>
#include "std_msgs/String.h"              //ros定义的String数据类型
#include <std_msgs/Float32.h>
#include <sensor_msgs/Imu.h>

#include "packet.h"
#include "imu_data_decode.h"
 
using namespace std;
using namespace boost::asio;           //定义一个命名空间,用于后面的读写操作
 
unsigned char buf[17];                      //定义字符串长度,IMU返回的数据是17个字节一组,可用串口调试助手获得
 
std::string string_to_hex(const std::string& input)
{
   static const char* const lut = "0123456789ABCDEF";
   size_t len = input.length();
   std::string output;
   output.reserve(2 * len);
   for (size_t i = 0; i < len; ++i)
   {
    const unsigned char c = input[i];
    output.push_back(lut[c >> 4]);
    output.push_back(lut[c & 15]);
   }
   return output;
}

 
int main(int argc, char** argv)
{
 
    ros::init(argc, argv, "imu");       //初始化节点
    ros::NodeHandle n;
 
    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);      //定义发布消息的名称及sulv
 
    ros::Rate loop_rate(100);
 
    io_service iosev;
    serial_port sp(iosev, "/dev/ttyUSB0");         //定义传输的串口
    sp.set_option(serial_port::baud_rate(115200));
    sp.set_option(serial_port::flow_control());
    sp.set_option(serial_port::parity());
    sp.set_option(serial_port::stop_bits());
    sp.set_option(serial_port::character_size(8));

     //int fd=open_port("/dev/ttyUSB0");
     imu_data_decode_init();
    
	uint8_t ID = 0;
	int16_t Acc[3] = {0};
	int16_t Gyo[3] = {0};
	int16_t Mag[3] = {0};
	float Eular[3] = {0};
	float Quat[4]  = {0};
	int32_t Pressure = 0;

	int i;
	uint8_t buf[1024];
    while (ros::ok())
    {
        ssize_t n = read(sp,buffer(buf));
		for(i=0; i<n; i++)
        {
              Packet_Decode(buf[i]);
        }
			
			get_raw_acc(Acc);
			get_raw_gyo(Gyo);
			get_raw_mag(Mag);
			get_eular(Eular);
            get_quat(Quat);
			get_id(&ID);
			printf("Acc:%d %d %d\r\n",Acc[0], Acc[1], Acc[2]);
			printf("Gyo:%d %d %d\r\n",Gyo[0], Gyo[1], Gyo[2]);
			printf("Mag:%d %d %d\r\n",Mag[0], Mag[1], Mag[2]);
			printf("Eular(P R Y):%0.2f %0.2f %0.2f\r\n",Eular[0], Eular[1], Eular[2]);
			printf("quat(W X Y Z):%0.3f %0.3f %0.3f %0.3f\r\n",Quat[0], Quat[1], Quat[2], Quat[3]);

            // ROS_INFO(("quat(W X Y Z):%0.3f %0.3f %0.3f %0.3f\r\n",Quat[0], Quat[1], Quat[2], Quat[3]);
            sensor_msgs::Imu imu_data;
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            imu_data.orientation.x = Quat[1];
            imu_data.orientation.y = Quat[2];
            imu_data.orientation.z = Quat[3];
            imu_data.orientation.w = Quat[0];
            


            imu_data.orientation_covariance[0] = 1000000;
            imu_data.orientation_covariance[1] = 0;
            imu_data.orientation_covariance[2] = 0;
            imu_data.orientation_covariance[3] = 0;
            imu_data.orientation_covariance[4] = 1000000;
            imu_data.orientation_covariance[5] = 0;
            imu_data.orientation_covariance[6] = 0;
            imu_data.orientation_covariance[7] = 0;
            imu_data.orientation_covariance[8] = 0.000001;


           imu_data.angular_velocity.x = 0.0;
           imu_data.angular_velocity.y = 0.0;
           imu_data.angular_velocity.z = (double)-1*(Gyo[2]*3.14/(180*100));

           imu_data.linear_acceleration.x = (double)(-1*((Acc[0]+10) * 9.80665/1000.f));
           imu_data.linear_acceleration.y = (double)(-1*((Acc[1]+24) * 9.80665/1000.f));
           imu_data.linear_acceleration.z = (double)(-1*((Acc[2]-1070) * 9.80665/1000.f));

           IMU_pub.publish(imu_data);
			
        	// usleep(20*1000);
     
    //    read (sp,buffer(buf));
    //    string str(&buf[0],&buf[17]);            //将数组转化为字符串
 
    //    std_msgs::String msg;
    //    std::stringstream ss;
    //    ss << str;
    //    std_msgs::Float32  Yaw;        //定义机器人偏航角,单位为度数
    //    char higher;
    //    char lower;
    //    higher = buf[5];
    //    lower = buf[4];
    //    Yaw.data= (float)((higher * 256 + lower))/100;
    //    cout << Yaw << endl;   //输出偏航角到终端
    //    msg.data = string_to_hex(ss.str());
    //    ROS_INFO("%s", msg.data.c_str());        //打印接受到的字符串
    //    IMU_pub.publish(Yaw);   //发布消息
       ros::spinOnce();
       loop_rate.sleep();
    }
    iosev.run();
    return 0;
}

https://www.cnblogs.com/kuangxionghui/p/8328131.html#undefined

IMU HI216用户手册驱动

展开阅读全文

没有更多推荐了,返回首页