├── CMakeLists.txt
├── README.md
├── launch
    └── demo.launch
├── package.xml
└── src
    └── mpu6050_serial_to_imu_node.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(imu_serial_node)
 3 | 
 4 | 
 5 | find_package(catkin REQUIRED COMPONENTS
 6 |   roscpp
 7 |   geometry_msgs
 8 |   sensor_msgs
 9 |   serial
10 |   std_msgs
11 |   std_srvs
12 |   tf
13 | )
14 | 
15 | 
16 | include_directories(
17 |   ${catkin_INCLUDE_DIRS}
18 | )
19 | 
20 | add_executable(imu_serial_node src/mpu6050_serial_to_imu_node.cpp)
21 | target_link_libraries(imu_serial_node   ${catkin_LIBRARIES} )
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | imu_serial_node
 2 | =
 3 | a simple ros serial node for imu, such as mpu 6050.
 4 | 
 5 | you should change it by your serial Communication protocol.
 6 |   this package depend on serial: http://wiki.ros.org/rosserial/
 7 | 
 8 | #### Published Topics
 9 | 
10 | * **`imu`** ([sensor_msgs::Imu])
11 | 
12 | 	The resulting Imu orientation.
13 | 
14 | #### Published TF Transforms
15 | 
16 | *	The resulting orientation is published as a tf transform, the frame names can be set using the parameters.
17 | 
18 | 
19 | #### Services
20 | 
21 | * **`set_zero_orientation`** ([std_srvs/Empty])
22 | 
23 | 	This service sets the current orientation as the new zero orientation so that from now on only the difference to this orientation is sent.
24 | 
25 | 
26 | #### Parameters
27 | 
28 | * **`port`** (string, default: "/dev/ttyACM0")
29 | 
30 | 	The name of the serial port.
31 | 
32 | * **`time_offset_in_seconds`** (double, default: 0.0)
33 | 
34 | 	This sets an offset which is added to the header timestamp of the imu-topic and the TF transforms. This can be used to synchronise the IMUs orientation values to values of another sensor.
35 | 
36 | 
37 | * **`imu_frame_id`** (string, default: "imu_base")
38 | 
39 | 	Sets the name of the base frame for imu messages.
40 | 
41 | 
42 | * **`tf_parent_frame_id`** (string, default: "imu_base")
43 | 
44 | 	Sets the name of the parent frame in the tf transform.
45 | 
46 | 
47 | * **`tf_frame_id`** (string, default: "imu")
48 | 
49 | 	Sets the name of the own frame in the tf transform.
50 | 


--------------------------------------------------------------------------------
/launch/demo.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <node pkg="imu_serial_node" type="imu_serial_node" name="mpu6050_node" required="true">
3 |     <param name="port" value="/dev/ttyUSB0"/>
4 |     <param name="imu_frame_id" value="imu_base"/>
5 |   </node>
6 |   <node pkg="tf" type="static_transform_publisher" name="imu_static_tf" args="0 0 0 0 0 0 1 base_footprint imu_base 100" />
7 | </launch>
8 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>imu_serial_node</name>
 4 |   <version>0.0.1</version>
 5 |   <description>The mpu6050_serial_to_imu package</description>
 6 | 
 7 |   <maintainer email="huasheng_zyh@163.com">kint.zhao</maintainer>
 8 | 
 9 | 
10 |   <license>BSD</license>
11 | 
12 | 
13 |   <buildtool_depend>catkin</buildtool_depend>
14 |   <build_depend>roscpp</build_depend>
15 |   <build_depend>geometry_msgs</build_depend>
16 |   <build_depend>sensor_msgs</build_depend>
17 |   <build_depend>serial</build_depend>
18 |   <build_depend>std_msgs</build_depend>
19 |   <build_depend>std_srvs</build_depend>
20 |   <build_depend>tf</build_depend>
21 |   <run_depend>roscpp</run_depend>
22 |   <run_depend>geometry_msgs</run_depend>
23 |   <run_depend>sensor_msgs</run_depend>
24 |   <run_depend>serial</run_depend>
25 |   <run_depend>std_msgs</run_depend>
26 |   <run_depend>std_srvs</run_depend>
27 |   <run_depend>tf</run_depend>
28 | 
29 | 
30 |   <export>
31 | 
32 |   </export>
33 | </package>
34 | 


