新增c++代码读取IIC数据

rasp_imu_hat_6dof/CMakeLists.txt

@@ -3,14 +3,35 @@ project(rasp_imu_hat_6dof)
 
 find_package(catkin REQUIRED COMPONENTS 
     rospy
-    dynamic_reconfigure)
+    roscpp
+    sensor_msgs
+    dynamic_reconfigure
+    cmake_modules
+    )
+
+find_package(Eigen REQUIRED)
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen_INCLUDE_DIRS}
+    )
+
+add_definitions(${EIGEN_DEFINITIONS})
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
     cfg/imu.cfg
 )
-
-catkin_package()
+catkin_package(
+    INCLUDE_DIRS include
+    CATKIN_DEPENDS roscpp
+    )
+
+add_executable(imu_pub_node src/imu_data.cpp src/imu_node.cpp)
+target_link_libraries(imu_pub_node
+    ${catkin_LIBRARIES}
+    )
 
 include_directories(
     ${catkin_INCLUDE_DIRS}

rasp_imu_hat_6dof/include/imu_data.h

@@ -0,0 +1,51 @@
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+#define I2C_DEV_ADDR  0x50
+
+#define ACC_X_REG  0x34
+#define ACC_Y_REG  0x35
+#define ACC_Z_REG  0x36
+
+#define GYO_X_REG  0x37
+#define GYO_Y_REG  0x38
+#define GYO_Z_REG  0x39
+
+#define ROLL_REG   0x3d
+#define PITCH_REG  0x3e
+#define YAW_REG    0x3f
+
+class myIMU
+{
+    public:
+        int initIICDevice();
+
+        float getRollData();
+        float getPitchData();
+        float getYawData();
+
+        float getAccXData();
+        float getAccYData();
+        float getAccZData();
+
+        float getGyoXData();
+        float getGyoYData();
+        float getGyoZData();
+
+    private:
+        int iicFD;
+        char accX[2];
+        char accY[2];
+        char accZ[2];
+
+        char gyoX[2];
+        char gyoY[2];
+        char gyoZ[2];
+
+        char roll[2];
+        char pitch[2];
+        char yaw[2];
+};
+
+#endif
+

rasp_imu_hat_6dof/package.xml

@@ -16,8 +16,10 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roscpp</build_depend>
 
   <run_depend>rospy</run_depend>
+  <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>

rasp_imu_hat_6dof/src/imu_data.cpp

@@ -0,0 +1,153 @@
+#include "imu_data.h"
+#include <ros/ros.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <linux/i2c-dev.h>
+#include <sys/ioctl.h>
+
+int myIMU::initIICDevice()
+{
+    int fd;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    iicFD = fd;
+    ROS_INFO("init iicFD = %d", iicFD);
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+
+    return 0;
+}
+
+float myIMU::getAccXData()
+{
+    float data = 0;
+    ROS_INFO("iicFD = %d", iicFD);
+
+    if(read(iicFD, &accX, 2))
+    {
+        data = (((short)accX[1]<<8)|accX[0])/32768.0*16.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read accX data error !");
+    }
+    return data;
+}
+
+float myIMU::getAccYData()
+{
+    float data = 0;
+    if(read(iicFD, &accY, 2))
+    {
+        data = (((short)accY[1]<<8)|accY[0])/32768.0*16.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read accY data error !");
+    }
+    return data;
+}
+
+float myIMU::getAccZData()
+{
+    float data = 0;
+    if(read(iicFD, &accZ, 2))
+    {
+        data = (((short)accZ[1]<<8)|accZ[0])/32768.0*16.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read accZ data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getGyoXData()
+{
+    float data = 0;
+    if(read(iicFD, &gyoX, 2))
+    {
+        data = (((short)gyoX[1]<<8)|gyoX[0])/32768.0*2000.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read gyoX data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getGyoYData()
+{
+    float data = 0;
+    if(read(iicFD, &gyoY, 2))
+    {
+        data = (((short)gyoY[1]<<8)|gyoY[0])/32768.0*2000.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read gyoY data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getGyoZData()
+{
+    float data = 0;
+    if(read(iicFD, &gyoZ, 2))
+    {
+        data = (((short)gyoZ[1]<<8)|gyoZ[0])/32768.0*2000.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read gyoZ register data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getRollData()
+{
+    float data = 0;
+    if(read(iicFD, &roll, 2))
+    {
+        data = (((short)roll[1]<<8)|roll[0])/32768.0*180.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read roll register data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getPitchData()
+{
+    float data = 0;
+    if(read(iicFD, &pitch, 2))
+    {
+        data = (((short)pitch[1]<<8)|pitch[0])/32768.0*180.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read pitch register data error !");
+    }
+
+    return data;
+}
+
+float myIMU::getYawData()
+{
+    float data = 0;
+    if(read(iicFD, &yaw, 2))
+    {
+        data = (((short)yaw[1]<<8)|yaw[0])/32768.0*180.0;
+    }
+    {
+        ROS_ERROR_STREAM("Read yaw register data error !");
+    }
+
+    return data;
+}
+

rasp_imu_hat_6dof/src/imu_node.cpp

@@ -0,0 +1,81 @@
+#include <ros/ros.h>
+#include "imu_data.h"
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+#include <sensor_msgs/Imu.h>
+
+using namespace Eigen;
+using namespace std;
+
+Eigen::Quaterniond euler2Quater(float roll, float yaw, float pitch)
+{
+    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;
+    cout << "Euler2Quaternion result is:" <<endl;
+    cout << "x = " << q.x() <<endl;
+    cout << "y = " << q.y() <<endl;
+    cout << "z = " << q.z() <<endl;
+    cout << "w = " << q.w() <<endl<<endl;
+
+    return q;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "imu_data_pub_node");
+    ros::NodeHandle handle;
+    myIMU my_imu;
+    my_imu.initIICDevice();
+
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>("imu_data", 2);
+    ros::Rate loop_rate(10);
+    sensor_msgs::Imu imu_msg;
+    float yaw_tmp;
+    float pitch_tmp;
+    float roll_tmp;
+
+    while(ros::ok())
+    {
+        imu_msg.header.stamp = ros::Time::now();
+        imu_msg.header.frame_id = "base_link";
+
+        roll_tmp  = my_imu.getRollData();
+        pitch_tmp = my_imu.getPitchData();
+        yaw_tmp   = my_imu.getYawData();
+
+        Eigen::Quaterniond q = euler2Quater(roll_tmp, pitch_tmp, yaw_tmp);
+        imu_msg.orientation.x = q.x();
+        imu_msg.orientation.y = q.y();
+        imu_msg.orientation.z = q.z();
+        imu_msg.orientation.w = q.w();
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+
+        imu_msg.angular_velocity.x = my_imu.getGyoXData();
+        imu_msg.angular_velocity.y = my_imu.getGyoYData();
+        imu_msg.angular_velocity.z = my_imu.getGyoZData();
+        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
+                                               0, 0.02, 0,
+                                               0, 0, 0.02};
+
+        imu_msg.linear_acceleration.x = my_imu.getAccXData();
+        imu_msg.linear_acceleration.y = my_imu.getAccYData();
+        imu_msg.linear_acceleration.z = my_imu.getAccZData();
+        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
+                                                   0, 0.04, 0,
+                                                   0, 0, 0.04};
+
+        imu_pub.publish(imu_msg);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
+