myahrs_driver.cpp
Go to the documentation of this file.
00001 //------------------------------------------------------------------------------
00002 // Copyright (c) 2015, Yoonseok Pyo
00003 // All rights reserved.
00004 
00005 // License: BSD
00006 
00007 // Redistribution and use in source and binary forms, with or without
00008 // modification, are permitted provided that the following conditions are met:
00009 
00010 // * Redistributions of source code must retain the above copyright notice, this
00011 //   list of conditions and the following disclaimer.
00012 
00013 // * Redistributions in binary form must reproduce the above copyright notice,
00014 //   this list of conditions and the following disclaimer in the documentation
00015 //   and/or other materials provided with the distribution.
00016 
00017 // * Neither the name of myahrs_driver nor the names of its
00018 //   contributors may be used to endorse or promote products derived from
00019 //   this software without specific prior written permission.
00020 
00021 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00024 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00025 // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00026 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00027 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00029 // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00030 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031 //------------------------------------------------------------------------------
00032 #include <myahrs_driver/myahrs_plus.hpp>
00033 
00034 #include <ros/ros.h>
00035 #include <std_msgs/Float64.h>
00036 #include <sensor_msgs/Imu.h>
00037 #include <sensor_msgs/MagneticField.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/transform_datatypes.h>
00040 
00041 //------------------------------------------------------------------------------
00042 using namespace WithRobot;
00043 
00044 //------------------------------------------------------------------------------
00045 class MyAhrsDriverForROS : public iMyAhrsPlus
00046 {
00047 private:
00048   ros::NodeHandle nh_;
00049   ros::NodeHandle nh_priv_;
00050 
00051   ros::Publisher imu_data_raw_pub_;
00052   ros::Publisher imu_data_pub_;
00053   ros::Publisher imu_mag_pub_;
00054   ros::Publisher imu_temperature_pub_;
00055 
00056   tf::TransformBroadcaster broadcaster_;
00057 
00058   Platform::Mutex lock_;
00059   SensorData sensor_data_;
00060   bool publish_tf;
00061   std::string parent_frame_id_;
00062   std::string frame_id_;
00063   double linear_acceleration_stddev_;
00064   double angular_velocity_stddev_;
00065   double magnetic_field_stddev_;
00066   double orientation_stddev_;
00067 
00068   void OnSensorData(int sensor_id, SensorData data)
00069   {
00070     LockGuard _l(lock_);
00071     sensor_data_ = data;
00072     publish_topic(sensor_id);
00073   }
00074 
00075   void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
00076   {
00077     printf("OnAttributeChange(id %d, %s, %s)\n", sensor_id, attribute_name.c_str(), value.c_str());
00078   }
00079 
00080 public:
00081   MyAhrsDriverForROS(std::string port="", int baud_rate=115200)
00082   : iMyAhrsPlus(port, baud_rate),
00083     nh_priv_("~")
00084   {
00085     // dependent on user device
00086     nh_priv_.setParam("port", port);
00087     nh_priv_.setParam("baud_rate", baud_rate);
00088     nh_priv_.param("publish_tf", publish_tf, false);
00089     // default frame id
00090     nh_priv_.param("frame_id", frame_id_, std::string("imu_link"));
00091     // for testing the tf
00092     nh_priv_.param("parent_frame_id_", parent_frame_id_, std::string("base_link"));
00093     // defaults obtained experimentally from device
00094     nh_priv_.param("linear_acceleration_stddev", linear_acceleration_stddev_, 0.026831);
00095     nh_priv_.param("angular_velocity_stddev", angular_velocity_stddev_, 0.002428);
00096     nh_priv_.param("magnetic_field_stddev", magnetic_field_stddev_, 0.00000327486);
00097     nh_priv_.param("orientation_stddev", orientation_stddev_, 0.002143);
00098     // publisher for streaming
00099     imu_data_raw_pub_   = nh_.advertise<sensor_msgs::Imu>("imu/data_raw", 1);
00100     imu_data_pub_       = nh_.advertise<sensor_msgs::Imu>("imu/data", 1);
00101     imu_mag_pub_        = nh_.advertise<sensor_msgs::MagneticField>("imu/mag", 1);
00102     imu_temperature_pub_= nh_.advertise<std_msgs::Float64>("imu/temperature", 1);
00103   }
00104 
00105   ~MyAhrsDriverForROS()
00106   {}
00107 
00108   bool initialize()
00109   {
00110     bool ok = false;
00111 
00112     do
00113     {
00114       if(start() == false) break;
00115       //Euler angle(x, y, z axis)
00116       //IMU(linear_acceleration, angular_velocity, magnetic_field)
00117       if(cmd_binary_data_format("EULER, IMU") == false) break;
00118       // 100Hz
00119       if(cmd_divider("1") == false) break;
00120       // Binary and Continue mode
00121       if(cmd_mode("BC") == false) break;
00122       ok = true;
00123     } while(0);
00124 
00125     return ok;
00126   }
00127 
00128   inline void get_data(SensorData& data)
00129   {
00130     LockGuard _l(lock_);
00131     data = sensor_data_;
00132   }
00133 
00134   inline SensorData get_data()
00135   {
00136     LockGuard _l(lock_);
00137     return sensor_data_;
00138   }
00139 
00140   void publish_topic(int sensor_id)
00141   {
00142     sensor_msgs::Imu imu_data_raw_msg;
00143     sensor_msgs::Imu imu_data_msg;
00144     sensor_msgs::MagneticField imu_magnetic_msg;
00145     std_msgs::Float64 imu_temperature_msg;
00146 
00147     double linear_acceleration_cov = linear_acceleration_stddev_ * linear_acceleration_stddev_;
00148     double angular_velocity_cov    = angular_velocity_stddev_ * angular_velocity_stddev_;
00149     double magnetic_field_cov      = magnetic_field_stddev_ * magnetic_field_stddev_;
00150     double orientation_cov         = orientation_stddev_ * orientation_stddev_;
00151 
00152     imu_data_raw_msg.linear_acceleration_covariance[0] =
00153     imu_data_raw_msg.linear_acceleration_covariance[4] =
00154     imu_data_raw_msg.linear_acceleration_covariance[8] =
00155     imu_data_msg.linear_acceleration_covariance[0] =
00156     imu_data_msg.linear_acceleration_covariance[4] =
00157     imu_data_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
00158 
00159     imu_data_raw_msg.angular_velocity_covariance[0] =
00160     imu_data_raw_msg.angular_velocity_covariance[4] =
00161     imu_data_raw_msg.angular_velocity_covariance[8] =
00162     imu_data_msg.angular_velocity_covariance[0] =
00163     imu_data_msg.angular_velocity_covariance[4] =
00164     imu_data_msg.angular_velocity_covariance[8] = angular_velocity_cov;
00165 
00166     imu_data_msg.orientation_covariance[0] =
00167     imu_data_msg.orientation_covariance[4] =
00168     imu_data_msg.orientation_covariance[8] = orientation_cov;
00169 
00170     imu_magnetic_msg.magnetic_field_covariance[0] =
00171     imu_magnetic_msg.magnetic_field_covariance[4] =
00172     imu_magnetic_msg.magnetic_field_covariance[8] = magnetic_field_cov;
00173 
00174     static double convertor_g2a  = 9.80665;    // for linear_acceleration (g to m/s^2)
00175     static double convertor_d2r  = M_PI/180.0; // for angular_velocity (degree to radian)
00176     static double convertor_r2d  = 180.0/M_PI; // for easy understanding (radian to degree)
00177     static double convertor_ut2t = 1000000;    // for magnetic_field (uT to Tesla)
00178     static double convertor_c    = 1.0;        // for temperature (celsius)
00179 
00180     double roll, pitch, yaw;
00181 
00182     // original sensor data used the degree unit, convert to radian (see ROS REP103)
00183     // we used the ROS's axes orientation like x forward, y left and z up
00184     // so changed the y and z aixs of myAHRS+ board
00185     roll  =  sensor_data_.euler_angle.roll*convertor_d2r;
00186     pitch = -sensor_data_.euler_angle.pitch*convertor_d2r;
00187     yaw   = -sensor_data_.euler_angle.yaw*convertor_d2r;
00188 
00189     ImuData<float>& imu = sensor_data_.imu;
00190 
00191     tf::Quaternion orientation = tf::createQuaternionFromRPY(roll, pitch, yaw);
00192 
00193     ros::Time now = ros::Time::now();
00194 
00195     imu_data_raw_msg.header.stamp =
00196     imu_data_msg.header.stamp     =
00197     imu_magnetic_msg.header.stamp = now;
00198 
00199     imu_data_raw_msg.header.frame_id =
00200     imu_data_msg.header.frame_id     =
00201     imu_magnetic_msg.header.frame_id = frame_id_;
00202 
00203     // orientation
00204     imu_data_msg.orientation.x = orientation[0];
00205     imu_data_msg.orientation.y = orientation[1];
00206     imu_data_msg.orientation.z = orientation[2];
00207     imu_data_msg.orientation.w = orientation[3];
00208 
00209     // original data used the g unit, convert to m/s^2
00210     imu_data_raw_msg.linear_acceleration.x =
00211     imu_data_msg.linear_acceleration.x     =  imu.ax * convertor_g2a;
00212     imu_data_raw_msg.linear_acceleration.y =
00213     imu_data_msg.linear_acceleration.y     = -imu.ay * convertor_g2a;
00214     imu_data_raw_msg.linear_acceleration.z =
00215     imu_data_msg.linear_acceleration.z     = -imu.az * convertor_g2a;
00216 
00217     // original data used the degree/s unit, convert to radian/s
00218     imu_data_raw_msg.angular_velocity.x =
00219     imu_data_msg.angular_velocity.x     =  imu.gx * convertor_d2r;
00220     imu_data_raw_msg.angular_velocity.y =
00221     imu_data_msg.angular_velocity.y     = -imu.gy * convertor_d2r;
00222     imu_data_raw_msg.angular_velocity.z =
00223     imu_data_msg.angular_velocity.z     = -imu.gz * convertor_d2r;
00224 
00225     // original data used the uTesla unit, convert to Tesla
00226     imu_magnetic_msg.magnetic_field.x =  imu.mx / convertor_ut2t;
00227     imu_magnetic_msg.magnetic_field.y = -imu.my / convertor_ut2t;
00228     imu_magnetic_msg.magnetic_field.z = -imu.mz / convertor_ut2t;
00229 
00230     // original data used the celsius unit
00231     imu_temperature_msg.data = imu.temperature;
00232 
00233     // publish the IMU data
00234     imu_data_raw_pub_.publish(imu_data_raw_msg);
00235     imu_data_pub_.publish(imu_data_msg);
00236     imu_mag_pub_.publish(imu_magnetic_msg);
00237     imu_temperature_pub_.publish(imu_temperature_msg);
00238 
00239     // publish tf
00240     if(publish_tf)
00241     {
00242       broadcaster_.sendTransform(tf::StampedTransform(tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
00243                                  tf::Vector3(0.0, 0.0, 0.0)),
00244                                  ros::Time::now(),
00245                                  frame_id_,
00246                                  parent_frame_id_));
00247     }
00248   }
00249 };
00250 
00251 
00252 //------------------------------------------------------------------------------
00253 int main(int argc, char* argv[])
00254 {
00255   ros::init(argc, argv, "myahrs_driver");
00256 
00257   std::string port = std::string("/dev/ttyACM0");
00258   int baud_rate    = 115200;
00259 
00260   ros::param::get("~port", port);
00261   ros::param::get("~baud_rate", baud_rate);
00262 
00263   MyAhrsDriverForROS sensor(port, baud_rate);
00264 
00265   if(sensor.initialize() == false)
00266   {
00267     ROS_ERROR("%s\n", "Initialize() returns false, please check your devices.");
00268     return 0;
00269   }
00270   else
00271   {
00272     ROS_INFO("Initialization OK!\n");
00273   }
00274 
00275   ros::spin();
00276 
00277   return 0;
00278 }
00279 
00280 //------------------------------------------------------------------------------


myahrs_driver
Author(s): Yoonseok Pyo
autogenerated on Sat Jun 8 2019 10:27:56