├── interfaces
    ├── srv
    │   └── ImuReset.srv
    ├── CMakeLists.txt
    └── package.xml
├── iahrs_driver
    ├── launch
    │   ├── iahrs_driver_launch.xml
    │   └── iahrs_driver.py
    ├── package.xml
    ├── CMakeLists.txt
    └── src
    │   └── iahrs_driver.cpp
└── README.md


/interfaces/srv/ImuReset.srv:
--------------------------------------------------------------------------------
1 | #This message is used to send command to IAHRS(IMU)
2 | #Euler_angle Reset command
3 | 
4 | ---
5 | bool result
6 | 


--------------------------------------------------------------------------------
/iahrs_driver/launch/iahrs_driver_launch.xml:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <node pkg="iahrs_driver" exec="iahrs_driver" name="iahrs_driver" output="screen">
3 |     <param name="m_bSingle_TF_option" value="true"/>
4 |    </node>
5 |     
6 | </launch>
7 | 


--------------------------------------------------------------------------------
/interfaces/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | 
 3 | project(interfaces)
 4 | 
 5 | # Default to C++14
 6 | if(NOT CMAKE_CXX_STANDARD)
 7 |   set(CMAKE_CXX_STANDARD 14)
 8 | endif()
 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10 |   add_compile_options(-Wall -Wextra -Wpedantic)
11 | endif()
12 | 
13 | find_package(ament_cmake REQUIRED)
14 | find_package(builtin_interfaces REQUIRED)
15 | find_package(std_msgs REQUIRED)
16 | find_package(std_srvs REQUIRED)
17 | find_package(geometry_msgs REQUIRED)
18 | find_package(rosidl_default_generators REQUIRED)
19 | 
20 | rosidl_generate_interfaces(${PROJECT_NAME}
21 |   "srv/ImuReset.srv"
22 |   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs std_srvs
23 | )
24 | 
25 | 
26 | ament_export_dependencies(rosidl_default_runtime)
27 | ament_package()
28 | 


--------------------------------------------------------------------------------
/interfaces/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>interfaces</name>
 5 |   <version>0.0.1</version>
 6 |   <description>ROS2 Interface</description>
 7 |   <maintainer email="wbjin@hyulimrobot.com">wbjin</maintainer>
 8 |   <license>Apache License 2.0</license>
 9 | 
10 | 
11 |   <buildtool_depend>ament_cmake</buildtool_depend>
12 | 
13 |   <depend>rclcpp</depend>
14 |   <depend>std_msgs</depend>
15 |   <depend>std_srvs</depend>
16 |   <depend>builtin_interfaces</depend>
17 |   <depend>rosidl_default_generators</depend>
18 |   <depend>geometry_msgs</depend>
19 | 
20 |   <member_of_group>rosidl_interface_packages</member_of_group>
21 | 
22 |   <export>
23 |     <build_type>ament_cmake</build_type>
24 |   </export>
25 | </package>
26 | 


--------------------------------------------------------------------------------
/iahrs_driver/launch/iahrs_driver.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | import os
 4 | 
 5 | from launch import LaunchDescription
 6 | from launch.actions import ExecuteProcess, IncludeLaunchDescription
 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource
 8 | from launch.substitutions import LaunchConfiguration
 9 | 
10 | from launch_ros.actions import Node
11 | 
12 | # this is the function launch  system will look for
13 | def generate_launch_description():
14 | 
15 |     iahrs_driver_node = Node(
16 |         package='iahrs_driver', 
17 |         executable='iahrs_driver',
18 |         output='screen',
19 |         parameters=[
20 |             {"m_bSingle_TF_option": True}
21 |         ]
22 |     )
23 | 
24 |     # create and return launch description object
25 |     return LaunchDescription(
26 |         [
27 |             iahrs_driver_node
28 |         ]
29 |     )
30 | 


