├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── wit_imu_driver │ ├── wit_imu.h │ └── wt901c.h ├── launch └── wit901.launch.py ├── package.xml └── src ├── wit_imu_driver_node.cpp └── wt901c.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(wit_imu_driver) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(sensor_msgs REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(std_srvs REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | 16 | set(dependencies 17 | rclcpp 18 | sensor_msgs 19 | std_msgs 20 | std_srvs 21 | tf2 22 | ) 23 | 24 | include_directories(include) 25 | 26 | add_executable( 27 | wit_imu_node 28 | "src/wt901c.cpp" 29 | "src/wit_imu_driver_node.cpp" 30 | ) 31 | 32 | ament_target_dependencies(wit_imu_node ${dependencies}) 33 | 34 | install( 35 | TARGETS wit_imu_node 36 | DESTINATION lib/${PROJECT_NAME} 37 | ) 38 | 39 | install( 40 | DIRECTORY launch 41 | DESTINATION share/${PROJECT_NAME} 42 | ) 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | ament_lint_auto_find_test_dependencies() 47 | endif() 48 | 49 | ament_package() 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 YLFeng 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_wit_imu_node 2 | WIT motion's IMU driver node for ROS2. Refer to [wit_imu_driver](https://github.com/strv/wit_imu_driver) in ROS1. 3 | 4 | # Installation 5 | 6 | To install the ros_wit_imu_node, open a bash terminal, clone the package from Github, and build it: 7 | 8 | ```bash 9 | cd ~/ros_ws/src/ 10 | git clone https://github.com/Ericsii/ros_wit_imu_node.git 11 | cd .. 12 | rosdep install --from-paths src --ignore-src -r -y 13 | colcon build --symlink-install 14 | ``` 15 | 16 | # Usage 17 | 18 | ```bash 19 | cd ~/ros_ws/src/ 20 | . ./install/setup.bash # use setup.zsh if using zsh as default shell 21 | ros2 run wit_imu_driver wit_imu_node 22 | ``` 23 | 24 | # Published Topics 25 | 26 | - `~/data_raw`: IMU data (`sensor_msg::Imu`) 27 | - `~/temperature`: Temperature from sensor in device (`sensor_msgs::Temperature`) 28 | - `~/mag`: MagneticField (`sensor_msg::MagneticField`) 29 | 30 | # Parameters 31 | 32 | - `gravity`: Gravity at your site. Default: `9.797673` 33 | - `frame_id`: frame_id of output topics. Default: `imu_link` 34 | - `device`: Device name to communicate. Default : `/dev/ttyUSB0` 35 | - `baud`: Communication baudrate. Default : `9600` 36 | 37 | # Services 38 | 39 | - `trigger_yaw_clear`: Type `std_srvs::Trigger` 40 | - `trigger_acc_calibration`: Type `std_srvs::Trigger` 41 | - `trigger_mag_calibration`: Type `std_srvs::Trigger` 42 | - `trigger_exit_calibration`: Type `std_srvs::Trigger` 43 | 44 | # License 45 | 46 | `ros_wit_imu_node` is under the terms of the MIT license. 47 | -------------------------------------------------------------------------------- /include/wit_imu_driver/wit_imu.h: -------------------------------------------------------------------------------- 1 | #ifndef WIT_IMU_DRIVER_WIT_IMU_H 2 | #define WIT_IMU_DRIVER_WIT_IMU_H 3 | 4 | #include 5 | #include 6 | 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "sensor_msgs/msg/imu.hpp" 9 | #include "sensor_msgs/msg/magnetic_field.hpp" 10 | #include "sensor_msgs/msg/temperature.hpp" 11 | 12 | namespace wit_imu_driver 13 | { 14 | 15 | class WitImu 16 | { 17 | public: 18 | enum PRODUCT 19 | { 20 | WT901C 21 | }; 22 | 23 | explicit WitImu(const double co_gravity = 9.8, const size_t msg_buffer_size = 100) 24 | : buf_(1024) 25 | , co_gravity_(co_gravity) 26 | , msg_buf_max_(msg_buffer_size) 27 | { 28 | } 29 | 30 | virtual void pushBytes(const std::vector& bytes, 31 | const size_t size, 32 | const rclcpp::Time& stamp) = 0; 33 | 34 | bool popImuData(sensor_msgs::msg::Imu* const p_msg) 35 | { 36 | if (imu_buf_.empty()) 37 | { 38 | return false; 39 | } 40 | *p_msg = imu_buf_.front(); 41 | imu_buf_.pop(); 42 | return true; 43 | }; 44 | 45 | size_t sizeImuData() const 46 | { 47 | return imu_buf_.size(); 48 | }; 49 | 50 | bool popTempData(sensor_msgs::msg::Temperature* const p_msg) 51 | { 52 | if (temp_buf_.empty()) 53 | { 54 | return false; 55 | } 56 | *p_msg = temp_buf_.front(); 57 | temp_buf_.pop(); 58 | return true; 59 | }; 60 | 61 | size_t sizeTempData() const 62 | { 63 | return temp_buf_.size(); 64 | }; 65 | 66 | bool popMagData(sensor_msgs::msg::MagneticField* const p_msg) 67 | { 68 | if (mag_buf_.empty()) 69 | { 70 | return false; 71 | } 72 | *p_msg = mag_buf_.front(); 73 | mag_buf_.pop(); 74 | return true; 75 | }; 76 | 77 | size_t sizeMagData() const 78 | { 79 | return mag_buf_.size(); 80 | }; 81 | 82 | virtual std::vector genYawClr() const = 0; 83 | virtual std::vector genHightClr() const = 0; 84 | virtual std::vector genAccCal() const = 0; 85 | virtual std::vector genMagCal() const = 0; 86 | virtual std::vector genExitCal() const = 0; 87 | protected: 88 | std::vector buf_; 89 | const double co_gravity_; 90 | const size_t msg_buf_max_; 91 | std::queue imu_buf_; 92 | std::queue temp_buf_; 93 | std::queue mag_buf_; 94 | 95 | static int bytes2int(const uint8_t h, const uint8_t l) 96 | { 97 | return static_cast( 98 | ((static_cast(h) << 8) & 0xFF00) 99 | | (static_cast(l) & 0x00FF)); 100 | } 101 | }; 102 | } // namespace wit_imu_driver 103 | 104 | #endif // WIT_IMU_DRIVER_WIT_IMU_H 105 | -------------------------------------------------------------------------------- /include/wit_imu_driver/wt901c.h: -------------------------------------------------------------------------------- 1 | #ifndef WIT_IMU_DRIVER_WT901C_H 2 | #define WIT_IMU_DRIVER_WT901C_H 3 | 4 | #include 5 | #include 6 | 7 | #include "wit_imu_driver/wit_imu.h" 8 | #include "rclcpp/rclcpp.hpp" 9 | #include "sensor_msgs/msg/imu.hpp" 10 | #include "sensor_msgs/msg/magnetic_field.hpp" 11 | #include "sensor_msgs/msg/temperature.hpp" 12 | 13 | namespace wit_imu_driver 14 | { 15 | class Wt901c : public WitImu 16 | { 17 | public: 18 | explicit Wt901c(const double co_gravity = 9.8); 19 | void pushBytes(const std::vector& bytes, 20 | const size_t size, 21 | const rclcpp::Time& stamp); 22 | virtual std::vector genYawClr() const; 23 | virtual std::vector genHightClr() const; 24 | virtual std::vector genAccCal() const; 25 | virtual std::vector genMagCal() const; 26 | virtual std::vector genExitCal() const; 27 | private: 28 | sensor_msgs::msg::Imu work_imu_; 29 | sensor_msgs::msg::Temperature work_temp_; 30 | sensor_msgs::msg::MagneticField work_mag_; 31 | const double co_acc_; 32 | const double co_avel_; 33 | const double co_temp_; 34 | const double co_mag_; 35 | const double co_pose_; 36 | }; 37 | } // namespace wit_imu_driver 38 | 39 | #endif // WIT_IMU_DRIVER_WT901C_H 40 | -------------------------------------------------------------------------------- /launch/wit901.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch.launch_description import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | 6 | from launch_ros.actions import Node 7 | 8 | 9 | def generate_launch_description(): 10 | # Declare arguments 11 | declare_port = DeclareLaunchArgument( 12 | 'port', default_value='/dev/ttyUSB0', 13 | description='Port for wit901 imu') 14 | declare_buadrate = DeclareLaunchArgument( 15 | 'buadrate', default_value='115200', 16 | description='Buadrate for wit901 imu') 17 | declare_frame_id = DeclareLaunchArgument( 18 | 'frame_id', default_value='imu_link', 19 | description='Frame id for wit901 imu') 20 | declare_gravity = DeclareLaunchArgument( 21 | 'gravity', default_value='9.80665', 22 | description='Gravity for wit901 imu') 23 | 24 | # Node 25 | node = Node( 26 | package='wit_imu_driver', 27 | executable='wit_imu_node', 28 | name='wit_imu_node', 29 | output='screen', 30 | parameters=[{ 31 | 'device': LaunchConfiguration('port'), 32 | 'baud': LaunchConfiguration('buadrate'), 33 | 'frame_id': LaunchConfiguration('frame_id'), 34 | 'gravity': LaunchConfiguration('gravity') 35 | }], 36 | namespace='imu' 37 | ) 38 | 39 | ld = LaunchDescription() 40 | ld.add_action(declare_port) 41 | ld.add_action(declare_buadrate) 42 | ld.add_action(declare_frame_id) 43 | ld.add_action(declare_gravity) 44 | ld.add_action(node) 45 | 46 | return ld 47 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | wit_imu_driver 5 | 1.0.0 6 | Package for wit imu device 7 | Eric Feng 8 | MIT License 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | std_msgs 15 | std_srvs 16 | tf2 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/wit_imu_driver_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // #include "rclcpp/rclcpp.hpp" 11 | // #include 12 | // #include 13 | // #include 14 | // #include 15 | // #include 16 | #include "std_srvs/srv/trigger.hpp" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono; 27 | 28 | namespace wit_imu_driver 29 | { 30 | namespace ba = boost::asio; 31 | 32 | enum TrgState{ 33 | YAWCLR, 34 | ACCCAL, 35 | MAGCAL, 36 | EXICAL 37 | }; 38 | 39 | class WitImuDriver 40 | { 41 | using imu_msg = sensor_msgs::msg::Imu; 42 | using temp_msg = sensor_msgs::msg::Temperature; 43 | using mag_msg = sensor_msgs::msg::MagneticField; 44 | using trigger = std_srvs::srv::Trigger; 45 | public: 46 | WitImuDriver(rclcpp::Node::SharedPtr node) 47 | : node_(node) 48 | , port_io_() 49 | , port_(port_io_) 50 | , wdg_timeout_(0.5s) 51 | , rx_buf_(1024) 52 | { 53 | node_->declare_parameter("gravity", 9.797673); 54 | node_->declare_parameter("frame_id", "imu_link"); 55 | node_->declare_parameter("product", (int)WitImu::PRODUCT::WT901C); 56 | node_->declare_parameter("device", "/dev/ttyUSB0"); 57 | node_->declare_parameter("baud", 9600); 58 | // pnh_.param("gravity", co_gravity_, 9.797673); 59 | // pnh_.param("frame_id", frame_id_, "imu_link"); 60 | // pnh_.param("product", product_, WitImu::PRODUCT::WT901C); 61 | } 62 | 63 | bool open() 64 | { 65 | std::string dev; 66 | int baud; 67 | dev = node_->get_parameter("device").as_string(); 68 | baud = node_->get_parameter("baud").as_int(); 69 | // pnh_.param("device", dev, "/dev/ttyUSB0"); 70 | // pnh_.param("baud", baud, 9600); 71 | 72 | boost::system::error_code ec; 73 | port_.open(dev, ec); 74 | if (ec.value() != 0) 75 | { 76 | RCLCPP_ERROR( 77 | node_->get_logger(), 78 | "Failed to open %s. Error code : %d", 79 | dev.c_str(), 80 | ec.value() 81 | ); 82 | return false; 83 | } 84 | port_.set_option(ba::serial_port_base::baud_rate(baud), ec); 85 | if (ec.value() != 0) 86 | { 87 | RCLCPP_ERROR(node_->get_logger(), 88 | "Failed to set baudrate. Error code: %d", 89 | ec.value() 90 | ); 91 | return false; 92 | } 93 | port_.set_option(ba::serial_port_base::character_size(8), ec); 94 | if (ec.value() != 0) 95 | { 96 | RCLCPP_ERROR(node_->get_logger(), 97 | "Failed to set options. Error code: %d", 98 | ec.value() 99 | ); 100 | return false; 101 | } 102 | port_.set_option(ba::serial_port_base::flow_control( 103 | ba::serial_port_base::flow_control::none), ec); 104 | if (ec.value() != 0) 105 | { 106 | RCLCPP_ERROR(node_->get_logger(), 107 | "Failed to set options. Error code: %d", 108 | ec.value() 109 | ); 110 | return false; 111 | } 112 | port_.set_option(ba::serial_port_base::parity( 113 | ba::serial_port_base::parity::none), ec); 114 | if (ec.value() != 0) 115 | { 116 | RCLCPP_ERROR(node_->get_logger(), 117 | "Failed to set options. Error code: %d", 118 | ec.value() 119 | ); 120 | return false; 121 | } 122 | port_.set_option(ba::serial_port_base::stop_bits( 123 | ba::serial_port_base::stop_bits::one), ec); 124 | if (ec.value() != 0) 125 | { 126 | RCLCPP_ERROR(node_->get_logger(), 127 | "Failed to set options. Error code: %d", 128 | ec.value() 129 | ); 130 | return false; 131 | } 132 | // FTDI USB-serial device has 16 msec latency in default. 133 | // It makes large latency and jitter for high rate measurement. 134 | const int fd = port_.native_handle(); 135 | serial_struct port_info; 136 | ioctl(fd, TIOCGSERIAL, &port_info); 137 | port_info.flags |= ASYNC_LOW_LATENCY; 138 | ioctl(fd, TIOCSSERIAL, &port_info); 139 | 140 | return true; 141 | } 142 | 143 | bool spin() 144 | { 145 | product_ = node_->get_parameter("product").as_int(); 146 | switch (product_) 147 | { 148 | case WitImu::PRODUCT::WT901C: 149 | { 150 | co_gravity_ = node_->get_parameter("gravity").as_double(); 151 | 152 | pub_imu_ = node_->create_publisher("data_raw", 10); 153 | pub_temp_ = node_->create_publisher("temperature", 10); 154 | pub_mag_ = node_->create_publisher("mag", 10); 155 | ptr_imu_ = std::make_shared(co_gravity_); 156 | 157 | std::function callback = \ 158 | std::bind(&WitImuDriver::cbSrvTrgCommand, 159 | this, 160 | TrgState::YAWCLR, 161 | std::placeholders::_1, 162 | std::placeholders::_2); 163 | srv_trg_yaw_clr_ = node_->create_service("trigger_yaw_clear", callback); 164 | 165 | callback = std::bind(&WitImuDriver::cbSrvTrgCommand, 166 | this, 167 | TrgState::ACCCAL, 168 | std::placeholders::_1, 169 | std::placeholders::_2); 170 | srv_trg_acc_cal_ = node_->create_service("trigger_acc_calibration", callback); 171 | 172 | callback = std::bind(&WitImuDriver::cbSrvTrgCommand, 173 | this, 174 | TrgState::MAGCAL, 175 | std::placeholders::_1, 176 | std::placeholders::_2); 177 | srv_trg_mag_cal_ = node_->create_service("trigger_mag_calibration", callback); 178 | 179 | callback = std::bind(&WitImuDriver::cbSrvTrgCommand, 180 | this, 181 | TrgState::EXICAL, 182 | std::placeholders::_1, 183 | std::placeholders::_2); 184 | srv_trg_exit_cal_ = node_->create_service("trigger_exit_calibration", callback); 185 | } 186 | break; 187 | 188 | default: 189 | RCLCPP_ERROR(node_->get_logger(), "Product is not provided"); 190 | return false; 191 | } 192 | startRead(); 193 | auto io_run = [this]() 194 | { 195 | boost::system::error_code ec; 196 | port_io_.run(ec); 197 | }; 198 | io_thread_ = std::thread(io_run); 199 | 200 | wdg_ = node_->create_wall_timer(wdg_timeout_.to_chrono(), std::bind(&WitImuDriver::cbWdg, this)); 201 | 202 | // wdg_ = pnh_.createTimer(wdg_timeout_, &WitImuDriver::cbWdg, this, true, false); 203 | 204 | rclcpp::spin(node_); 205 | close(); 206 | return true; 207 | } 208 | 209 | private: 210 | rclcpp::Node::SharedPtr node_; 211 | ba::io_service port_io_; 212 | ba::serial_port port_; 213 | rclcpp::Duration wdg_timeout_; 214 | rclcpp::TimerBase::SharedPtr wdg_; 215 | 216 | rclcpp::Publisher::SharedPtr pub_imu_; 217 | rclcpp::Publisher::SharedPtr pub_temp_; 218 | rclcpp::Publisher::SharedPtr pub_mag_; 219 | rclcpp::Service::SharedPtr srv_trg_yaw_clr_; 220 | rclcpp::Service::SharedPtr srv_trg_height_clr_; 221 | rclcpp::Service::SharedPtr srv_trg_acc_cal_; 222 | rclcpp::Service::SharedPtr srv_trg_mag_cal_; 223 | rclcpp::Service::SharedPtr srv_trg_exit_cal_; 224 | 225 | std::string frame_id_; 226 | std::thread io_thread_; 227 | 228 | double co_gravity_; 229 | int product_; 230 | std::vector rx_buf_; 231 | 232 | std::shared_ptr ptr_imu_; 233 | 234 | void close() 235 | { 236 | if (port_.is_open()) 237 | { 238 | port_.cancel(); 239 | port_.close(); 240 | } 241 | if (!port_io_.stopped()) 242 | { 243 | port_io_.stop(); 244 | } 245 | if (io_thread_.joinable()) 246 | { 247 | io_thread_.join(); 248 | } 249 | } 250 | 251 | void startRead() 252 | { 253 | ba::async_read(port_, 254 | ba::buffer(rx_buf_), 255 | ba::transfer_at_least(1), 256 | boost::bind(&WitImuDriver::cbPort, 257 | this, 258 | ba::placeholders::error, 259 | ba::placeholders::bytes_transferred)); 260 | } 261 | 262 | void cbPort(const boost::system::error_code& ec, 263 | std::size_t size) 264 | { 265 | if (!ec) 266 | { 267 | frame_id_ = node_->get_parameter("frame_id").as_string(); 268 | // ptr_imu_->pushBytes(rx_buf_, size, ros::Time::now()); 269 | ptr_imu_->pushBytes(rx_buf_, size, rclcpp::Clock().now()); 270 | while (ptr_imu_->sizeImuData() != 0) 271 | { 272 | imu_msg msg; 273 | ptr_imu_->popImuData(&msg); 274 | msg.header.frame_id = frame_id_; 275 | pub_imu_->publish(msg); 276 | } 277 | while (ptr_imu_->sizeTempData() != 0) 278 | { 279 | temp_msg msg; 280 | ptr_imu_->popTempData(&msg); 281 | msg.header.frame_id = frame_id_; 282 | pub_temp_->publish(msg); 283 | } 284 | while (ptr_imu_->sizeMagData() != 0) 285 | { 286 | mag_msg msg; 287 | ptr_imu_->popMagData(&msg); 288 | msg.header.frame_id = frame_id_; 289 | pub_mag_->publish(msg); 290 | } 291 | startRead(); 292 | resetWdg(); 293 | } 294 | else if (ec == boost::system::errc::operation_canceled) 295 | { 296 | // Enter to this state when stop a connection 297 | } 298 | else 299 | { 300 | // Unknown state 301 | RCLCPP_ERROR(node_->get_logger(), "seiral error : %s", ec.message().c_str()); 302 | rclcpp::shutdown(); 303 | } 304 | } 305 | 306 | void cbSrvTrgCommand(TrgState state, 307 | const std::shared_ptr req, 308 | const std::shared_ptr res) 309 | { 310 | (void) req; 311 | bool ret = false; 312 | switch(state){ 313 | case YAWCLR: 314 | ret = sendBytes(ptr_imu_->genYawClr()); 315 | break; 316 | case ACCCAL: 317 | ret = sendBytes(ptr_imu_->genAccCal()); 318 | break; 319 | case MAGCAL: 320 | ret = sendBytes(ptr_imu_->genMagCal()); 321 | break; 322 | case EXICAL: 323 | ret = sendBytes(ptr_imu_->genExitCal()); 324 | break; 325 | default: 326 | break; 327 | } 328 | if (ret) 329 | { 330 | res->message = "Success"; 331 | res->success = true; 332 | } 333 | else 334 | { 335 | res->message = "Failed"; 336 | res->success = false; 337 | } 338 | } 339 | 340 | // void cbSrvTrgWriteCommand(const std::shared_ptr req, 341 | // const std::shared_ptr res, 342 | // const std::vector& bytes) 343 | // { 344 | // (void) req; 345 | // bool ret = sendBytes(bytes); 346 | // if (ret) 347 | // { 348 | // res->message = "Success"; 349 | // res->success = true; 350 | // } 351 | // else 352 | // { 353 | // res->message = "Failed"; 354 | // res->success = false; 355 | // } 356 | // } 357 | 358 | void cbWdg() 359 | { 360 | if (port_.is_open()) 361 | { 362 | RCLCPP_ERROR(node_->get_logger(), "Timeouted. No data received from IMU."); 363 | rclcpp::shutdown(); 364 | } 365 | } 366 | 367 | void resetWdg() 368 | { 369 | wdg_->cancel(); 370 | wdg_->reset(); 371 | } 372 | 373 | bool sendBytes(const std::vector& bytes) 374 | { 375 | boost::system::error_code ec; 376 | const size_t w_len = ba::write(port_, 377 | ba::buffer(bytes), 378 | ec); 379 | if (w_len != bytes.size()) 380 | { 381 | RCLCPP_WARN(node_->get_logger(), "Could not send full length of packet."); 382 | return false; 383 | } 384 | else if (ec.value() != 0) 385 | { 386 | RCLCPP_WARN(node_->get_logger(), 387 | "Failed to write. Error code : %d", 388 | ec.value()); 389 | return false; 390 | } 391 | return true; 392 | } 393 | }; 394 | } // namespace wit_imu_driver 395 | 396 | int main(int argc, char** argv) 397 | { 398 | rclcpp::init(argc, argv); 399 | auto node = std::make_shared("wit_imu_driver"); 400 | // ros::init(argc, argv, "wit_imu_driver"); 401 | RCLCPP_INFO(node->get_logger(), "Start wit_imu_driver"); 402 | 403 | wit_imu_driver::WitImuDriver imu(node); 404 | if (!imu.open()) 405 | { 406 | RCLCPP_ERROR(rclcpp::get_logger("wit_imu_driver"), "Failed to open."); 407 | return 1; 408 | } 409 | 410 | if (!imu.spin()) 411 | { 412 | RCLCPP_ERROR(rclcpp::get_logger("wit_imu_driver"), "Exit by error"); 413 | return 1; 414 | } 415 | 416 | return 0; 417 | } 418 | -------------------------------------------------------------------------------- /src/wt901c.cpp: -------------------------------------------------------------------------------- 1 | #include "wit_imu_driver/wt901c.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "tf2/LinearMath/Quaternion.h" 7 | 8 | namespace wit_imu_driver 9 | { 10 | Wt901c::Wt901c(const double co_gravity) 11 | : WitImu(co_gravity) 12 | , co_acc_(co_gravity_ * -16.0 / 32768.0) 13 | , co_avel_(M_PI * 2000.0 / (32768.0 * 180.0)) 14 | , co_temp_(0.01) 15 | , co_mag_(1.0) 16 | , co_pose_(M_PI / 32768.0) 17 | { 18 | } 19 | 20 | void Wt901c::pushBytes(const std::vector& bytes, 21 | const size_t size, 22 | const rclcpp::Time& stamp) 23 | { 24 | buf_.insert(buf_.cend(), bytes.cbegin(), bytes.cbegin() + size); 25 | if (buf_.size() < 11) 26 | { 27 | // Not enough data length 28 | return; 29 | } 30 | while (buf_.size() >= 11) 31 | { 32 | if (buf_[0] != 0x55) 33 | { 34 | buf_.erase(buf_.cbegin()); 35 | continue; 36 | } 37 | uint8_t cs = std::accumulate(buf_.begin(), buf_.begin() + 10, 0); 38 | if (cs != buf_[10]) 39 | { 40 | std::printf("[Wt901c] Checksum is not match. Rx : %02X, Calc : %02X\n", buf_[10], cs); 41 | buf_.erase(buf_.cbegin()); 42 | continue; 43 | } 44 | switch (buf_[1]) 45 | { 46 | case 0x51: 47 | work_imu_.header.stamp = stamp; 48 | work_imu_.linear_acceleration.x = - co_acc_ * bytes2int(buf_[3], buf_[2]); 49 | work_imu_.linear_acceleration.y = - co_acc_ * bytes2int(buf_[5], buf_[4]); 50 | work_imu_.linear_acceleration.z = - co_acc_ * bytes2int(buf_[7], buf_[6]); 51 | 52 | work_temp_.header.stamp = stamp; 53 | work_temp_.temperature = co_temp_ * bytes2int(buf_[9], buf_[8]); 54 | break; 55 | 56 | case 0x52: 57 | work_imu_.angular_velocity.x = co_avel_ * bytes2int(buf_[3], buf_[2]); 58 | work_imu_.angular_velocity.y = co_avel_ * bytes2int(buf_[5], buf_[4]); 59 | work_imu_.angular_velocity.z = co_avel_ * bytes2int(buf_[7], buf_[6]); 60 | work_temp_.temperature += co_temp_ * bytes2int(buf_[9], buf_[8]); 61 | break; 62 | 63 | case 0x53: 64 | { 65 | double r, p, y; 66 | r = co_pose_ * bytes2int(buf_[3], buf_[2]); 67 | p = co_pose_ * bytes2int(buf_[5], buf_[4]); 68 | y = co_pose_ * bytes2int(buf_[7], buf_[6]); 69 | tf2::Quaternion q; 70 | q.setRPY(r, p, y); 71 | q.normalize(); 72 | work_imu_.orientation.x = q.x(); 73 | work_imu_.orientation.y = q.y(); 74 | work_imu_.orientation.z = q.z(); 75 | work_imu_.orientation.w = q.w(); 76 | imu_buf_.push(work_imu_); 77 | if (imu_buf_.size() >= msg_buf_max_) 78 | { 79 | imu_buf_.pop(); 80 | } 81 | } 82 | { 83 | work_temp_.temperature += co_temp_ * bytes2int(buf_[9], buf_[8]); 84 | work_temp_.temperature /= 3.0; 85 | temp_buf_.push(work_temp_); 86 | if (temp_buf_.size() >= msg_buf_max_) 87 | { 88 | temp_buf_.pop(); 89 | } 90 | } 91 | break; 92 | 93 | case 0x54: 94 | work_mag_.header.stamp = stamp; 95 | work_mag_.magnetic_field.x = co_mag_ * bytes2int(buf_[3], buf_[2]); 96 | work_mag_.magnetic_field.y = co_mag_ * bytes2int(buf_[5], buf_[4]); 97 | work_mag_.magnetic_field.z = co_mag_ * bytes2int(buf_[7], buf_[6]); 98 | mag_buf_.push(work_mag_); 99 | if (mag_buf_.size() >= msg_buf_max_) 100 | { 101 | mag_buf_.pop(); 102 | } 103 | break; 104 | } 105 | buf_.erase(buf_.cbegin(), buf_.cbegin() + 11); 106 | } 107 | return; 108 | } 109 | 110 | std::vector Wt901c::genYawClr() const 111 | { 112 | return std::vector{0xFF, 0xAA, 0x01, 0x04, 0x00}; 113 | } 114 | 115 | std::vector Wt901c::genHightClr() const 116 | { 117 | return std::vector{0xFF, 0xAA, 0x01, 0x03, 0x00}; 118 | } 119 | 120 | std::vector Wt901c::genAccCal() const 121 | { 122 | return std::vector{0xFF, 0xAA, 0x01, 0x01, 0x00}; 123 | } 124 | 125 | std::vector Wt901c::genMagCal() const 126 | { 127 | return std::vector{0xFF, 0xAA, 0x01, 0x02, 0x00}; 128 | } 129 | 130 | std::vector Wt901c::genExitCal() const 131 | { 132 | return std::vector{0xFF, 0xAA, 0x01, 0x00, 0x00}; 133 | } 134 | 135 | } // namespace wit_imu_driver 136 | --------------------------------------------------------------------------------