├── .catkin_workspace ├── CMakeLists.txt ├── README.md ├── launch └── iahrs_driver.launch ├── package.xml ├── src └── iahrs_driver.cpp └── srv ├── all_data_reset.srv ├── euler_angle_init.srv ├── euler_angle_reset.srv ├── pose_velocity_reset.srv └── reboot_sensor.srv /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(iahrs_driver) 3 | 4 | ################################### 5 | # Packages 6 | ################################### 7 | find_package(catkin REQUIRED 8 | roscpp 9 | std_msgs 10 | std_srvs 11 | sensor_msgs 12 | tf 13 | geometry_msgs 14 | ) 15 | 16 | ## add Service.... 17 | add_service_files( 18 | FILES 19 | all_data_reset.srv 20 | euler_angle_init.srv 21 | euler_angle_reset.srv 22 | pose_velocity_reset.srv 23 | reboot_sensor.srv 24 | ) 25 | 26 | generate_messages( 27 | DEPENDENCIES 28 | std_msgs 29 | std_srvs 30 | geometry_msgs 31 | ) 32 | 33 | ################################### 34 | ## catkin specific configuration ## 35 | ################################### 36 | catkin_package( 37 | LIBRARIES iahrs_driver 38 | CATKIN_DEPENDS roscpp std_msgs std_srvs sensor_msgs tf 39 | DEPENDS system_lib 40 | ) 41 | 42 | ########### 43 | ## Build ## 44 | ########### 45 | include_directories( 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | #add_compile_options(-std=c++11) 50 | add_executable(iahrs_driver src/iahrs_driver.cpp) 51 | add_dependencies(iahrs_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 52 | target_link_libraries(iahrs_driver ${catkin_LIBRARIES}) 53 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # iAHRS ROS Package 2 | 3 | ===[Device Setting]==================================== 4 | 5 | -iAHRS는 USB로 연결되는데 상황에 따라서 인식되는 ttyUSB 포트의 번호가 바뀔 수 있음으로 장치 이름을 고정해서 사용할게요. 6 | 7 | 1) ttyUSB권한 설정. 8 | 9 | USB Port에 대한 권한은 ‘sudo chmod 777 ttyUSB0’ 명령어를 통해서 줄 수 있지만, 매번 설정이 번거로우므로 dialout그룹에 추가하는 방법을 이용. 10 | 11 |  sudo usermod -a -G dialout $USER 12 | 13 | 14 | 15 | 2) ttyUSB rule의 설정을 위한 심볼릭 링크 ttyUSBx만들기 16 | 17 | - 심볼릭 링크를 만드는데 필요한 정보는 아래 3가지. 18 | 19 |  Vender ID 20 | 21 |  Product ID 22 | 23 |  Serial Number 24 | 25 | - 위의 3가지 정보는 아래 2개의 명령어로 찾을 수 있어요. 26 | 27 |  $ lsusb 28 | 29 |  $ udevadm info -a /dev/ttyUSB0 | grep '{serial}' <-- 저는 처음에 iAHRS가 ttyUSB0으로 잡혀서 이렇게 했어요. 만약 번호가 다르면 그 번호를 입력하세요. 30 | 31 | ![id](https://user-images.githubusercontent.com/58063370/153543765-2284bc56-23ce-4a3d-b261-c855f3ec5089.PNG) 32 | 33 | 위에 출력된 정보가 USB to Serial Device의 Serial Number이며, 해당 번호는 제품별로 다를 수도 있습니다. 34 | 35 | 36 | - 알아낸 정보를 이용한 .rules file생성. 37 | 38 | ‘IMU.rules’ 파일을 ‘/etc/udev/rules.d’ 경로에 생성한 후에 해당 파일의 내용을 아래와 같이 작성한 후 저장. 39 | 40 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DM03L0C6", MODE:="0666", GROUP:="dialout", SYMLINK+="IMU" 41 | 42 | - udev 재시작 명령어를 호출한 후에 PC의 재 시작을 해주어야 적용 됩니다. 43 | 44 |  $ sudo service udev restart 45 | 46 | 47 | ![rules](https://user-images.githubusercontent.com/58063370/153543247-8c446c45-bcab-4ec5-ac96-2550942f5915.PNG) 48 | - 심볼릭 등록의 확인. (아래와 같은 명령어를 호출하면 위의 그림처럼 ttyUSB가 IMU로 고정된 것을 볼 수 있어요 .) 49 | 50 |  $ ll /dev/ 51 | 52 | 53 | ===[사용하기]==================================== 54 | 55 | 1) roslaunch 하기 56 | 57 |  $ roslaunch iahrs_driver iahrs_driver.launch 58 | 59 | 2) imu topic확인하기 60 | 61 |  $ rostopic echo /imu/data 62 | ![222](https://user-images.githubusercontent.com/58063370/153544313-3acc1524-badf-4e7c-af9f-bcf0700ac4e6.PNG) 63 | 64 | 3) rviz를 사용하여 확인하기 65 | 66 |  $ rviz 67 | 68 | - rviz화면을 아래와 같이 설정 69 | 70 | ![rviz_setting](https://user-images.githubusercontent.com/58063370/153546094-76292dd3-dbd8-4e89-9d9c-5242887af163.PNG) 71 | 72 | - 동작에 대한 영상은 바로 아래~~~ 73 | 74 | ![ezgif com-crop](https://user-images.githubusercontent.com/58063370/153545614-95801ac6-b86e-4bd2-b46b-3aff10e58a9d.gif) 75 | 76 | 77 | 4) 사용 가능한 서비스 (Input: 없음) 78 | - all_data_reset_cmd : 모든 보정 데이터 초기화(c=9) 79 | 80 | - euler_angle_init_cmd: 현재 Euler angle을 0위치로 설정(c=5) 81 | 82 | - euler_angle_reset_cmd: Euler angle초기화 (c=7) 83 | 84 | - pose_velocity_reset_cmd: 위치와 속도 초기화 (c=8) 85 | 86 | - reboot_sensor_cmd: 센서 재시작(c=99) 87 | 88 | -------------------------------------------------------------------------------- /launch/iahrs_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | iahrs_driver 4 | 0.0.1 5 | The iAHRS_Driver package 6 | wbjin 7 | BSD 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | std_srvs 13 | sensor_msgs 14 | tf 15 | geometry_msgs 16 | 17 | roscpp 18 | std_msgs 19 | std_srvs 20 | sensor_msgs 21 | tf 22 | geometry_msgs 23 | 24 | -------------------------------------------------------------------------------- /src/iahrs_driver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | //Service_include... 11 | #include "iahrs_driver/all_data_reset.h" 12 | #include "iahrs_driver/euler_angle_init.h" 13 | #include "iahrs_driver/euler_angle_reset.h" 14 | #include "iahrs_driver/pose_velocity_reset.h" 15 | #include "iahrs_driver/reboot_sensor.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #define SERIAL_PORT "/dev/IMU" 33 | #define SERIAL_SPEED B115200 34 | 35 | typedef struct IMU_DATA 36 | { 37 | double dQuaternion_x = 0.0; 38 | double dQuaternion_y = 0.0; 39 | double dQuaternion_z = 0.0; 40 | double dQuaternion_w = 1.0; 41 | 42 | double dAngular_velocity_x = 0.0; 43 | double dAngular_velocity_y = 0.0; 44 | double dAngular_velocity_z = 0.0; 45 | 46 | double dLinear_acceleration_x = 0.0; 47 | double dLinear_acceleration_y = 0.0; 48 | double dLinear_acceleration_z = 0.0; 49 | 50 | double dEuler_angle_Roll = 0.0; 51 | double dEuler_angle_Pitch = 0.0; 52 | double dEuler_angle_Yaw = 0.0; 53 | 54 | }IMU_DATA; 55 | IMU_DATA _pIMU_data; 56 | 57 | int serial_fd = -1; 58 | double time_offset_in_seconds; 59 | double dSend_Data[10]; 60 | double m_dRoll, m_dPitch, m_dYaw; 61 | sensor_msgs::Imu imu_data_msg; 62 | //tf_prefix add 63 | std::string tf_prefix_; 64 | //single_used TF 65 | bool m_bSingle_TF_option = false; 66 | 67 | //IMU_service 68 | ros::ServiceServer all_data_reset_service; 69 | iahrs_driver::all_data_reset all_data_reset_cmd; 70 | ros::ServiceServer euler_angle_init_service; 71 | iahrs_driver::euler_angle_init euler_angle_init_cmd; 72 | ros::ServiceServer euler_angle_reset_service; 73 | iahrs_driver::euler_angle_reset euler_angle_reset_cmd; 74 | ros::ServiceServer pose_velocity_reset_service; 75 | iahrs_driver::pose_velocity_reset pose_velocity_reset_cmd; 76 | ros::ServiceServer reboot_sensor_service; 77 | iahrs_driver::reboot_sensor reboot_sensor_cmd; 78 | 79 | int serial_open () 80 | { 81 | printf ("Try to open serial: %s\n", SERIAL_PORT); 82 | 83 | serial_fd = open(SERIAL_PORT, O_RDWR|O_NOCTTY); 84 | if (serial_fd < 0) { 85 | printf ("Error unable to open %s\n", SERIAL_PORT); 86 | return -1; 87 | } 88 | printf ("%s open success\n", SERIAL_PORT); 89 | 90 | struct termios tio; 91 | tcgetattr(serial_fd, &tio); 92 | cfmakeraw(&tio); 93 | tio.c_cflag = CS8|CLOCAL|CREAD; 94 | tio.c_iflag &= ~(IXON | IXOFF); 95 | cfsetspeed(&tio, SERIAL_SPEED); 96 | tio.c_cc[VTIME] = 0; 97 | tio.c_cc[VMIN] = 0; 98 | 99 | int err = tcsetattr(serial_fd, TCSAFLUSH, &tio); 100 | if (err != 0) 101 | { 102 | printf ("Error tcsetattr() function return error\n"); 103 | close(serial_fd); 104 | serial_fd = -1; 105 | return -1; 106 | } 107 | return 0; 108 | } 109 | 110 | static unsigned long GetTickCount() 111 | { 112 | struct timespec ts; 113 | 114 | clock_gettime (CLOCK_MONOTONIC, &ts); 115 | 116 | return ts.tv_sec*1000 + ts.tv_nsec/1000000; 117 | } 118 | 119 | int SendRecv(const char* command, double* returned_data, int data_length) 120 | { 121 | #define COMM_RECV_TIMEOUT 30 122 | 123 | char temp_buff[256]; 124 | read (serial_fd, temp_buff, 256); 125 | 126 | int command_len = strlen(command); 127 | int n = write(serial_fd, command, command_len); 128 | 129 | if (n < 0) return -1; 130 | 131 | const int buff_size = 1024; 132 | int recv_len = 0; 133 | char recv_buff[buff_size + 1]; 134 | 135 | unsigned long time_start = GetTickCount(); 136 | 137 | while (recv_len < buff_size) { 138 | int n = read (serial_fd, recv_buff + recv_len, buff_size - recv_len); 139 | if (n < 0) 140 | { 141 | return -1; 142 | } 143 | else if (n == 0) 144 | { 145 | usleep(1000); 146 | } 147 | else if (n > 0) 148 | { 149 | recv_len += n; 150 | 151 | if (recv_buff[recv_len - 1] == '\r' || recv_buff[recv_len - 1] == '\n') 152 | { 153 | break; 154 | } 155 | } 156 | 157 | unsigned long time_current = GetTickCount(); 158 | unsigned long time_delta = time_current - time_start; 159 | 160 | if (time_delta >= COMM_RECV_TIMEOUT) break; 161 | } 162 | recv_buff[recv_len] = '\0'; 163 | 164 | if (recv_len > 0) 165 | { 166 | if (recv_buff[0] == '!') 167 | { 168 | return -1; 169 | } 170 | } 171 | 172 | if (strncmp(command, recv_buff, command_len - 1) == 0) { 173 | if (recv_buff[command_len - 1] == '=') { 174 | int data_count = 0; 175 | 176 | char* p = &recv_buff[command_len]; 177 | char* pp = NULL; 178 | 179 | for (int i = 0; i < data_length; i++) 180 | { 181 | if (p[0] == '0' && p[1] == 'x') 182 | { 183 | returned_data[i] = strtol(p+2, &pp, 16); 184 | data_count++; 185 | } 186 | else 187 | { 188 | returned_data[i] = strtod(p, &pp); 189 | data_count++; 190 | } 191 | 192 | if (*pp == ',') 193 | { 194 | p = pp + 1; 195 | } 196 | else 197 | { 198 | break; 199 | } 200 | } 201 | return data_count; 202 | } 203 | } 204 | return 0; 205 | } 206 | 207 | void my_handler(sig_atomic_t s) 208 | { 209 | printf("Caught signal %d\n",s); 210 | exit(1); 211 | } 212 | 213 | //ROS Service Callback//////////////////////////////////////////////////////////////////////// 214 | bool All_Data_Reset_Command(iahrs_driver::all_data_reset::Request &req, 215 | iahrs_driver::all_data_reset::Response &res) 216 | { 217 | bool bResult = false; 218 | 219 | double dSend_Data[10]; 220 | SendRecv("rc\n", dSend_Data, 10); 221 | 222 | bResult = true; 223 | res.command_Result = bResult; 224 | return true; 225 | } 226 | 227 | bool Euler_Angle_Init_Command(iahrs_driver::euler_angle_init::Request &req, 228 | iahrs_driver::euler_angle_init::Response &res) 229 | { 230 | bool bResult = false; 231 | 232 | double dSend_Data[10]; 233 | SendRecv("za\n", dSend_Data, 10); 234 | 235 | bResult = true; 236 | res.command_Result = bResult; 237 | return true; 238 | } 239 | 240 | bool Euler_Angle_Reset_Command(iahrs_driver::euler_angle_reset::Request &req, 241 | iahrs_driver::euler_angle_reset::Response &res) 242 | { 243 | bool bResult = false; 244 | 245 | double dSend_Data[10]; 246 | SendRecv("ra\n", dSend_Data, 10); 247 | 248 | bResult = true; 249 | res.command_Result = bResult; 250 | return true; 251 | } 252 | 253 | bool Pose_Velocity_Reset_Command(iahrs_driver::pose_velocity_reset::Request &req, 254 | iahrs_driver::pose_velocity_reset::Response &res) 255 | { 256 | bool bResult = false; 257 | 258 | double dSend_Data[10]; 259 | SendRecv("rp\n", dSend_Data, 10); 260 | 261 | bResult = true; 262 | res.command_Result = bResult; 263 | return true; 264 | } 265 | 266 | bool Reboot_Sensor_Command(iahrs_driver::reboot_sensor::Request &req, 267 | iahrs_driver::reboot_sensor::Response &res) 268 | { 269 | bool bResult = false; 270 | 271 | double dSend_Data[10]; 272 | SendRecv("rd\n", dSend_Data, 10); 273 | 274 | bResult = true; 275 | res.command_Result = bResult; 276 | return true; 277 | } 278 | 279 | 280 | int main (int argc, char** argv) 281 | { 282 | signal(SIGINT,my_handler); 283 | ros::init(argc, argv, "iahrs_driver"); 284 | 285 | ros::NodeHandle private_node_handle("~"); 286 | private_node_handle.param("time_offset_in_seconds", time_offset_in_seconds, 0.0); 287 | 288 | ros::param::get("tf_prefix", tf_prefix_); 289 | 290 | tf::TransformBroadcaster br; 291 | tf::Transform transform; 292 | 293 | 294 | // These values do not need to be converted 295 | imu_data_msg.linear_acceleration_covariance[0] = 0.0064; 296 | imu_data_msg.linear_acceleration_covariance[4] = 0.0063; 297 | imu_data_msg.linear_acceleration_covariance[8] = 0.0064; 298 | 299 | imu_data_msg.angular_velocity_covariance[0] = 0.032*(M_PI/180.0); 300 | imu_data_msg.angular_velocity_covariance[4] = 0.028*(M_PI/180.0); 301 | imu_data_msg.angular_velocity_covariance[8] = 0.006*(M_PI/180.0); 302 | 303 | imu_data_msg.orientation_covariance[0] = 0.013*(M_PI/180.0); 304 | imu_data_msg.orientation_covariance[4] = 0.011*(M_PI/180.0); 305 | imu_data_msg.orientation_covariance[8] = 0.006*(M_PI/180.0); 306 | 307 | ros::NodeHandle nh; 308 | ros::Publisher imu_data_pub = nh.advertise("imu/data", 1); 309 | 310 | //IMU Service/////////////////////////////////////////////////////////////////////////////////////////////// 311 | ros::NodeHandle sh; 312 | all_data_reset_service = sh.advertiseService("all_data_reset_cmd", All_Data_Reset_Command); 313 | euler_angle_init_service = sh.advertiseService("euler_angle_init_cmd", Euler_Angle_Init_Command); 314 | euler_angle_reset_service = sh.advertiseService("euler_angle_reset_cmd", Euler_Angle_Reset_Command); 315 | pose_velocity_reset_service = sh.advertiseService("pose_velocity_reset_cmd", Pose_Velocity_Reset_Command); 316 | reboot_sensor_service = sh.advertiseService("reboot_sensor_cmd", Reboot_Sensor_Command); 317 | 318 | nh.getParam("m_bSingle_TF_option", m_bSingle_TF_option); 319 | printf("##m_bSingle_TF_option: %d \n", m_bSingle_TF_option); 320 | 321 | ros::Rate loop_rate(100); //HZ 322 | serial_open(); 323 | 324 | SendRecv("za\n", dSend_Data, 10); // Euler Angle -> '0.0' Reset 325 | usleep(10000); 326 | 327 | while(ros::ok()) 328 | { 329 | ros::spinOnce(); 330 | 331 | if (serial_fd >= 0) 332 | { 333 | const int max_data = 10; 334 | double data[max_data]; 335 | int no_data = 0; 336 | 337 | no_data = SendRecv("g\n", data, max_data); // Read angular_velocity _ wx, wy, wz 338 | if (no_data >= 3) 339 | { 340 | // angular_velocity 341 | imu_data_msg.angular_velocity.x = _pIMU_data.dAngular_velocity_x = data[0]*(M_PI/180.0); 342 | imu_data_msg.angular_velocity.y = _pIMU_data.dAngular_velocity_y = data[1]*(M_PI/180.0); 343 | imu_data_msg.angular_velocity.z = _pIMU_data.dAngular_velocity_z = data[2]*(M_PI/180.0); 344 | 345 | } 346 | 347 | no_data = SendRecv("a\n", data, max_data); // Read linear_acceleration unit: m/s^2 348 | if (no_data >= 3) 349 | { 350 | //// linear_acceleration g to m/s^2 351 | imu_data_msg.linear_acceleration.x = _pIMU_data.dLinear_acceleration_x = data[0] * 9.80665; 352 | imu_data_msg.linear_acceleration.y = _pIMU_data.dLinear_acceleration_y = data[1] * 9.80665; 353 | imu_data_msg.linear_acceleration.z = _pIMU_data.dLinear_acceleration_z = data[2] * 9.80665; 354 | } 355 | 356 | no_data = SendRecv("e\n", data, max_data); // Read Euler angle 357 | if (no_data >= 3) 358 | { 359 | // Euler _ rad 360 | _pIMU_data.dEuler_angle_Roll = data[0]*(M_PI/180.0); 361 | _pIMU_data.dEuler_angle_Pitch = data[1]*(M_PI/180.0); 362 | _pIMU_data.dEuler_angle_Yaw = data[2]*(M_PI/180.0); 363 | } 364 | 365 | //editing_ 366 | tf::Quaternion orientation = tf::createQuaternionFromRPY(_pIMU_data.dEuler_angle_Roll , _pIMU_data.dEuler_angle_Pitch, _pIMU_data.dEuler_angle_Yaw); 367 | // orientation 368 | imu_data_msg.orientation.x = orientation[0]; 369 | imu_data_msg.orientation.y = orientation[1]; 370 | imu_data_msg.orientation.z = orientation[2]; 371 | imu_data_msg.orientation.w = orientation[3]; 372 | 373 | // calculate measurement time 374 | ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds); 375 | 376 | imu_data_msg.header.stamp = measurement_time; 377 | imu_data_msg.header.frame_id = tf_prefix_ + "/imu_link"; // "imu_link" 378 | 379 | // publish the IMU data 380 | imu_data_pub.publish(imu_data_msg); 381 | 382 | //Publish tf 383 | if(m_bSingle_TF_option) 384 | { 385 | transform.setOrigin( tf::Vector3(0.0, 0.0, 0.2) ); 386 | tf::Quaternion q; 387 | q.setRPY(_pIMU_data.dEuler_angle_Roll, _pIMU_data.dEuler_angle_Pitch, _pIMU_data.dEuler_angle_Yaw); 388 | transform.setRotation(q); 389 | //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "imu_link")); 390 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), tf_prefix_ + "/base_link", tf_prefix_ + "/imu_link")); 391 | } 392 | 393 | 394 | } 395 | 396 | 397 | 398 | loop_rate.sleep(); 399 | } 400 | 401 | close (serial_fd); 402 | 403 | return 0; 404 | } 405 | -------------------------------------------------------------------------------- /srv/all_data_reset.srv: -------------------------------------------------------------------------------- 1 | #This message is used to send command to IAHRS(IMU) 2 | #All Data Reset command 3 | 4 | --- 5 | bool command_Result 6 | -------------------------------------------------------------------------------- /srv/euler_angle_init.srv: -------------------------------------------------------------------------------- 1 | #This message is used to send command to IAHRS(IMU) 2 | #Euler_angle_init command 3 | 4 | --- 5 | bool command_Result 6 | -------------------------------------------------------------------------------- /srv/euler_angle_reset.srv: -------------------------------------------------------------------------------- 1 | #This message is used to send command to IAHRS(IMU) 2 | #Euler_angle Reset command 3 | 4 | --- 5 | bool command_Result 6 | -------------------------------------------------------------------------------- /srv/pose_velocity_reset.srv: -------------------------------------------------------------------------------- 1 | #This message is used to send command to IAHRS(IMU) 2 | #Pose_Velocity_Reset command 3 | 4 | --- 5 | bool command_Result 6 | -------------------------------------------------------------------------------- /srv/reboot_sensor.srv: -------------------------------------------------------------------------------- 1 | #This message is used to send command to IAHRS(IMU) 2 | #Reboot Sensor command 3 | 4 | --- 5 | bool command_Result 6 | --------------------------------------------------------------------------------