--------------------------------------------------------------------------------
/iahrs_driver/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>iahrs_driver</name>
 5 |   <version>0.0.1</version>
 6 |   <description>iahrs_driver ROS2 package</description>
 7 |   <maintainer email="wbjin@hyulimrobot.com">wbjin</maintainer>
 8 |   <license>Apache License 2.0</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 |   
12 |   <build_depend>rclcpp</build_depend>
13 |   <build_depend>rmw_implementation</build_depend>
14 |   <build_depend>std_msgs</build_depend>
15 |   <build_depend>std_srvs</build_depend>
16 |   <build_depend>sensor_msgs</build_depend>
17 |   <build_depend>tf2</build_depend>
18 |   <build_depend>tf2_ros</build_depend>
19 |   <build_depend>geometry_msgs</build_depend>
20 |   <build_depend>interfaces</build_depend>
21 |   
22 |   <exec_depend>rclcpp</exec_depend>
23 |   <exec_depend>rmw_implementation</exec_depend>
24 |   <exec_depend>std_msgs</exec_depend>
25 |   <exec_depend>std_srvs</exec_depend>
26 |   <exec_depend>sensor_msgs</exec_depend>
27 |   <exec_depend>tf2</exec_depend>
28 |   <exec_depend>tf2_ros</exec_depend>
29 |   <exec_depend>geometry_msgs</exec_depend>
30 |   <exec_depend>interfaces</exec_depend>
31 | 
32 |   <exec_depend>ros2launch</exec_depend>
33 | 
34 |   <test_depend>ament_cmake_gtest</test_depend>
35 |   <test_depend>ament_lint_auto</test_depend>
36 |   <test_depend>ament_lint_common</test_depend>
37 |   <test_depend>rosidl_cmake</test_depend>
38 | 
39 |   <export>
40 |     <build_type>ament_cmake</build_type>
41 |   </export>
42 | </package>
43 | 


--------------------------------------------------------------------------------
/iahrs_driver/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | project(iahrs_driver)
 3 | 
 4 | # Default to C99
 5 | if(NOT CMAKE_C_STANDARD)
 6 |   set(CMAKE_C_STANDARD 99)
 7 | endif()
 8 | 
 9 | # Default to C++14
10 | if(NOT CMAKE_CXX_STANDARD)
11 |   set(CMAKE_CXX_STANDARD 14)
12 | endif()
13 | 
14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
15 |   add_compile_options(-Wall -Wextra -Wpedantic)
16 | endif()
17 | 
18 | # find dependencies
19 | find_package(ament_cmake REQUIRED)
20 | find_package(rclcpp REQUIRED)
21 | find_package(rmw_implementation REQUIRED)
22 | find_package(std_srvs REQUIRED)
23 | find_package(sensor_msgs REQUIRED)
24 | find_package(tf2 REQUIRED)
25 | find_package(tf2_ros REQUIRED)
26 | find_package(tf2_geometry_msgs REQUIRED)
27 | find_package(geometry_msgs REQUIRED)
28 | find_package(visualization_msgs REQUIRED)
29 | find_package(interfaces REQUIRED) #interfaces package
30 | 
31 | 
32 | add_executable(${PROJECT_NAME} src/iahrs_driver.cpp)
33 | 
34 | set(dependencies
35 |   geometry_msgs
36 |   rclcpp
37 |   sensor_msgs
38 |   std_msgs
39 |   tf2
40 |   tf2_geometry_msgs
41 |   tf2_ros
42 |   geometry_msgs
43 |   visualization_msgs
44 |   interfaces
45 | )
46 | 
47 | ament_target_dependencies(${PROJECT_NAME}
48 |   ${dependencies}
49 | )
50 | 
51 | install(TARGETS 
52 |   iahrs_driver
53 | 	DESTINATION lib/${PROJECT_NAME}
54 | )
55 | 
56 | install(
57 |   DIRECTORY launch
58 |   DESTINATION share/${PROJECT_NAME}
59 | )
60 | 
61 |   	
62 | if(BUILD_TESTING)
63 |     find_package(ament_lint_auto REQUIRED)
64 |     ament_lint_auto_find_test_dependencies()
65 | endif()
66 | 
67 | ament_export_dependencies(${dependencies})
68 | 
69 | ament_package()
70 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # iahrs_driver_ros2
 2 | ros2(foxy / humble / jazzy ) iahrs cpp package
 3 | 
 4 | ===[Device Setting]====================================
 5 | 
 6 | -iAHRS는 USB로 연결되는데 상황에 따라서 인식되는 ttyUSB 포트의 번호가 바뀔 수 있음으로 장치 이름을 고정해서 사용할게요.
 7 | 
 8 | ttyUSB권한 설정.
 9 | USB Port에 대한 권한은 ‘sudo chmod 777 ttyUSB0’ 명령어를 통해서 줄 수 있지만, 매번 설정이 번거로우므로 dialout그룹에 추가하는 방법을 이용.
