00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
00086 nh_priv_.setParam("port", port);
00087 nh_priv_.setParam("baud_rate", baud_rate);
00088 nh_priv_.param("publish_tf", publish_tf, false);
00089
00090 nh_priv_.param("frame_id", frame_id_, std::string("imu_link"));
00091
00092 nh_priv_.param("parent_frame_id_", parent_frame_id_, std::string("base_link"));
00093
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
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
00116
00117 if(cmd_binary_data_format("EULER, IMU") == false) break;
00118
00119 if(cmd_divider("1") == false) break;
00120
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;
00175 static double convertor_d2r = M_PI/180.0;
00176 static double convertor_r2d = 180.0/M_PI;
00177 static double convertor_ut2t = 1000000;
00178 static double convertor_c = 1.0;
00179
00180 double roll, pitch, yaw;
00181
00182
00183
00184
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
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
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
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
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
00231 imu_temperature_msg.data = imu.temperature;
00232
00233
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
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