00001 // 00002 // Created by tom on 15/05/16. 00003 // 00004 00005 #include <robotican_hardware_interface/Imu.h> 00006 00007 namespace robotican_hardware { 00008 00009 void Imu::update(const DeviceMessage *deviceMessage) { 00010 if(isReady()) { 00011 if(deviceMessage->deviceMessageType == DeviceMessageType::ImuFeedback) { 00012 ImuFeedback *feedback = (ImuFeedback *) deviceMessage; 00013 00014 sensor_msgs::Imu imuMsg; 00015 imuMsg.header.frame_id = _frameId; 00016 imuMsg.header.stamp = ros::Time::now(); 00017 imuMsg.orientation.x = feedback->orientationX; 00018 imuMsg.orientation.y = feedback->orientationY; 00019 imuMsg.orientation.z = feedback->orientationZ; 00020 imuMsg.orientation.w = feedback->orientationW; 00021 imuMsg.linear_acceleration.x = (feedback->accelerationX * _imuLinearAccFix[0][0] 00022 + feedback->accelerationY * _imuLinearAccFix[0][1] 00023 + feedback->accelerationZ * _imuLinearAccFix[0][2]) * G2ms; 00024 imuMsg.linear_acceleration.y = (feedback->accelerationX * _imuLinearAccFix[1][0] 00025 + feedback->accelerationY * _imuLinearAccFix[1][1] 00026 + feedback->accelerationZ * _imuLinearAccFix[1][2]) * G2ms; 00027 imuMsg.linear_acceleration.z = (feedback->accelerationX * _imuLinearAccFix[2][0] 00028 + feedback->accelerationY * _imuLinearAccFix[2][1] 00029 + feedback->accelerationZ * _imuLinearAccFix[2][2]) * G2ms; 00030 imuMsg.angular_velocity.x = (feedback->velocityX * _imuAngularVelocityFix[0][0] 00031 + feedback->velocityY * _imuAngularVelocityFix[0][1] 00032 + feedback->velocityZ * _imuAngularVelocityFix[0][2]) * DEGs2RADs; 00033 imuMsg.angular_velocity.y = (feedback->velocityX * _imuAngularVelocityFix[1][0] 00034 + feedback->velocityY * _imuAngularVelocityFix[1][1] 00035 + feedback->velocityZ * _imuAngularVelocityFix[1][2]) * DEGs2RADs; 00036 imuMsg.angular_velocity.z = (feedback->velocityX * _imuAngularVelocityFix[2][0] 00037 + feedback->velocityY * _imuAngularVelocityFix[2][1] 00038 + feedback->velocityZ * _imuAngularVelocityFix[2][2]) * DEGs2RADs; 00039 00040 sensor_msgs::MagneticField magneticField; 00041 magneticField.header.frame_id = _frameId; 00042 magneticField.header.stamp = ros::Time::now(); 00043 magneticField.magnetic_field.x = (feedback->magnetometerX * _imuMagnetometerFix[0][0] 00044 + feedback->magnetometerY * _imuMagnetometerFix[0][1] 00045 + feedback->magnetometerZ * _imuMagnetometerFix[0][2]) * MILLI_GAUSS_2_TESLAS; 00046 magneticField.magnetic_field.y = (feedback->magnetometerX * _imuMagnetometerFix[1][0] 00047 + feedback->magnetometerY * _imuMagnetometerFix[1][1] 00048 + feedback->magnetometerZ * _imuMagnetometerFix[1][2]) * MILLI_GAUSS_2_TESLAS; 00049 magneticField.magnetic_field.z = (feedback->magnetometerX * _imuMagnetometerFix[2][0] 00050 + feedback->magnetometerY * _imuMagnetometerFix[2][1] 00051 + feedback->magnetometerZ * _imuMagnetometerFix[2][2]) * MILLI_GAUSS_2_TESLAS; 00052 00053 tf::Quaternion quaternion; 00054 tf::quaternionMsgToTF(imuMsg.orientation, quaternion); 00055 00056 double roll, pitch, yaw; 00057 double newRoll, newPitch, newYaw; 00058 tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw); 00059 00060 00061 newRoll = roll * _imuRotationFix[0][0] + pitch * _imuRotationFix[0][1] + yaw * _imuRotationFix[0][2]; 00062 newRoll += _imuRotationOffset[0]; 00063 00064 newPitch = roll * _imuRotationFix[1][0] + pitch * _imuRotationFix[1][1] + yaw * _imuRotationFix[1][2]; 00065 newPitch += _imuRotationOffset[1]; 00066 00067 newYaw = roll * _imuRotationFix[2][0] + pitch * _imuRotationFix[2][1] + yaw * _imuRotationFix[2][2]; 00068 newYaw += _imuRotationOffset[2]; 00069 00070 quaternion.setRPY(newRoll, newPitch, newYaw); 00071 00072 tf::quaternionTFToMsg(quaternion, imuMsg.orientation); 00073 00074 tf::Transform transform; 00075 transform.setRotation(quaternion); 00076 00077 _imuAMQ.publish(imuMsg); 00078 _imuM.publish(magneticField); 00079 _broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "imu_link")); 00080 00081 #ifdef DEBUG_VER 00082 tf::Matrix3x3(quaternion).getRPY(newRoll, newPitch, newYaw); 00083 ROS_INFO("[%s]: roll: %.2f , pitch: %.2f, yaw: %.2f", ros::this_node::getName().c_str(), newRoll * 180 / M_PI, newPitch * 180 / M_PI, newYaw * 180 / M_PI); 00084 #endif 00085 00086 } 00087 else if(deviceMessage->deviceMessageType == DeviceMessageType::ImuClibFeedback) { 00088 if(!_isStopClib) { 00089 ImuClibFeedback *clibFeedback = (ImuClibFeedback *) deviceMessage; 00090 robotican_hardware_interface::imuClib clibMsg; 00091 clibMsg.max.header.frame_id = "base_link"; 00092 clibMsg.min.header.frame_id = "base_link"; 00093 clibMsg.max.header.stamp = ros::Time::now(); 00094 clibMsg.min.header.stamp = ros::Time::now(); 00095 00096 clibMsg.max.vector.x = clibFeedback->max[0]; 00097 clibMsg.max.vector.y = clibFeedback->max[1]; 00098 clibMsg.max.vector.z = clibFeedback->max[2]; 00099 00100 clibMsg.min.vector.x = clibFeedback->min[0]; 00101 clibMsg.min.vector.y = clibFeedback->min[1]; 00102 clibMsg.min.vector.z = clibFeedback->min[2]; 00103 00104 _clibPub.publish(clibMsg); 00105 } 00106 00107 } 00108 } 00109 00110 } 00111 00112 void Imu::write() { 00113 if(isReady()) { 00114 if(_isStateChange) { 00115 _isStateChange = false; 00116 ImuSetClibState state; 00117 state.length = sizeof(state); 00118 state.checkSum = 0; 00119 state.id = getId(); 00120 00121 state.state = _imuState; 00122 uint8_t *rawData = (uint8_t*) &state; 00123 state.checkSum = _transportLayer->calcChecksum(rawData, state.length); 00124 _transportLayer->write(rawData, state.length); 00125 } 00126 } 00127 00128 } 00129 00130 void Imu::buildDevice() { 00131 BuildImu buildImu; 00132 buildImu.length = sizeof(buildImu); 00133 buildImu.checkSum = 0; 00134 buildImu.id = getId(); 00135 buildImu.fusionHz = _fusionHz; 00136 buildImu.enableGyro =_enableGyro; 00137 buildImu.fuseCompass =_fuseCompass; 00138 00139 uint8_t *rawData = (uint8_t*) &buildImu; 00140 00141 buildImu.checkSum = _transportLayer->calcChecksum(rawData, buildImu.length); 00142 _transportLayer->write(rawData, buildImu.length); 00143 ros::Duration(1.0).sleep(); 00144 00145 } 00146 00147 00148 void Imu::deviceAck(const DeviceAck* ack){ 00149 Device::deviceAck(ack); 00150 if(isReady()) { 00151 ros_utils::rosInfo("Imu is ready"); 00152 _imuAMQ = _nodeHandle.advertise<sensor_msgs::Imu>("imu", 10); 00153 _imuM = _nodeHandle.advertise<sensor_msgs::MagneticField>("imu_M", 10); 00154 } 00155 else { 00156 ros_utils::rosError("RiCBoard can't build Imu object for spme reason, this program will shut down now"); 00157 ros::shutdown(); 00158 } 00159 } 00160 00161 Imu::Imu(byte id, TransportLayer *transportLayer, uint16_t fusionHz, std::string frameId, bool enableGyro, 00162 bool fuseCompass, std::vector<double> imuLinearAccFix, std::vector<double> imuAngularVelocityFix, 00163 std::vector<double> imuMagnetometerFix, std::vector<double> imuRotationFix, 00164 std::vector<double> imuRotationOffset) 00165 : Device(id, transportLayer) { 00166 _fusionHz = fusionHz; 00167 _frameId = frameId; 00168 _isStopClib = true; 00169 _isStateChange = false; 00170 _enableGyro = enableGyro; 00171 _fuseCompass = fuseCompass; 00172 _imuState = robotican_hardware_interface::setImuClibRequest::STOP; 00173 _setImuClibService = _nodeHandle.advertiseService("set_imu_calibration_state", &Imu::onSetImuClib, this); 00174 initArrays(); 00175 00176 _imuLinearAccFix[0][0] = imuLinearAccFix[0]; 00177 _imuLinearAccFix[0][1] = imuLinearAccFix[1]; 00178 _imuLinearAccFix[0][2] = imuLinearAccFix[2]; 00179 _imuLinearAccFix[1][0] = imuLinearAccFix[3]; 00180 _imuLinearAccFix[1][1] = imuLinearAccFix[4]; 00181 _imuLinearAccFix[1][2] = imuLinearAccFix[5]; 00182 _imuLinearAccFix[2][0] = imuLinearAccFix[6]; 00183 _imuLinearAccFix[2][1] = imuLinearAccFix[7]; 00184 _imuLinearAccFix[2][2] = imuLinearAccFix[8]; 00185 00186 _imuAngularVelocityFix[0][0] = imuAngularVelocityFix[0]; 00187 _imuAngularVelocityFix[0][1] = imuAngularVelocityFix[1]; 00188 _imuAngularVelocityFix[0][2] = imuAngularVelocityFix[2]; 00189 _imuAngularVelocityFix[1][0] = imuAngularVelocityFix[3]; 00190 _imuAngularVelocityFix[1][1] = imuAngularVelocityFix[4]; 00191 _imuAngularVelocityFix[1][2] = imuAngularVelocityFix[5]; 00192 _imuAngularVelocityFix[2][0] = imuAngularVelocityFix[6]; 00193 _imuAngularVelocityFix[2][1] = imuAngularVelocityFix[7]; 00194 _imuAngularVelocityFix[2][2] = imuAngularVelocityFix[8]; 00195 00196 _imuMagnetometerFix[0][0] = imuMagnetometerFix[0]; 00197 _imuMagnetometerFix[0][1] = imuMagnetometerFix[1]; 00198 _imuMagnetometerFix[0][2] = imuMagnetometerFix[2]; 00199 _imuMagnetometerFix[1][0] = imuMagnetometerFix[3]; 00200 _imuMagnetometerFix[1][1] = imuMagnetometerFix[4]; 00201 _imuMagnetometerFix[1][2] = imuMagnetometerFix[5]; 00202 _imuMagnetometerFix[2][0] = imuMagnetometerFix[6]; 00203 _imuMagnetometerFix[2][1] = imuMagnetometerFix[7]; 00204 _imuMagnetometerFix[2][2] = imuMagnetometerFix[8]; 00205 00206 _imuRotationFix[0][0] = imuRotationFix[0]; 00207 _imuRotationFix[0][1] = imuRotationFix[1]; 00208 _imuRotationFix[0][2] = imuRotationFix[2]; 00209 _imuRotationFix[1][0] = imuRotationFix[3]; 00210 _imuRotationFix[1][1] = imuRotationFix[4]; 00211 _imuRotationFix[1][2] = imuRotationFix[5]; 00212 _imuRotationFix[2][0] = imuRotationFix[6]; 00213 _imuRotationFix[2][1] = imuRotationFix[7]; 00214 _imuRotationFix[2][2] = imuRotationFix[8]; 00215 00216 _imuRotationOffset[0] = imuRotationOffset[0]; 00217 _imuRotationOffset[1] = imuRotationOffset[1]; 00218 _imuRotationOffset[2] = imuRotationOffset[2]; 00219 00220 00221 00222 } 00223 00224 00225 bool Imu::onSetImuClib(robotican_hardware_interface::setImuClib::Request &request, robotican_hardware_interface::setImuClib::Response &response) { 00226 if(request.state == robotican_hardware_interface::setImuClibRequest::CALIBRATION) { 00227 _isStateChange = _imuState != request.state; 00228 _isStopClib = false; 00229 _imuState = robotican_hardware_interface::setImuClibRequest::CALIBRATION; 00230 _clibPub = _nodeHandle.advertise<robotican_hardware_interface::imuClib>("imu_calibration", 10); 00231 } 00232 else if(request.state == robotican_hardware_interface::setImuClibRequest::STOP) { 00233 _isStateChange = _imuState != request.state; 00234 _isStopClib = true; 00235 _imuState = robotican_hardware_interface::setImuClibRequest::STOP; 00236 _clibPub.shutdown(); 00237 } 00238 else if(request.state == robotican_hardware_interface::setImuClibRequest::SAVE) { 00239 _isStateChange = _imuState != request.state; 00240 _imuState = robotican_hardware_interface::setImuClibRequest::SAVE; 00241 } 00242 response.ack = true; 00243 return true; 00244 } 00245 00246 void Imu::initArrays() { 00247 for(int i = 0; i < 3; ++i) 00248 for(int j = 0; j < 3; ++j) { 00249 _imuLinearAccFix[i][j] = 0.0; 00250 _imuAngularVelocityFix[i][j] = 0.0; 00251 _imuMagnetometerFix[i][j] = 0.0; 00252 _imuRotationFix[i][j] = 0.0; 00253 } 00254 _imuRotationOffset[0] = _imuRotationOffset[1] = _imuRotationOffset[2] = 0.0; 00255 } 00256 }