rt_usb_9axisimu_driver.cpp
Go to the documentation of this file.
1 /*
2  * rt_usb_9axisimu_driver.cpp
3  *
4  * License: BSD-3-Clause
5  *
6  * Copyright (c) 2015-2020 RT Corporation <support@rt-net.jp>
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * 1. Redistributions of source code must retain the above copyright notice,
13  * this list of conditions and the following disclaimer.
14  * 2. Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * 3. Neither the name of RT Corporation 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
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <string>
35 #include <vector>
36 
37 #include "ros/ros.h"
38 #include "sensor_msgs/Imu.h"
39 #include "sensor_msgs/MagneticField.h"
40 #include "std_msgs/Float64.h"
42 
43 
44 // Method to combine two separate one-byte data into one two-byte data
45 int16_t RtUsb9axisimuRosDriver::combineByteData(unsigned char data_h, unsigned char data_l)
46 {
47  int16_t short_data = 0;
48 
49  short_data = data_h;
50  short_data = short_data << 8;
51  short_data |= data_l;
52 
53  return short_data;
54 }
55 
56 // Method to extract binary sensor data from communication buffer
59 {
61 
62  imu_rawdata.firmware_ver = imu_data_buf[consts.IMU_BIN_FIRMWARE];
63  imu_rawdata.timestamp = imu_data_buf[consts.IMU_BIN_TIMESTAMP];
64  imu_rawdata.temperature = combineByteData(imu_data_buf[consts.IMU_BIN_TEMP_H], imu_data_buf[consts.IMU_BIN_TEMP_L]);
65  imu_rawdata.ax = combineByteData(imu_data_buf[consts.IMU_BIN_ACC_X_H], imu_data_buf[consts.IMU_BIN_ACC_X_L]);
66  imu_rawdata.ay = combineByteData(imu_data_buf[consts.IMU_BIN_ACC_Y_H], imu_data_buf[consts.IMU_BIN_ACC_Y_L]);
67  imu_rawdata.az = combineByteData(imu_data_buf[consts.IMU_BIN_ACC_Z_H], imu_data_buf[consts.IMU_BIN_ACC_Z_L]);
68  imu_rawdata.gx = combineByteData(imu_data_buf[consts.IMU_BIN_GYRO_X_H], imu_data_buf[consts.IMU_BIN_GYRO_X_L]);
69  imu_rawdata.gy = combineByteData(imu_data_buf[consts.IMU_BIN_GYRO_Y_H], imu_data_buf[consts.IMU_BIN_GYRO_Y_L]);
70  imu_rawdata.gz = combineByteData(imu_data_buf[consts.IMU_BIN_GYRO_Z_H], imu_data_buf[consts.IMU_BIN_GYRO_Z_L]);
71  imu_rawdata.mx = combineByteData(imu_data_buf[consts.IMU_BIN_MAG_X_H], imu_data_buf[consts.IMU_BIN_MAG_X_L]);
72  imu_rawdata.my = combineByteData(imu_data_buf[consts.IMU_BIN_MAG_Y_H], imu_data_buf[consts.IMU_BIN_MAG_Y_L]);
73  imu_rawdata.mz = combineByteData(imu_data_buf[consts.IMU_BIN_MAG_Z_H], imu_data_buf[consts.IMU_BIN_MAG_Z_L]);
74 
75  return imu_rawdata;
76 }
77 
78 bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char* imu_data_buf)
79 {
80  if (imu_data_buf[consts.IMU_BIN_HEADER_R] == 'R' && imu_data_buf[consts.IMU_BIN_HEADER_T] == 'T')
81  {
82  return true;
83  }
84  return false;
85 }
86 
88 {
89  unsigned char imu_data_buf[256];
91 
93  int data_size_of_buf = readFromDevice(imu_data_buf, consts.IMU_BIN_DATA_SIZE);
94 
95  if (data_size_of_buf < consts.IMU_BIN_DATA_SIZE)
96  {
97  if (data_size_of_buf <= 0)
98  {
99  return false; // Raise communication error
100  }
101  return false;
102  }
103 
104  if (isBinarySensorData(imu_data_buf) == false)
105  {
106  return false;
107  }
108 
109  imu_rawdata = extractBinarySensorData(imu_data_buf); // Extract sensor data
110 
111  sensor_data_.setImuRawData(imu_rawdata); // Update raw data
112  sensor_data_.convertRawDataUnit(); // Convert raw data to physical quantity
114 
115  return true;
116 }
117 
118 bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector<std::string> str_vector)
119 {
120  for (int i = 1; i < consts.IMU_ASCII_DATA_SIZE; i++)
121  {
122  if (strspn(str_vector[i].c_str(), "-.0123456789") != str_vector[i].size())
123  {
124  return false;
125  }
126  }
127  return true;
128 }
129 
131 {
132  static std::vector<std::string> imu_data_vector_buf;
133 
134  unsigned char imu_data_buf[256];
136  std::string imu_data_oneline_buf;
137 
138  has_refreshed_imu_data_ = false;
139  imu_data_oneline_buf.clear();
140 
141  int data_size_of_buf = readFromDevice(imu_data_buf, sizeof(imu_data_buf));
142 
143  if (data_size_of_buf <= 0)
144  {
145  return false; // Raise communication error
146  }
147 
148  for (int char_count = 0; char_count < data_size_of_buf; char_count++)
149  {
150  if (imu_data_buf[char_count] == ',' || imu_data_buf[char_count] == '\n')
151  {
152  imu_data_vector_buf.push_back(imu_data_oneline_buf);
153  imu_data_oneline_buf.clear();
154  }
155  else
156  {
157  imu_data_oneline_buf += imu_data_buf[char_count];
158  }
159 
160  if (imu_data_buf[char_count] == '\n' && imu_data_vector_buf.size() == consts.IMU_ASCII_DATA_SIZE &&
161  imu_data_vector_buf[0].find(".") == std::string::npos && isValidAsciiSensorData(imu_data_vector_buf))
162  {
163  imu_data.gx = std::stof(imu_data_vector_buf[consts.IMU_ASCII_GYRO_X]);
164  imu_data.gy = std::stof(imu_data_vector_buf[consts.IMU_ASCII_GYRO_Y]);
165  imu_data.gz = std::stof(imu_data_vector_buf[consts.IMU_ASCII_GYRO_Z]);
166  imu_data.ax = std::stof(imu_data_vector_buf[consts.IMU_ASCII_ACC_X]);
167  imu_data.ay = std::stof(imu_data_vector_buf[consts.IMU_ASCII_ACC_Y]);
168  imu_data.az = std::stof(imu_data_vector_buf[consts.IMU_ASCII_ACC_Z]);
169  imu_data.mx = std::stof(imu_data_vector_buf[consts.IMU_ASCII_MAG_X]);
170  imu_data.my = std::stof(imu_data_vector_buf[consts.IMU_ASCII_MAG_Y]);
171  imu_data.mz = std::stof(imu_data_vector_buf[consts.IMU_ASCII_MAG_Z]);
172  imu_data.temperature = std::stof(imu_data_vector_buf[consts.IMU_ASCII_TEMP]);
173 
174  imu_data_vector_buf.clear();
175  sensor_data_.setImuData(imu_data);
177  }
178  else if (imu_data_vector_buf.size() > consts.IMU_ASCII_DATA_SIZE)
179  {
180  imu_data_vector_buf.clear();
181  ROS_WARN("ASCII data size is incorrect.");
182  }
183  }
184 
185  return true;
186 }
187 
189  : rt_usb_9axisimu::SerialPort(port.c_str())
190 {
191  // publisher for streaming
192  imu_data_raw_pub_ = nh_.advertise<sensor_msgs::Imu>("imu/data_raw", 1);
193  imu_mag_pub_ = nh_.advertise<sensor_msgs::MagneticField>("imu/mag", 1);
194  imu_temperature_pub_ = nh_.advertise<std_msgs::Float64>("imu/temperature", 1);
195 
197  data_format_ = DataFormat::NONE;
198  has_refreshed_imu_data_ = false;
199 }
200 
202 {
203 }
204 
206 {
207  frame_id_ = frame_id;
208 }
209 
210 void RtUsb9axisimuRosDriver::setImuPortName(std::string serialport)
211 {
212  rt_usb_9axisimu::SerialPort(serialport.c_str());
213 }
214 
215 void RtUsb9axisimuRosDriver::setImuStdDev(double linear_acceleration, double angular_velocity,
216  double magnetic_field)
217 {
218  linear_acceleration_stddev_ = linear_acceleration;
219  angular_velocity_stddev_ = angular_velocity;
220  magnetic_field_stddev_ = magnetic_field;
221 }
222 
224 {
225  // returns serial port open status
226  return openSerialPort();
227 }
228 
230 {
231  closeSerialPort();
232 }
233 
235 {
236  if (data_format_ == DataFormat::NONE)
237  {
238  unsigned char data_buf[256];
239  int data_size_of_buf = readFromDevice(data_buf, consts.IMU_BIN_DATA_SIZE);
240  if (data_size_of_buf == consts.IMU_BIN_DATA_SIZE)
241  {
242  if (isBinarySensorData(data_buf))
243  {
244  data_format_ = DataFormat::BINARY;
246  }
247  else
248  {
249  data_format_ = DataFormat::NOT_BINARY;
250  }
251  }
252  }
253  else if (data_format_ == DataFormat::NOT_BINARY)
254  {
255  data_format_ = DataFormat::ASCII;
257  }
258 }
259 
261 {
263 }
264 
266 {
267  return data_format_ == DataFormat::ASCII;
268 }
269 
271 {
272  return data_format_ == DataFormat::BINARY;
273 }
274 
276 {
278 }
279 
281 {
283  sensor_msgs::Imu imu_data_raw_msg;
284  sensor_msgs::MagneticField imu_magnetic_msg;
285  std_msgs::Float64 imu_temperature_msg;
286 
287  imu = sensor_data_.getImuData(); // Get physical quantity
288 
289  // Calculate linear_acceleration_covariance diagonal elements
290  double linear_acceleration_cov = linear_acceleration_stddev_ * linear_acceleration_stddev_;
291  // Calculate angular_velocity_covariance diagonal elements
292  double angular_velocity_cov = angular_velocity_stddev_ * angular_velocity_stddev_;
293  // Calculate magnetic_field_covariance diagonal elements
294  double magnetic_field_cov = magnetic_field_stddev_ * magnetic_field_stddev_;
295 
296  // imu_data_raw_msg has no orientation values
297  imu_data_raw_msg.orientation_covariance[0] = -1;
298 
299  imu_data_raw_msg.linear_acceleration_covariance[0] = imu_data_raw_msg.linear_acceleration_covariance[4] =
300  imu_data_raw_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
301 
302  imu_data_raw_msg.angular_velocity_covariance[0] = imu_data_raw_msg.angular_velocity_covariance[4] =
303  imu_data_raw_msg.angular_velocity_covariance[8] = angular_velocity_cov;
304 
305  imu_magnetic_msg.magnetic_field_covariance[0] = imu_magnetic_msg.magnetic_field_covariance[4] =
306  imu_magnetic_msg.magnetic_field_covariance[8] = magnetic_field_cov;
307 
308  ros::Time now = ros::Time::now();
309 
310  imu_data_raw_msg.header.stamp = imu_magnetic_msg.header.stamp = now;
311 
312  imu_data_raw_msg.header.frame_id = imu_magnetic_msg.header.frame_id = frame_id_;
313 
314  // original data used the g unit, convert to m/s^2
315  imu_data_raw_msg.linear_acceleration.x = imu.ax * consts.CONVERTOR_G2A;
316  imu_data_raw_msg.linear_acceleration.y = imu.ay * consts.CONVERTOR_G2A;
317  imu_data_raw_msg.linear_acceleration.z = imu.az * consts.CONVERTOR_G2A;
318 
319  if (data_format_ == DataFormat::BINARY)
320  {
321  // original binary data used the degree/s unit, convert to radian/s
322  imu_data_raw_msg.angular_velocity.x = imu.gx * consts.CONVERTOR_D2R;
323  imu_data_raw_msg.angular_velocity.y = imu.gy * consts.CONVERTOR_D2R;
324  imu_data_raw_msg.angular_velocity.z = imu.gz * consts.CONVERTOR_D2R;
325  }
326  else if (data_format_ == DataFormat::ASCII)
327  {
328  // original ascii data used the radian/s
329  imu_data_raw_msg.angular_velocity.x = imu.gx;
330  imu_data_raw_msg.angular_velocity.y = imu.gy;
331  imu_data_raw_msg.angular_velocity.z = imu.gz;
332  }
333 
334  // original data used the uTesla unit, convert to Tesla
335  imu_magnetic_msg.magnetic_field.x = imu.mx / consts.CONVERTOR_UT2T;
336  imu_magnetic_msg.magnetic_field.y = imu.my / consts.CONVERTOR_UT2T;
337  imu_magnetic_msg.magnetic_field.z = imu.mz / consts.CONVERTOR_UT2T;
338 
339  // original data used the celsius unit
340  imu_temperature_msg.data = imu.temperature;
341 
342  // publish the IMU data
343  imu_data_raw_pub_.publish(imu_data_raw_msg);
344  imu_mag_pub_.publish(imu_magnetic_msg);
345  imu_temperature_pub_.publish(imu_temperature_msg);
346 
347  return true;
348 }
349 
350 // Method to receive IMU data, convert those units to SI, and publish to ROS
351 // topic
353 {
354  if (data_format_ == DataFormat::BINARY)
355  {
356  return readBinaryData();
357  }
358  else if (data_format_ == DataFormat::ASCII)
359  {
360  return readAsciiData();
361  }
362 
363  return false;
364 }
void setImuPortName(std::string serialport)
SerialPort(const char *port="")
void setImuRawData(ImuData< int16_t > &i)
void publish(const boost::shared_ptr< M > &message) const
rt_usb_9axisimu::Consts consts
void setImuData(ImuData< double > &i)
#define ROS_WARN(...)
rt_usb_9axisimu::ImuData< int16_t > extractBinarySensorData(unsigned char *imu_data_buf)
void setImuStdDev(double linear_acceleration, double angular_velocity, double magnetic_field)
ImuData< double > getImuData()
bool isBinarySensorData(unsigned char *imu_data_buf)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int16_t combineByteData(unsigned char data_h, unsigned char data_l)
rt_usb_9axisimu::SensorData sensor_data_
RtUsb9axisimuRosDriver(std::string serialport)
static Time now()
int readFromDevice(unsigned char *buf, unsigned int buf_len)
bool isValidAsciiSensorData(std::vector< std::string > imu_data_vector_buf)
void setImuFrameIdName(std::string frame_id)


rt_usb_9axisimu_driver
Author(s): RT Corporation
autogenerated on Mon Dec 14 2020 03:34:37