一、IMU基本知識
1、產品選型
選用的IMU為維特智能的WTGAHRS2，內置芯片為JY-901B和GPS模塊。
2、通訊協議
二、創建ROS節點發布IMU信息
1、安裝串口模塊
sudo apt-get install ros-kinetic-serial #ros為Kinect版本
2、話題發布
設置IMU模塊的量程為16g，串口通訊波特率為115200，停止位1，校驗位0。
節點代碼如下：
#include "ros/ros.h" #include "std_msgs/String.h" #include <serial/serial.h> #include <sensor_msgs/Imu.h> #include <sstream> #include <iostream> //用於四元數與歐拉角的轉換 #include "tf/transform_datatypes.h" //轉換函數頭文件 //#include <nav_msgs/Odometry.h> //里程計信息格式 //加速度處理函數 //buf：串口數據緩沖區 //i:此時文件頭數據在buf中的索引 //imu_data:imu消息格式 inline void accelerationCompute( const uint8_t* buf, int i, sensor_msgs::Imu& imu_data ) { //解算每一幀數據 uint16_t AxL = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+2] ) ); uint16_t AxH = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+3] ) ); uint16_t AyL = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+4] ) ); uint16_t AyH = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+5] ) ); uint16_t AzL = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+6] ) ); uint16_t AzH = static_cast<uint16_t>( static_cast<uint8_t>( buf[i+7] ) ); // *paramOut << hex; // *paramOut << "AxL:" << AxL << " " << "AxH:" << AxH << " " << "AyL:" << AyL << " " << "AyH:" << AyH << " " << "AzL:" << AzL << " " << "AzH:" << AzH << endl; double ax = static_cast<double>( static_cast<int16_t>( ( AxH << 8 ) | AxL ) ) / 32768 * 16;// * 9.8; //x軸加速度(g) double ay = static_cast<double>( static_cast<int16_t>( ( AyH << 8 ) | AyL ) ) / 32768 * 16;// * 9.8; //y軸加速度(g) double az = static_cast<double>( static_cast<int16_t>( ( AzH << 8 ) | AzL ) ) / 32768 * 16;// * 9.8; //z軸加速度(g) imu_data.linear_acceleration.x = ax * 9.8; imu_data.linear_acceleration.y = ay * 9.8; imu_data.linear_acceleration.z = az * 9.8; } //角速度處理函數 //buf：串口數據緩沖區 //i:此時文件頭數據在buf中的索引 //imu_data:imu消息格式 inline void angularVelocityCompute( const uint8_t* buf, int i, sensor_msgs::Imu& imu_data ) { //解算每一幀數據 uint16_t wxL = static_cast<uint8_t>( buf[i+2] ); uint16_t wxH = static_cast<uint8_t>( buf[i+3] ); uint16_t wyL = static_cast<uint8_t>( buf[i+4] ); uint16_t wyH = static_cast<uint8_t>( buf[i+5] ); uint16_t wzL = static_cast<uint8_t>( buf[i+6] ); uint16_t wzH = static_cast<uint8_t>( buf[i+7] ); double angVelx = static_cast<double>( static_cast<int16_t>( ( wxH << 8 ) | wxL ) ) / 32768 * 2000; //x軸角速度 double angVely = static_cast<double>( static_cast<int16_t>( ( wyH << 8 ) | wyL ) ) / 32768 * 2000; //y軸角速度 double angVelz = static_cast<double>( static_cast<int16_t>( ( wzH << 8 ) | wzL ) ) / 32768 * 2000; //z軸角速度 imu_data.angular_velocity.x = angVelx * 3.14 / 180; imu_data.angular_velocity.y = angVely * 3.14 / 180; imu_data.angular_velocity.z = angVelz * 3.14 / 180; } //角度處理函數 //buf：串口數據緩沖區 //i:此時文件頭數據在buf中的索引 //imu_data:imu消息格式 inline void angularCompute( const uint8_t* buf, int i, sensor_msgs::Imu& imu_data ) { //解算每一幀數據 uint16_t RollL = static_cast<uint8_t>( buf[i+2] ); uint16_t RollH = static_cast<uint8_t>( buf[i+3] ); uint16_t pitchL = static_cast<uint8_t>( buf[i+4] ); uint16_t pitchH = static_cast<uint8_t>( buf[i+5] ); uint16_t YawL = static_cast<uint8_t>( buf[i+6] ); uint16_t YawH = static_cast<uint8_t>( buf[i+7] ); double Roll = static_cast<double>( static_cast<int16_t>( ( RollH << 8 ) | RollL ) ) / 32768 * 180; //x軸角度 double Pitch = static_cast<double>( static_cast<int16_t>( ( pitchH << 8 ) | pitchL ) ) / 32768 * 180; //y軸角度 double Yaw = static_cast<double>( static_cast<int16_t>( ( YawH << 8 ) | YawL ) ) / 32768 * 180; //z軸角度 //角度制轉弧度制 Roll = Roll * 3.14 / 180; Pitch = Pitch * 3.14 / 180; Yaw = Yaw * 3.14 / 180; geometry_msgs::Quaternion geoQuat( tf::createQuaternionMsgFromRollPitchYaw( Roll , Pitch, Yaw ) ); //歐拉角轉四元數，其中歐拉角是rpy，對應旋轉順序為ZYX imu_data.orientation = geoQuat; //sensor_msgs::Imu的orientation類型是geometry_msg::Quaternion //tf::quaternionTFToMsg( orientation, imu_data.orientation ); //tf的四元數轉geometry_msg::Quaternion( sensor_msgs::Imu的orientation類型是geometry_msg::Quaternion ) //static double rollInit = Roll; //東北天坐標系下角度的Roll初始值 //static double pitchInit = Pitch; //東北天坐標系下的Pitch初始值 //static double yawInit = Yaw; //東北天坐標系下的Yaw初始值 //imu_data.orientation.x = ( Roll - rollInit ) * 3.1415926 / 180; //以機器人初始位姿為世界坐標系的x軸角度信息 //imu_data.orientation.y = ( Pitch - pitchInit ) * 3.1415926 / 180; //以機器人初始位姿為世界坐標系的y軸角度信息 //imu_data.orientation.z = ( Yaw - yawInit ) * 3.1415926 / 180; //以機器人初始位姿為世界坐標系的z軸角度信息 //imu_data.orientation.w = 1.0; } int main( int argc, char** argv ) { //初始化節點 ros::init( argc, argv, "serial_imu_node" ); //聲明節點句柄 ros::NodeHandle nh; //訂閱主題，配置回調函數 //ros::Subscriber IMU_write_pub = nh.subscribe( "imu_command", 1000, write_callback ); //發布話題，消息格式使用sensor_msg::Imu標准格式（topic名稱，隊列長度） ros::Publisher IMU_read_pub = nh.advertise<sensor_msgs::Imu>( "imu/data", 1000 ); //開啟串口 serial::Serial serialport; //聲明串口對象 try { //設置串口屬性並開啟串口 serialport.setPort( "/dev/ttyUSB0" ); serialport.setBaudrate( 115200 ); serial::Timeout to = serial::Timeout::simpleTimeout(100); serialport.setTimeout( to ); serialport.open(); } catch ( serial::IOException& e ) { ROS_ERROR_STREAM( "unable to open serialport" ); return -1; } //檢測串口是否已經打開，並給出提示信息 if( serialport.isOpen() ) { ROS_INFO_STREAM( "Serialport initialized" ); } else return -1; //消息發布頻率 ros::Rate loop_rate( 100 ); //打包IMU數據 sensor_msgs::Imu imu_data; while( ros::ok() ) { //處理串口發過來的數據 uint8_t data_size = serialport.available();//serialport.available()當串口沒有緩存的時候，這個函數會一直等到有緩存才返回字符數 if( data_size >= 11 ) //長度大於等於11的數據才是有效數據，因為IMU的發送數據長度為11位 { uint8_t buf[data_size]; serialport.read( buf, data_size ); for( int i = 0; i < data_size; i++ ) { //尋找加速度有效幀 if( static_cast<uint8_t>( buf[i] ) == 0x55 && static_cast<uint8_t>( buf[i+1] ) == 0x51 && data_size - i >= 11 ) { //求取數據位的和，做和校驗 uint8_t sum = static_cast<uint8_t>(buf[i]) + static_cast<uint8_t>(buf[i+1]) + static_cast<uint8_t>(buf[i+2]) + static_cast<uint8_t>(buf[i+3]) + static_cast<uint8_t>(buf[i+4]) + static_cast<uint8_t>(buf[i+5]) + static_cast<uint8_t>(buf[i+6]) + static_cast<uint8_t>(buf[i+7]) + static_cast<uint8_t>(buf[i+8]) + static_cast<uint8_t>(buf[i+9]); //數據位完整性校驗 if( sum == static_cast<uint8_t>(buf[i+10]) ) { accelerationCompute( buf, i, imu_data ); //加速度數據處理函數 } } //尋找角速度有效幀 if( static_cast<uint8_t>( buf[i] ) == 0x55 && static_cast<uint8_t>( buf[i+1] ) == 0x52 && data_size - i >= 11 ) { //求取數據位的和，做和校驗 uint8_t sum = static_cast<uint8_t>(buf[i]) + static_cast<uint8_t>(buf[i+1]) + static_cast<uint8_t>(buf[i+2]) + static_cast<uint8_t>(buf[i+3]) + static_cast<uint8_t>(buf[i+4]) + static_cast<uint8_t>(buf[i+5]) + static_cast<uint8_t>(buf[i+6]) + static_cast<uint8_t>(buf[i+7]) + static_cast<uint8_t>(buf[i+8]) + static_cast<uint8_t>(buf[i+9]); //數據位完整性校驗 if( sum == static_cast<uint8_t>(buf[i+10]) ) { angularVelocityCompute( buf, i, imu_data ); //角速度數據處理函數 } } //尋找角度有效幀 if( static_cast<uint8_t>( buf[i] ) == 0x55 && static_cast<uint8_t>( buf[i+1] ) == 0x53 && data_size - i >= 11 ) { //求取數據位的和，做和校驗 uint8_t sum = static_cast<uint8_t>(buf[i]) + static_cast<uint8_t>(buf[i+1]) + static_cast<uint8_t>(buf[i+2]) + static_cast<uint8_t>(buf[i+3]) + static_cast<uint8_t>(buf[i+4]) + static_cast<uint8_t>(buf[i+5]) + static_cast<uint8_t>(buf[i+6]) + static_cast<uint8_t>(buf[i+7]) + static_cast<uint8_t>(buf[i+8]) + static_cast<uint8_t>(buf[i+9]); //數據位完整性校驗 if( sum == static_cast<uint8_t>(buf[i+10]) ) { angularCompute( buf, i, imu_data ); //角速度數據處理函數 } } } //發布topic imu_data.header.stamp = ros::Time::now(); imu_data.header.frame_id = "base_link"; IMU_read_pub.publish(imu_data); } //處理ROS消息 ros::spinOnce(); loop_rate.sleep(); } return 0; }
3、CMakeLists.txt文件
cmake_minimum_required(VERSION 3.0.2) project(imu) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages #find_package(catkin REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## ################################################ ## To declare and build messages, services or actions from within this ## package, follow these steps: ## * Let MSG_DEP_SET be the set of packages whose message types you use in ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## * In the file package.xml: ## * add a build_depend tag for "message_generation" ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in ## but can be declared for certainty nonetheless: ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation serial nav_msgs ) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate actions in the 'action' folder # add_action_files( # FILES # Action1.action # Action2.action # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # Or other packages containing msgs # ) ################################################ ## Declare ROS dynamic reconfigure parameters ## ################################################ ## To declare and build dynamic reconfigure parameters within this ## package, follow these steps: ## * In the file package.xml: ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to ## find_package(catkin REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder # generate_dynamic_reconfigure_options( # cfg/DynReconf1.cfg # cfg/DynReconf2.cfg # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES imu CATKIN_DEPENDS roscpp rospy std_msgs serial # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} # include ${catkin_INCLUDE_DIRS} ${serial_INCLUDE_DIRS} ) ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/imu.cpp # ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide add_executable(serial_imu_node src/imu_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(serial_imu_node ${catkin_LIBRARIES} ) ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html # install(TARGETS ${PROJECT_NAME}_node # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html # install(TARGETS ${PROJECT_NAME} # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_imu.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) # add_executable(imu_publisher # src/imu_node.cpp # ) # add_dependencies(imu_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # target_link_libraries(imu_publisher # ${catkin_LIBRARIES} #)
4、package.xml文件
<?xml version="1.0"?> <package format="2"> <name>imu</name> <version>0.0.0</version> <description>The imu package</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="oay@todo.todo">oay</maintainer> <!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <license>TODO</license> <!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Example: --> <!-- <url type="website">http://wiki.ros.org/imu</url> --> <!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> <!-- <author email="jane.doe@example.com">Jane Doe</author> --> <!-- The *depend tags are used to specify dependencies --> <!-- Dependencies can be catkin packages or system dependencies --> <!-- Examples: --> <!-- Use depend as a shortcut for packages that are both build and exec dependencies --> <!-- <depend>roscpp</depend> --> <!-- Note that this is equivalent to the following: --> <!-- <build_depend>roscpp</build_depend> --> <!-- <exec_depend>roscpp</exec_depend> --> <!-- Use build_depend for packages you need at compile time: --> <!-- <build_depend>message_generation</build_depend> --> <!-- Use build_export_depend for packages you need in order to build against this package: --> <!-- <build_export_depend>message_generation</build_export_depend> --> <!-- Use buildtool_depend for build tool packages: --> <!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- Use exec_depend for packages you need at runtime: --> <!-- <exec_depend>message_runtime</exec_depend> --> <!-- Use test_depend for packages you need only for testing: --> <!-- <test_depend>gtest</test_depend> --> <!-- Use doc_depend for packages you need only for building documentation: --> <!-- <doc_depend>doxygen</doc_depend> --> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_export_depend>roscpp</build_export_depend> <exec_depend>roscpp</exec_depend> <build_depend>rospy</build_depend> <build_export_depend>rospy</build_export_depend> <exec_depend>rospy</exec_depend> <build_depend>std_msgs</build_depend> <build_export_depend>std_msgs</build_export_depend> <exec_depend>std_msgs</exec_depend> <build_depend>serial</build_depend> <build_export_depend>serial</build_export_depend> <exec_depend>serial</exec_depend> <!-- The export tag contains other, unspecified, tags --> <export> <!-- Other tools can request additional information be placed here --> </export> </package>
orientation