├── .gitignore ├── mpu6050.png ├── src ├── echo_imu.py └── mpu6050_node.cpp ├── LICENSE ├── README.md ├── package.xml └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | 3 | -------------------------------------------------------------------------------- /mpu6050.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matpalm/ros-mpu6050-node/HEAD/mpu6050.png -------------------------------------------------------------------------------- /src/echo_imu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import json 3 | import rospy 4 | from sensor_msgs.msg import Imu 5 | 6 | def callback(msg): 7 | av = msg.angular_velocity 8 | lv = msg.linear_acceleration 9 | d = {"angular_velocity": {"x": av.x, "y": av.y, "z": av.z}, 10 | "linear_acceleration": {"x": lv.x, "y": lv.y, "z": lv.z}} 11 | print json.dumps(d) 12 | 13 | rospy.init_node('imu_echo') 14 | rospy.Subscriber("/imu/data_raw", Imu, callback) 15 | 16 | r = rospy.Rate(5) 17 | while not rospy.is_shutdown(): 18 | r.sleep() 19 | 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Mat Kelcey 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 | c++ [ROS](http://www.ros.org/) node wrapper for the [mpu6050](http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/) gyroscope / accelerometer. 2 | 3 | ![mpu6050](mpu6050.png) 4 | 5 | Publishes [sensor_msgs::IMU](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) messages at 10Hz to `/mpu6050` topic. 6 | Specifically populates `angular_velocity` & `linear_acceleration`. 7 | 8 | on rasp pi 2 uses ~1% cpu and <1% mem 9 | 10 | ```` 11 | rostopic echo /mpu6050 12 | --- 13 | header: 14 | seq: 87 15 | stamp: 16 | secs: 0 17 | nsecs: 0 18 | frame_id: '' 19 | orientation: 20 | x: 0.0 21 | y: 0.0 22 | z: 0.0 23 | w: 0.0 24 | orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 25 | angular_velocity: 26 | x: -2.27480912209 27 | y: 0.618320584297 28 | z: -0.648854970932 29 | angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 30 | linear_acceleration: 31 | x: 0.0751953125 32 | y: -0.05419921875 33 | z: 1.01928710938 34 | --- 35 | ```` 36 | 37 | TODOS 38 | * no tf support at all, at very least some kind of zeroing service call would be nice... (also set frame_id) 39 | * no handling of sensor noise (though perhaps better to include directly in filter using this for tracking actual rover) 40 | * populate covariance values. 41 | -------------------------------------------------------------------------------- /src/mpu6050_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | const int I2C_ADDR = 0x68; 9 | const int PWR_MGMT_1 = 0x6B; 10 | 11 | float read_word_2c(int fd, int addr) { 12 | int high = wiringPiI2CReadReg8(fd, addr); 13 | int low = wiringPiI2CReadReg8(fd, addr+1); 14 | int val = (high << 8) + low; 15 | return float((val >= 0x8000) ? -((65535 - val) + 1) : val); 16 | } 17 | 18 | int main(int argc, char **argv) { 19 | 20 | // Connect to device. 21 | int fd = wiringPiI2CSetup(I2C_ADDR); 22 | if (fd == -1) { 23 | printf("no i2c device found?\n"); 24 | return -1; 25 | } 26 | // Device starts in sleep mode so wake it up. 27 | wiringPiI2CWriteReg16(fd, PWR_MGMT_1, 0); 28 | 29 | // Start ROS node stuff. 30 | ros::init(argc, argv, "mpu6050"); 31 | ros::NodeHandle node; 32 | ros::Publisher pub = node.advertise("imu", 10); 33 | ros::Rate rate(10); // hz 34 | 35 | // Publish in loop. 36 | while(ros::ok()) { 37 | sensor_msgs::Imu msg; 38 | msg.header.stamp = ros::Time::now(); 39 | msg.header.frame_id = '0'; // no frame 40 | 41 | // Read gyroscope values. 42 | // At default sensitivity of 250deg/s we need to scale by 131. 43 | msg.angular_velocity.x = read_word_2c(fd, 0x43) / 131; 44 | msg.angular_velocity.y = read_word_2c(fd, 0x45) / 131; 45 | msg.angular_velocity.z = read_word_2c(fd, 0x47) / 131; 46 | 47 | // Read accelerometer values. 48 | // At default sensitivity of 2g we need to scale by 16384. 49 | // Note: at "level" x = y = 0 but z = 1 (i.e. gravity) 50 | // But! Imu msg docs say acceleration should be in m/2 so need to *9.807 51 | const float la_rescale = 16384.0 / 9.807; 52 | msg.linear_acceleration.x = read_word_2c(fd, 0x3b) / la_rescale; 53 | msg.linear_acceleration.y = read_word_2c(fd, 0x3d) / la_rescale; 54 | msg.linear_acceleration.z = read_word_2c(fd, 0x3f) / la_rescale; 55 | 56 | // Pub & sleep. 57 | pub.publish(msg); 58 | ros::spinOnce(); 59 | rate.sleep(); 60 | } 61 | return 0; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpu6050 4 | 1.0.0 5 | The mpu6050 package 6 | 7 | 8 | 9 | 10 | mat 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | https://github.com/matpalm/ros-mpu6050-node 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | roscpp 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mpu6050) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | geometry_msgs 10 | sensor_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs # Or other packages containing msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES mpu6050 105 | # CATKIN_DEPENDS roscpp 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(mpu6050 122 | # src/${PROJECT_NAME}/mpu6050.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(mpu6050 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | add_executable(mpu6050_node src/mpu6050_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(mpu6050_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | target_link_libraries(mpu6050_node 139 | ${catkin_LIBRARIES} 140 | wiringPi 141 | ) 142 | 143 | ############# 144 | ## Install ## 145 | ############# 146 | 147 | # all install targets should use catkin DESTINATION variables 148 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 149 | 150 | ## Mark executable scripts (Python etc.) for installation 151 | ## in contrast to setup.py, you can choose the destination 152 | # install(PROGRAMS 153 | # scripts/my_python_script 154 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 155 | # ) 156 | 157 | ## Mark executables and/or libraries for installation 158 | # install(TARGETS mpu6050 mpu6050_node 159 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark cpp header files for installation 165 | # install(DIRECTORY include/${PROJECT_NAME}/ 166 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 167 | # FILES_MATCHING PATTERN "*.h" 168 | # PATTERN ".svn" EXCLUDE 169 | # ) 170 | 171 | ## Mark other files for installation (e.g. launch and bag files, etc.) 172 | # install(FILES 173 | # # myfile1 174 | # # myfile2 175 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 176 | # ) 177 | 178 | ############# 179 | ## Testing ## 180 | ############# 181 | 182 | ## Add gtest based cpp test target and link libraries 183 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mpu6050.cpp) 184 | # if(TARGET ${PROJECT_NAME}-test) 185 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 186 | # endif() 187 | 188 | ## Add folders to be run by python nosetests 189 | # catkin_add_nosetests(test) 190 | --------------------------------------------------------------------------------