myahrs_driver.cpp
Go to the documentation of this file.
1 //------------------------------------------------------------------------------
2 // Copyright (c) 2015, Yoonseok Pyo
3 // All rights reserved.
4 
5 // License: BSD
6 
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are met:
9 
10 // * Redistributions of source code must retain the above copyright notice, this
11 // list of conditions and the following disclaimer.
12 
13 // * Redistributions in binary form must reproduce the above copyright notice,
14 // this list of conditions and the following disclaimer in the documentation
15 // and/or other materials provided with the distribution.
16 
17 // * Neither the name of myahrs_driver nor the names of its
18 // contributors may be used to endorse or promote products derived from
19 // this software without specific prior written permission.
20 
21 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 //------------------------------------------------------------------------------
33 
34 #include <ros/ros.h>
35 #include <std_msgs/Float64.h>
36 #include <sensor_msgs/Imu.h>
37 #include <sensor_msgs/MagneticField.h>
39 #include <tf/transform_datatypes.h>
40 
41 //------------------------------------------------------------------------------
42 using namespace WithRobot;
43 
44 //------------------------------------------------------------------------------
46 {
47 private:
50 
55 
57 
60 
61  std::string parent_frame_id_;
62  std::string frame_id_;
67 
68  void OnSensorData(int sensor_id, SensorData data)
69  {
70  LockGuard _l(lock_);
71  sensor_data_ = data;
72  publish_topic(sensor_id);
73  }
74 
75  void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
76  {
77  printf("OnAttributeChange(id %d, %s, %s)\n", sensor_id, attribute_name.c_str(), value.c_str());
78  }
79 
80 public:
81  MyAhrsDriverForROS(std::string port="", int baud_rate=115200)
82  : iMyAhrsPlus(port, baud_rate),
83  nh_priv_("~")
84  {
85  // dependent on user device
86  nh_priv_.setParam("port", port);
87  nh_priv_.setParam("baud_rate", baud_rate);
88  // default frame id
89  nh_priv_.param("frame_id", frame_id_, std::string("imu_link"));
90  // for testing the tf
91  nh_priv_.param("parent_frame_id_", parent_frame_id_, std::string("base_link"));
92  // defaults obtained experimentally from device
93  nh_priv_.param("linear_acceleration_stddev", linear_acceleration_stddev_, 0.026831);
94  nh_priv_.param("angular_velocity_stddev", angular_velocity_stddev_, 0.002428);
95  nh_priv_.param("magnetic_field_stddev", magnetic_field_stddev_, 0.00000327486);
96  nh_priv_.param("orientation_stddev", orientation_stddev_, 0.002143);
97  // publisher for streaming
98  imu_data_raw_pub_ = nh_.advertise<sensor_msgs::Imu>("imu/data_raw", 1);
99  imu_data_pub_ = nh_.advertise<sensor_msgs::Imu>("imu/data", 1);
100  imu_mag_pub_ = nh_.advertise<sensor_msgs::MagneticField>("imu/mag", 1);
101  imu_temperature_pub_= nh_.advertise<std_msgs::Float64>("imu/temperature", 1);
102  }
103 
105  {}
106 
107  bool initialize()
108  {
109  bool ok = false;
110 
111  do
112  {
113  if(start() == false) break;
114  //Euler angle(x, y, z axis)
115  //IMU(linear_acceleration, angular_velocity, magnetic_field)
116  if(cmd_binary_data_format("EULER, IMU") == false) break;
117  // 100Hz
118  if(cmd_divider("1") == false) break;
119  // Binary and Continue mode
120  if(cmd_mode("BC") == false) break;
121  ok = true;
122  } while(0);
123 
124  return ok;
125  }
126 
127  inline void get_data(SensorData& data)
128  {
129  LockGuard _l(lock_);
130  data = sensor_data_;
131  }
132 
134  {
135  LockGuard _l(lock_);
136  return sensor_data_;
137  }
138 
139  void publish_topic(int sensor_id)
140  {
141  sensor_msgs::Imu imu_data_raw_msg;
142  sensor_msgs::Imu imu_data_msg;
143  sensor_msgs::MagneticField imu_magnetic_msg;
144  std_msgs::Float64 imu_temperature_msg;
145 
146  double linear_acceleration_cov = linear_acceleration_stddev_ * linear_acceleration_stddev_;
147  double angular_velocity_cov = angular_velocity_stddev_ * angular_velocity_stddev_;
148  double magnetic_field_cov = magnetic_field_stddev_ * magnetic_field_stddev_;
149  double orientation_cov = orientation_stddev_ * orientation_stddev_;
150 
151  imu_data_raw_msg.linear_acceleration_covariance[0] =
152  imu_data_raw_msg.linear_acceleration_covariance[4] =
153  imu_data_raw_msg.linear_acceleration_covariance[8] =
154  imu_data_msg.linear_acceleration_covariance[0] =
155  imu_data_msg.linear_acceleration_covariance[4] =
156  imu_data_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
157 
158  imu_data_raw_msg.angular_velocity_covariance[0] =
159  imu_data_raw_msg.angular_velocity_covariance[4] =
160  imu_data_raw_msg.angular_velocity_covariance[8] =
161  imu_data_msg.angular_velocity_covariance[0] =
162  imu_data_msg.angular_velocity_covariance[4] =
163  imu_data_msg.angular_velocity_covariance[8] = angular_velocity_cov;
164 
165  imu_data_msg.orientation_covariance[0] =
166  imu_data_msg.orientation_covariance[4] =
167  imu_data_msg.orientation_covariance[8] = orientation_cov;
168 
169  imu_magnetic_msg.magnetic_field_covariance[0] =
170  imu_magnetic_msg.magnetic_field_covariance[4] =
171  imu_magnetic_msg.magnetic_field_covariance[8] = magnetic_field_cov;
172 
173  static double convertor_g2a = 9.80665; // for linear_acceleration (g to m/s^2)
174  static double convertor_d2r = M_PI/180.0; // for angular_velocity (degree to radian)
175  static double convertor_r2d = 180.0/M_PI; // for easy understanding (radian to degree)
176  static double convertor_ut2t = 1000000; // for magnetic_field (uT to Tesla)
177  static double convertor_c = 1.0; // for temperature (celsius)
178 
179  double roll, pitch, yaw;
180 
181  // original sensor data used the degree unit, convert to radian (see ROS REP103)
182  // we used the ROS's axes orientation like x forward, y left and z up
183  // so changed the y and z aixs of myAHRS+ board
184  roll = sensor_data_.euler_angle.roll*convertor_d2r;
185  pitch = -sensor_data_.euler_angle.pitch*convertor_d2r;
186  yaw = -sensor_data_.euler_angle.yaw*convertor_d2r;
187 
188  ImuData<float>& imu = sensor_data_.imu;
189 
190  tf::Quaternion orientation = tf::createQuaternionFromRPY(roll, pitch, yaw);
191 
192  ros::Time now = ros::Time::now();
193 
194  imu_data_raw_msg.header.stamp =
195  imu_data_msg.header.stamp =
196  imu_magnetic_msg.header.stamp = now;
197 
198  imu_data_raw_msg.header.frame_id =
199  imu_data_msg.header.frame_id =
200  imu_magnetic_msg.header.frame_id = frame_id_;
201 
202  // orientation
203  imu_data_msg.orientation.x = orientation[0];
204  imu_data_msg.orientation.y = orientation[1];
205  imu_data_msg.orientation.z = orientation[2];
206  imu_data_msg.orientation.w = orientation[3];
207 
208  // original data used the g unit, convert to m/s^2
209  imu_data_raw_msg.linear_acceleration.x =
210  imu_data_msg.linear_acceleration.x = imu.ax * convertor_g2a;
211  imu_data_raw_msg.linear_acceleration.y =
212  imu_data_msg.linear_acceleration.y = -imu.ay * convertor_g2a;
213  imu_data_raw_msg.linear_acceleration.z =
214  imu_data_msg.linear_acceleration.z = -imu.az * convertor_g2a;
215 
216  // original data used the degree/s unit, convert to radian/s
217  imu_data_raw_msg.angular_velocity.x =
218  imu_data_msg.angular_velocity.x = imu.gx * convertor_d2r;
219  imu_data_raw_msg.angular_velocity.y =
220  imu_data_msg.angular_velocity.y = -imu.gy * convertor_d2r;
221  imu_data_raw_msg.angular_velocity.z =
222  imu_data_msg.angular_velocity.z = -imu.gz * convertor_d2r;
223 
224  // original data used the uTesla unit, convert to Tesla
225  imu_magnetic_msg.magnetic_field.x = imu.mx / convertor_ut2t;
226  imu_magnetic_msg.magnetic_field.y = -imu.my / convertor_ut2t;
227  imu_magnetic_msg.magnetic_field.z = -imu.mz / convertor_ut2t;
228 
229  // original data used the celsius unit
230  imu_temperature_msg.data = imu.temperature;
231 
232  // publish the IMU data
233  imu_data_raw_pub_.publish(imu_data_raw_msg);
234  imu_data_pub_.publish(imu_data_msg);
235  imu_mag_pub_.publish(imu_magnetic_msg);
236  imu_temperature_pub_.publish(imu_temperature_msg);
237 
238  // publish tf
240  tf::Vector3(0.0, 0.0, 0.0)),
241  ros::Time::now(), parent_frame_id_, frame_id_));
242  }
243 };
244 
245 
246 //------------------------------------------------------------------------------
247 int main(int argc, char* argv[])
248 {
249  ros::init(argc, argv, "myahrs_driver");
250 
251  std::string port = std::string("/dev/ttyACM0");
252  int baud_rate = 115200;
253 
254  ros::param::get("~port", port);
255  ros::param::get("~baud_rate", baud_rate);
256 
257  MyAhrsDriverForROS sensor(port, baud_rate);
258 
259  if(sensor.initialize() == false)
260  {
261  ROS_ERROR("%s\n", "Initialize() returns false, please check your devices.");
262  return 0;
263  }
264  else
265  {
266  ROS_INFO("Initialization OK!\n");
267  }
268 
269  ros::spin();
270 
271  return 0;
272 }
273 
274 //------------------------------------------------------------------------------
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
ros::Publisher imu_data_pub_
void OnSensorData(int sensor_id, SensorData data)
std::string parent_frame_id_
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::TransformBroadcaster broadcaster_
ImuData< float > imu
SensorData get_data()
ros::Publisher imu_temperature_pub_
ros::NodeHandle nh_
ros::NodeHandle nh_priv_
MyAhrsDriverForROS(std::string port="", int baud_rate=115200)
ros::Publisher imu_mag_pub_
ROSCPP_DECL void spin(Spinner &spinner)
void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
void publish_topic(int sensor_id)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void sendTransform(const StampedTransform &transform)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Platform::Mutex lock_
SensorData sensor_data_
static Time now()
double linear_acceleration_stddev_
int main(int argc, char *argv[])
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
std::string frame_id_
void get_data(SensorData &data)
double angular_velocity_stddev_
ros::Publisher imu_data_raw_pub_


myahrs_driver
Author(s): Yoonseok Pyo
autogenerated on Thu Jul 16 2020 03:08:51