--------------------------------------------------------------------------------
/src/mpu6050_serial_to_imu_node.cpp:
--------------------------------------------------------------------------------
  1 | #include <geometry_msgs/Quaternion.h>
  2 | #include <ros/ros.h>
  3 | #include <serial/serial.h>
  4 | #include <sensor_msgs/Imu.h>
  5 | #include <std_msgs/String.h>
  6 | #include <std_srvs/Empty.h>
  7 | #include <string>
  8 | #include <tf/transform_broadcaster.h>
  9 | #include <tf/transform_datatypes.h>
 10 | 
 11 | 
 12 | bool zero_orientation_set = false;
 13 | 
 14 | bool set_zero_orientation(std_srvs::Empty::Request&,
 15 |                           std_srvs::Empty::Response&)
 16 | {
 17 |     ROS_INFO("Zero Orientation Set.");
 18 |     zero_orientation_set = false;
 19 |     return true;
 20 | }
 21 | 
 22 | bool DecodeIMUData(double result[], unsigned char chrTemp[], unsigned char mode )
 23 | {
 24 |     int scale;
 25 |     if( chrTemp[1] = mode)
 26 |     {
 27 |         switch(mode)
 28 |         {
 29 |         case 0x51: scale = 16; //a
 30 |             break;
 31 |         case 0x52: scale = 16; //w
 32 |             break;
 33 |         case 0x53: scale = 2000;//angle
 34 |             break;
 35 |         }
 36 |         result[0] = (short(chrTemp[3]<<8|chrTemp[2]))/32768.0*scale;
 37 |         result[1] = (short(chrTemp[5]<<8|chrTemp[4]))/32768.0*scale;
 38 |         result[2] = (short(chrTemp[7]<<8|chrTemp[6]))/32768.0*scale;
 39 | 
 40 | //      double T = (short(chrTemp[9]<<8|chrTemp[8]))/340.0+36.25;
 41 |         printf("aresult = %4.3f\t%4.3f\t%4.3f\t\r\n",result[0],result[1],result[2]);
 42 |         return true;
 43 |     }
 44 |     return false;
 45 | }
 46 | 
 47 | int main(int argc, char** argv)
 48 | {
 49 |     serial::Serial ser;
 50 |     std::string port;
 51 |     std::string tf_parent_frame_id;
 52 |     std::string tf_frame_id;
 53 |     std::string imu_frame_id;
 54 |     double time_offset_in_seconds;
 55 | 
 56 |     tf::Quaternion zero_orientation;
 57 | 
 58 |     ros::init(argc, argv, "imu_serial_node");
 59 | 
 60 |     ros::NodeHandle private_node_handle("~");
 61 |     private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0");
 62 |     private_node_handle.param<std::string>("tf_parent_frame_id", tf_parent_frame_id, "imu_base");
 63 |     private_node_handle.param<std::string>("tf_frame_id", tf_frame_id, "imu");
 64 |     private_node_handle.param<std::string>("imu_frame_id", imu_frame_id, "imu_base");
 65 |     private_node_handle.param<double>("time_offset_in_seconds", time_offset_in_seconds, 0.0);
 66 | 
 67 |     ros::NodeHandle nh;
 68 |     ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 100);
 69 |     ros::ServiceServer service = nh.advertiseService("set_zero_orientation", set_zero_orientation);
 70 | 
 71 |     ros::Rate r(100); // 1000 hz
 72 | 
 73 |     unsigned char chrBuffer[1000];
 74 |     unsigned char chrTemp[1000];
 75 |     unsigned short usLength=0,usRxLength=0;
 76 | 
 77 |     while(ros::ok())
 78 |     {
 79 |         try
 80 |         {
 81 |             if (ser.isOpen())
 82 |             {
 83 |                 // read string from serial device
 84 |                 if(ser.available())
 85 |                 {
 86 |                     usLength = ser.read(chrBuffer,33);
 87 |                     if (usLength>0)
 88 |                     {
 89 |                         usRxLength += usLength;
 90 |                         while (usRxLength >= 11)
 91 |                         {
 92 |                             memcpy(chrTemp,chrBuffer,usRxLength);
 93 |                             if (!((chrTemp[0] == 0x55) & ((chrTemp[1] == 0x51) | (chrTemp[1] == 0x52) | (chrTemp[1] == 0x53))))
 94 |                             {
 95 |                                 for (int i = 1; i < usRxLength; i++) chrBuffer[i - 1] = chrBuffer[i];
 96 |                                 usRxLength--;
 97 |                                 continue;
 98 |                             }
 99 |                             double*  Angle;
100 |                             if(!DecodeIMUData(Angle, chrTemp, 0X53))
101 |                                 continue;
102 | 
103 |                             tf::Quaternion orientation(Angle[2],Angle[1],Angle[0]);
104 | 
105 |                             if (!zero_orientation_set)
106 |                             {
107 |                                 zero_orientation = orientation;
108 |                                 zero_orientation_set = true;
109 |                             }
110 | 
111 |                             tf::Quaternion differential_rotation;
112 |                             differential_rotation = zero_orientation.inverse() * orientation;
113 | 
114 |                             // calculate measurement time
115 |                             ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds);
116 | 
117 |                             // publish imu message
118 |                             sensor_msgs::Imu imu;
119 |                             imu.header.stamp = measurement_time;
120 |                             imu.header.frame_id = imu_frame_id;
121 | 
122 |                             quaternionTFToMsg(differential_rotation, imu.orientation);
123 | 
124 |                             // i do not know the orientation covariance
125 |                             imu.orientation_covariance[0] = 1000000;
126 |                             imu.orientation_covariance[1] = 0;
127 |                             imu.orientation_covariance[2] = 0;
128 |                             imu.orientation_covariance[3] = 0;
129 |                             imu.orientation_covariance[4] = 1000000;
130 |                             imu.orientation_covariance[5] = 0;
131 |                             imu.orientation_covariance[6] = 0;
132 |                             imu.orientation_covariance[7] = 0;
133 |                             imu.orientation_covariance[8] = 0.000001;
134 | 
135 |                             // angular velocity is not provided
136 |                             imu.angular_velocity_covariance[0] = -1;
137 | 
138 |                             // linear acceleration is not provided
139 |                             imu.linear_acceleration_covariance[0] = -1;
140 | 
141 |                             imu_pub.publish(imu);
142 | 
143 |                             // publish tf transform
144 |                             static tf::TransformBroadcaster br;
145 |                             tf::Transform transform;
146 |                             transform.setRotation(differential_rotation);
147 |                             br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id));
148 | 
149 |                             for (int i = 11; i < usRxLength; i++)
150 |                                 chrBuffer[i - 11] = chrBuffer[i];
151 |                             usRxLength -= 11;
152 |                         }
153 |                     }
154 |                 }
155 |                 else  // ser not available
156 |                 {
157 | 
158 |                 }
159 |             }
160 |             else
161 |             {
162 |                 // try and open the serial port
163 |                 try
164 |                 {
165 |                     ser.setPort(port);
166 |                     ser.setBaudrate(115200);
167 |                     serial::Timeout to = serial::Timeout::simpleTimeout(1000);
168 |                     ser.setTimeout(to);
169 |                     ser.open();
170 |                 }
171 |                 catch (serial::IOException& e)
172 |                 {
173 |                     ROS_ERROR_STREAM("Unable to open serial port " << ser.getPort() << ". Trying again in 5 seconds.");
174 |                     ros::Duration(5).sleep();
175 |                 }
176 | 
177 |                 if(ser.isOpen())
178 |                 {
179 |                     ROS_DEBUG_STREAM("Serial port " << ser.getPort() << " initialized.");
180 |                 }
181 |                 else
182 |                 {
183 |                     //ROS_INFO_STREAM("Could not initialize serial port.");
184 |                 }
185 | 
186 |             }
187 |         }
188 |         catch (serial::IOException& e)
189 |         {
190 |             ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection.");
191 |             ser.close();
192 |         }
193 |         ros::spinOnce();
194 |         r.sleep();
195 |     }
196 | 
197 | }
198 | 
199 | 


--------------------------------------------------------------------------------