10 | 
11 |  sudo usermod -a -G dialout $USER
12 | 
13 | ttyUSB rule의 설정을 위한 심볼릭 링크 ttyUSBx만들기
14 | 심볼릭 링크를 만드는데 필요한 정보는 아래 3가지.
15 |  Vender ID
16 | 
17 |  Product ID
18 | 
19 |  Serial Number
20 | 
21 | 위의 3가지 정보는 아래 2개의 명령어로 찾을 수 있어요.
22 |  $ lsusb
23 | 
24 |  $ udevadm info -a /dev/ttyUSB0 | grep '{serial}' <-- 저는 처음에 iAHRS가 ttyUSB0으로 잡혀서 이렇게 했어요. 만약 번호가 다르면 그 번호를 입력하세요.
25 | 
26 | ![id](https://user-images.githubusercontent.com/58063370/153543765-2284bc56-23ce-4a3d-b261-c855f3ec5089.PNG)
27 | 
28 | 위에 출력된 정보가 USB to Serial Device의 Serial Number이며, 해당 번호는 제품별로 다를 수도 있습니다.
29 | 
30 | 알아낸 정보를 이용한 .rules file생성.
31 | ‘IMU.rules’ 파일을 ‘/etc/udev/rules.d’ 경로에 생성한 후에 해당 파일의 내용을 아래와 같이 작성한 후 저장.
32 | 
33 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DM03L0C6", MODE:="0666", GROUP:="dialout", SYMLINK+="IMU"
34 | 
35 | udev 재시작 명령어를 호출한 후에 PC의 재 시작을 해주어야 적용 됩니다.
36 |  $ sudo service udev restart
37 | 
38 | ![rules](https://user-images.githubusercontent.com/58063370/153543247-8c446c45-bcab-4ec5-ac96-2550942f5915.PNG)
39 | 
40 | 심볼릭 등록의 확인. (아래와 같은 명령어를 호출하면 위의 그림처럼 ttyUSB가 IMU로 고정된 것을 볼 수 있어요 .)
41 |  $ ll /dev/
42 | 
43 | ==================================================================================
44 | 
45 | 1) interfaces package와 iahrs_driver package로 구성되어 있음.
46 | 2) interfaces package 먼저 빌드 후에 iahrs_driver package 빌드 후 사용
47 | 3) 서비스는 오일러 각도 초기화 기능만 구현함.
48 | 4) 설명자료 링크: https://blog.naver.com/zzang0736/223204708311
49 | 
50 | ![Image](https://github.com/user-attachments/assets/f2efffa3-83f1-4d46-9404-13cc0d9d0ad1)
51 |    
52 | 


--------------------------------------------------------------------------------
/iahrs_driver/src/iahrs_driver.cpp:
--------------------------------------------------------------------------------
  1 | #include <chrono>
  2 | #include <memory>
  3 | #include <math.h>
  4 | #include <stdio.h>
  5 | #include <stdlib.h>
  6 | #include <string.h>
  7 | #include <termios.h>
  8 | #include <unistd.h>
  9 | #include <pthread.h>
 10 | #include <sys/fcntl.h>
 11 | #include <time.h>
 12 | #include <sys/types.h>
 13 | #include <vector>
 14 | #include <iostream>
 15 | #include <dirent.h>
 16 | #include <signal.h>
 17 | #include <atomic>
 18 | 
 19 | #include "rclcpp/rclcpp.hpp"
 20 | #include "std_msgs/msg/float64.hpp"
 21 | #include "sensor_msgs/msg/imu.hpp"
 22 | #include "sensor_msgs/msg/magnetic_field.hpp"
 23 | #include "geometry_msgs/msg/transform_stamped.hpp"
 24 | #include "tf2_ros/transform_broadcaster.h"
 25 | #include "tf2/LinearMath/Quaternion.h"
 26 | 
 27 | //interface package srv include...
 28 | #include "interfaces/srv/imu_reset.hpp"
 29 | 
 30 | 
 31 | #define SERIAL_PORT	"/dev/IMU"
 32 | #define SERIAL_SPEED	B115200
 33 | 
 34 | typedef struct IMU_DATA
 35 | {
 36 | 	double dQuaternion_x = 0.0;
 37 | 	double dQuaternion_y = 0.0;
 38 | 	double dQuaternion_z = 0.0;
 39 | 	double dQuaternion_w = 1.0;
 40 | 
 41 | 	double dAngular_velocity_x = 0.0;
 42 | 	double dAngular_velocity_y = 0.0;
 43 | 	double dAngular_velocity_z = 0.0;
 44 | 	
 45 | 	double dLinear_acceleration_x = 0.0;
 46 | 	double dLinear_acceleration_y = 0.0;
 47 | 	double dLinear_acceleration_z = 0.0;
 48 |     
 49 | 	double dEuler_angle_Roll = 0.0;
 50 | 	double dEuler_angle_Pitch = 0.0;
 51 | 	double dEuler_angle_Yaw = 0.0;
 52 | 
 53 | }IMU_DATA;
 54 | IMU_DATA _pIMU_data;
 55 | 
 56 | int serial_fd = -1;
 57 | double time_offset_in_seconds;
 58 | double dSend_Data[10];
 59 | double m_dRoll, m_dPitch, m_dYaw;
 60 | //single_used TF
 61 | bool m_bSingle_TF_option = true; //false;
 62 | using namespace std::chrono_literals;
 63 | 
 64 | std::atomic<bool> stop_requested(false);
 65 | void signal_handler(int signal) 
 66 | {
 67 |     stop_requested = true;
 68 |     printf("Signal received: %d. Preparing to shutdown...\n", signal);
 69 | }
 70 | 
 71 | //iahrs_driver class define
 72 | class IAHRS : public rclcpp::Node
 73 | {
 74 | public:
 75 | 	IAHRS() : Node("iahrs_driver")
 76 | 	{
 77 | 		tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 78 | 		imu_data_pub = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", rclcpp::SensorDataQoS());
 79 | 
 80 | 		//Service ListUp
 81 | 		euler_angle_reset_srv_ = create_service<interfaces::srv::ImuReset>(
 82 |         	"all_data_reset", 
 83 | 		std::bind(&IAHRS::Euler_angle_reset_callback, this, std::placeholders::_1, std::placeholders::_2));
 84 | 
 85 | 		//PARAM
 86 | 		this->declare_parameter("m_bSingle_TF_option", rclcpp::PARAMETER_BOOL);
 87 | 		m_bSingle_TF_option_param = this->get_parameter("m_bSingle_TF_option");
 88 | 		//Get Param
 89 | 		m_bSingle_TF_option = m_bSingle_TF_option_param.as_bool();
 90 | 
 91 | 	}
 92 | 
 93 | 	////value//////////////////////////////////////////////////////////////////////////////
 94 | 	std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
 95 | 	geometry_msgs::msg::TransformStamped transformStamped;
 96 | 	sensor_msgs::msg::Imu imu_data_msg;
 97 | 	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_data_pub;
 98 | 	rclcpp::Parameter m_bSingle_TF_option_param;
 99 | 
100 | 	////function//////////////////////////////////////////////////////////////////////////////
101 | 	int serial_open ()
102 | 	{
103 | 		printf ("Try to open serial: %s\n", SERIAL_PORT); 
104 | 
105 | 		serial_fd = open(SERIAL_PORT, O_RDWR|O_NOCTTY);
106 | 		if (serial_fd < 0) 
107 | 		{
108 | 			printf ("Error unable to open %s\n", SERIAL_PORT);
109 | 			return -1;
110 | 		}
111 | 		printf ("%s open success\n", SERIAL_PORT);
112 | 
113 | 		struct termios tio;
114 | 		tcgetattr(serial_fd, &tio);
115 | 		cfmakeraw(&tio);
116 | 		tio.c_cflag = CS8|CLOCAL|CREAD;
117 | 		tio.c_iflag &= ~(IXON | IXOFF);
118 | 		cfsetspeed(&tio, SERIAL_SPEED);
119 | 		tio.c_cc[VTIME] = 0;
120 | 		tio.c_cc[VMIN] = 0;
121 | 
122 | 		int err = tcsetattr(serial_fd, TCSAFLUSH, &tio);
123 | 		if (err != 0) 
124 | 		{
125 | 			printf ("Error tcsetattr() function return error\n");
126 | 			close(serial_fd);
127 | 			serial_fd = -1;
128 | 			return -1;
129 | 		}
130 | 		return 0;
131 | 	}
132 | 
133 | 	static unsigned long GetTickCount() 
134 | 	{
135 | 		struct timespec ts;
136 | 	
137 | 		clock_gettime (CLOCK_MONOTONIC, &ts);
138 | 
139 | 		return ts.tv_sec*1000 + ts.tv_nsec/1000000;
140 | 	}
141 | 
142 | 	int SendRecv(const char* command, double* returned_data, int data_length)
143 | 	{
144 | 		#define COMM_RECV_TIMEOUT	30	
145 | 
146 | 		char temp_buff[256];
147 | 		read (serial_fd, temp_buff, 256);
148 | 
149 | 		int command_len = strlen(command);
150 | 		int n = write(serial_fd, command, command_len);
151 | 
152 | 		if (n < 0) return -1;
153 | 
154 | 		const int buff_size = 1024;
155 | 		int  recv_len = 0;
156 | 		char recv_buff[buff_size + 1];
157 | 
158 | 		unsigned long time_start = GetTickCount();
159 | 
160 | 		while (recv_len < buff_size) 
161 | 		{
162 | 			int n = read (serial_fd, recv_buff + recv_len, buff_size - recv_len);
163 | 			if (n < 0) 
164 | 			{
165 | 				return -1;
166 | 			}
167 | 			else if (n == 0) 
168 | 			{
169 | 				usleep(1000);
170 | 			}
171 | 			else if (n > 0) 
172 | 			{
173 | 				recv_len += n;
174 | 
175 | 				if (recv_buff[recv_len - 1] == '\r' || recv_buff[recv_len - 1] == '\n') 
176 | 				{
177 | 					break;
178 | 				}
179 | 			}
180 | 
181 | 			unsigned long time_current = GetTickCount();
182 | 			unsigned long time_delta = time_current - time_start;
183 | 
184 | 			if (time_delta >= COMM_RECV_TIMEOUT) break;
185 | 		}
186 | 		recv_buff[recv_len] = '\0';
187 | 
188 | 		if (recv_len > 0) 
189 | 		{
190 | 			if (recv_buff[0] == '!') 
191 | 			{
192 | 				return -1;
193 | 			}
194 | 		}
195 | 
196 | 		if (strncmp(command, recv_buff, command_len - 1) == 0) 
197 | 		{
198 | 			if (recv_buff[command_len - 1] == '=') {
199 | 				int data_count = 0;
200 | 
201 | 				char* p = &recv_buff[command_len];
202 | 				char* pp = NULL;
203 | 
204 | 				for (int i = 0; i < data_length; i++) 
205 | 				{
206 | 					if (p[0] == '0' && p[1] == 'x') 
207 | 					{
208 | 						returned_data[i] = strtol(p+2, &pp, 16);
209 | 						data_count++;
210 | 					}
211 | 					else 
212 | 					{
213 | 						returned_data[i] = strtod(p, &pp);
214 | 						data_count++;
215 | 					}
216 | 
217 | 					if (*pp == ',') 
218 | 					{
219 | 						p = pp + 1;
220 | 					}
221 | 					else 
222 | 					{
223 | 						break;
224 | 					}
225 | 				}
226 | 				return data_count;
227 | 			}
228 | 		}
229 | 		return 0;
230 | 	}
231 | 
232 | private:
233 | 
234 | 	rclcpp::Service<interfaces::srv::ImuReset>::SharedPtr euler_angle_reset_srv_;
235 | 
236 | 	bool Euler_angle_reset_callback(
237 | 		const std::shared_ptr<interfaces::srv::ImuReset::Request> request, 
238 | 		const std::shared_ptr<interfaces::srv::ImuReset::Response> response)
239 | 	{
240 | 		bool bResult = false;
241 | 		double dSend_Data[10];
242 | 		SendRecv("ra\n", dSend_Data, 10);
243 | 		bResult = true;
244 | 		response->result = bResult;
245 | 		return true;
246 | 	}
247 | 
248 | 
249 | };
250 | 
251 | 
252 | int main (int argc, char** argv)
253 | {
254 | 	rclcpp::init(argc, argv);
255 | 	std::signal(SIGINT, signal_handler);
256 | 	// Create a function for when messages are to be sent.
257 | 	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
258 | 	auto node = std::make_shared<IAHRS>();
259 | 
260 | 	// These values do not need to be converted
261 | 	node->imu_data_msg.linear_acceleration_covariance[0] = 0.0064;
262 | 	node->imu_data_msg.linear_acceleration_covariance[4] = 0.0063;
263 | 	node->imu_data_msg.linear_acceleration_covariance[8] = 0.0064;
264 | 	node->imu_data_msg.angular_velocity_covariance[0] = 0.032*(M_PI/180.0);
265 | 	node->imu_data_msg.angular_velocity_covariance[4] = 0.028*(M_PI/180.0);
266 | 	node->imu_data_msg.angular_velocity_covariance[8] = 0.006*(M_PI/180.0);
267 | 	node->imu_data_msg.orientation_covariance[0] = 0.013*(M_PI/180.0);
268 | 	node->imu_data_msg.orientation_covariance[4] = 0.011*(M_PI/180.0);
269 | 	node->imu_data_msg.orientation_covariance[8] = 0.006*(M_PI/180.0);
270 | 
271 | 
272 | 	rclcpp::WallRate loop_rate(100);
273 | 	node->serial_open();
274 | 	node->SendRecv("za\n", dSend_Data, 10);	// Euler Angle -> '0.0' Reset
275 | 	usleep(10000);
276 | 	printf("                       | Z axis \n");
277 | 	printf("                       | \n");
278 | 	printf("                       |   / X axis \n");
279 | 	printf("                   ____|__/____ \n");
280 | 	printf("      Y axis     / *   | /    /| \n");
281 | 	printf("      _________ /______|/    // \n");
282 | 	printf("               /___________ // \n");
283 | 	printf("              |____iahrs___|/ \n");
284 | 
285 | 	while(rclcpp::ok() && !stop_requested)
286 |     	{
287 | 		rclcpp::spin_some(node);
288 | 		if (serial_fd >= 0) 
289 | 		{
290 | 			const int max_data = 10;
291 | 			double data[max_data];
292 | 			int no_data = 0;
293 | 			no_data = node->SendRecv("g\n", data, max_data);	// Read angular_velocity _ wx, wy, wz 
294 | 			if (no_data >= 3) 
295 | 			{
296 | 				// angular_velocity
297 | 				node->imu_data_msg.angular_velocity.x = _pIMU_data.dAngular_velocity_x = data[0]*(M_PI/180.0);
298 | 				node->imu_data_msg.angular_velocity.y = _pIMU_data.dAngular_velocity_y = data[1]*(M_PI/180.0);
299 | 				node->imu_data_msg.angular_velocity.z = _pIMU_data.dAngular_velocity_z = data[2]*(M_PI/180.0);
300 | 			}
301 | 			no_data = node->SendRecv("a\n", data, max_data);	// Read linear_acceleration 	unit: m/s^2
302 | 			if (no_data >= 3) 
303 | 			{
304 | 				//// linear_acceleration   g to m/s^2
305 | 				node->imu_data_msg.linear_acceleration.x = _pIMU_data.dLinear_acceleration_x = data[0] * 9.80665;
306 | 				node->imu_data_msg.linear_acceleration.y = _pIMU_data.dLinear_acceleration_y = data[1] * 9.80665;
307 | 				node->imu_data_msg.linear_acceleration.z = _pIMU_data.dLinear_acceleration_z = data[2] * 9.80665;
308 | 			}
309 | 			no_data = node->SendRecv("e\n", data, max_data);	// Read Euler angle
310 | 			if (no_data >= 3) 
311 | 			{
312 | 				// Euler _ rad
313 | 				_pIMU_data.dEuler_angle_Roll  = data[0]*(M_PI/180.0);
314 | 				_pIMU_data.dEuler_angle_Pitch = data[1]*(M_PI/180.0);
315 | 				_pIMU_data.dEuler_angle_Yaw	  = data[2]*(M_PI/180.0);
316 | 			}
317 | 
318 | 			tf2::Quaternion q;
319 | 			q.setRPY(_pIMU_data.dEuler_angle_Roll , _pIMU_data.dEuler_angle_Pitch, _pIMU_data.dEuler_angle_Yaw);
320 | 			// orientation
321 | 			node->imu_data_msg.orientation.x = q.x();
322 | 			node->imu_data_msg.orientation.y = q.y();
323 | 			node->imu_data_msg.orientation.z = q.z();
324 | 			node->imu_data_msg.orientation.w = q.w();
325 | 			node->imu_data_msg.header.stamp = node->now();
326 | 			node->imu_data_msg.header.frame_id = "imu_link";
327 | 
328 | 			// publish the IMU data
329 | 			node->imu_data_pub->publish(node->imu_data_msg);
330 | 
331 | 			if(m_bSingle_TF_option)
332 | 			{
333 | 				// Update the timestamp of the transform
334 | 				node->transformStamped.header.stamp = node->now();
335 | 				node->transformStamped.header.frame_id = "base_link";   // Parent frame ID
336 | 				node->transformStamped.child_frame_id = "imu_link";       // IMU frame ID
337 | 				// Set the transformation translation (position)
338 | 				node->transformStamped.transform.translation.x = 0.0;
339 | 				node->transformStamped.transform.translation.y = 0.0;
340 | 				node->transformStamped.transform.translation.z = 0.2;
341 | 				node->transformStamped.transform.rotation.x = q.x();
342 | 				node->transformStamped.transform.rotation.y = q.y();
343 | 				node->transformStamped.transform.rotation.z = q.z();
344 | 				node->transformStamped.transform.rotation.w = q.w();
345 | 				// Publish the transform
346 | 				node->tf_broadcaster->sendTransform(node->transformStamped);
347 | 			}
348 | 		}
349 | 
350 |         loop_rate.sleep();
351 |     }
352 | 
353 | 	close (serial_fd);
354 | 	rclcpp::shutdown();
355 | 
356 |     return 0;
357 | }
358 | 


--------------------------------------------------------------------------------