rt_usb_9axisimu_driver_node.cpp
Go to the documentation of this file.
1 
2 /*
3  * rt_usb_9axisimu_driver_node.cpp
4  *
5  * License: BSD-3-Clause
6  *
7  * Copyright (c) 2015-2020 RT Corporation <support@rt-net.jp>
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright notice,
14  * this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in the
17  * documentation and/or other materials provided with the distribution.
18  * 3. Neither the name of RT Corporation nor the names of its
19  * contributors may be used to endorse or promote products derived from
20  * this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
26  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
29  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
30  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
31  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <string>
36 
37 #include "ros/ros.h"
39 
40 int main(int argc, char** argv)
41 {
42  // init ROS node
43  ros::init(argc, argv, "rt_usb_9axisimu_driver");
44 
45  std::string imu_port = std::string("/dev/ttyACM0");
46  ros::param::get("~port", imu_port);
47  std::string imu_frame_id = std::string("imu_link");
48  ros::param::get("~frame_id", imu_frame_id);
49  rt_usb_9axisimu::Consts imu_consts;
50  double imu_stddev_linear_acceleration = imu_consts.DEFAULT_LINEAR_ACCELERATION_STDDEV;
51  ros::param::get("~linear_acceleration_stddev", imu_stddev_linear_acceleration);
52  double imu_stddev_angular_velocity = imu_consts.DEFAULT_ANGULAR_VELOCITY_STDDEV;
53  ros::param::get("~angular_velocity_stddev", imu_stddev_angular_velocity);
54  double imu_stddev_magnetic_field = imu_consts.DEFAULT_MAGNETIC_FIELD_STDDEV;
55  ros::param::get("~magnetic_field_stddev", imu_stddev_magnetic_field);
56 
57  RtUsb9axisimuRosDriver driver(imu_port);
58  driver.setImuFrameIdName(imu_frame_id);
59  driver.setImuStdDev(imu_stddev_linear_acceleration, imu_stddev_angular_velocity, imu_stddev_magnetic_field);
60 
61  if (driver.startCommunication())
62  {
63  while (ros::ok() && driver.hasCompletedFormatCheck() == false)
64  {
65  driver.checkDataFormat();
66  }
67 
68  if (ros::ok() && driver.hasCompletedFormatCheck())
69  {
70  ROS_INFO("Format check has completed.");
71  if (driver.hasAsciiDataFormat())
72  {
73  ROS_INFO("Data format is ascii.");
74  }
75  else if (driver.hasBinaryDataFormat())
76  {
77  ROS_INFO("Data format is binary.");
78  }
79  else
80  {
81  ROS_ERROR("Data format is neither binary nor ascii.");
82  }
83  }
84 
85  if (driver.hasAsciiDataFormat() || driver.hasBinaryDataFormat())
86  {
87  while (ros::ok())
88  {
89  if (driver.readSensorData())
90  {
91  if (driver.hasRefreshedImuData())
92  {
93  driver.publishImuData();
94  }
95  }
96  else
97  {
98  ROS_ERROR("readSensorData() returns false, please check your devices.\n");
99  }
100  }
101  }
102  driver.stopCommunication();
103  }
104  else
105  {
106  ROS_ERROR("Error opening sensor device, please re-check your devices.\n");
107  }
108 
109  ROS_INFO("Shutting down RT imu driver complete.\n");
110 
111  return 0;
112 }
const double DEFAULT_ANGULAR_VELOCITY_STDDEV
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const double DEFAULT_LINEAR_ACCELERATION_STDDEV
const double DEFAULT_MAGNETIC_FIELD_STDDEV
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define ROS_INFO(...)
void setImuStdDev(double linear_acceleration, double angular_velocity, double magnetic_field)
ROSCPP_DECL bool ok()
int main(int argc, char **argv)
#define ROS_ERROR(...)
void setImuFrameIdName(std::string frame_id)


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