├── README.md ├── src ├── singleSonar.cpp └── main.cpp ├── package.xml └── CMakeLists.txt /README.md: -------------------------------------------------------------------------------- 1 | # ROS package to publish multiple RangeMsgs 2 | - A ROS package for multiple ultrasonic sensor running simultaneously 3 | - Publishing range msgs at around 5 hz 4 | - Hardware: MB7380 HRXL-MaxSonar-WRT as ultrasonic sensor and Raspberry Pi as a main controller 5 | - WiringPi Lib is used to create threads for each ultrasonic sensor 6 | - UTs are connected to pi3 at pins 7 | - S0: Echo-17, Trig-27 8 | - S1: Echo-22, Trig-05 9 | - S2: Echo-06, Trig-13 10 | - S1: Echo-19, Trig-26 11 | - S1: Echo-21, Trig-20 12 | - where: S0 is the left most and S4 is the right most 13 | 14 | ##### NOTE : *this package requires super-user permission to work because it is using WiringPi Library to crearte thresds for each ultrasonic sensor* 15 | 16 | #### Video Demonstraion: *[Integration of MaxBotix MB7380 MaxSonar into ROS](https://www.youtube.com/watch?v=7dzfr7vlPmI)* 17 | -------------------------------------------------------------------------------- /src/singleSonar.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "ros/ros.h" 7 | #include "ros/time.h" 8 | #include "sensor_msgs/Range.h" 9 | 10 | #define ECHO_0 0 11 | #define ECHO_1 22 12 | #define ECHO_2 06 13 | #define ECHO_3 19 14 | #define ECHO_4 21 15 | 16 | volatile double distance0 = 0.0; 17 | volatile double distance1 = 0.0; 18 | volatile double distance2 = 0.0; 19 | volatile double distance3 = 0.0; 20 | volatile double distance4 = 0.0; 21 | //volatile int x = 0; 22 | //volatile int y = 0; 23 | //volatile int z = 0; 24 | 25 | 26 | void sonarCheck0 (void) 27 | { 28 | volatile static long startTime = 0.0; 29 | if(digitalRead(ECHO_0) == HIGH){ 30 | startTime = micros(); 31 | return; 32 | } 33 | 34 | if(digitalRead(ECHO_0) == LOW){ 35 | long travelTime = micros() - startTime; 36 | distance0 = travelTime / 1000.0; 37 | //x = 1; 38 | } 39 | } 40 | 41 | int setupWiringPi(void) { 42 | wiringPiSetup(); 43 | pinMode(ECHO_0, INPUT); 44 | wiringPiISR (ECHO_0, INT_EDGE_BOTH, &sonarCheck0); 45 | } 46 | 47 | int main(int argc, char **argv) 48 | { 49 | setupWiringPi(); 50 | 51 | ros::init(argc, argv, "range_msg"); 52 | ros::NodeHandle nh; 53 | 54 | ros::Publisher sonar0_pub = nh.advertise("sonar0", 5); 55 | ros::Rate loop_rate(10); 56 | 57 | while (ros::ok()) 58 | { 59 | sensor_msgs::Range rangeMsg; 60 | rangeMsg.header.stamp = ros::Time::now(); 61 | rangeMsg.header.frame_id = "/sonar0"; 62 | rangeMsg.radiation_type = 0, //0=ultrasonic, 1=IR 63 | rangeMsg.field_of_view = 0.05; 64 | rangeMsg.min_range = 0.0; 65 | rangeMsg.max_range = 5.0; 66 | rangeMsg.range = distance0; 67 | //broadcastTransform(threadName) 68 | //RangeSensorPub.publish(rangeMsg) 69 | 70 | sonar0_pub.publish(rangeMsg); 71 | ros::spinOnce(); 72 | loop_rate.sleep(); 73 | } 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sonar_test 4 | 0.0.0 5 | The sonar_test package 6 | 7 | 8 | 9 | 10 | rosimu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | sensor_msgs 45 | std_msgs 46 | roscpp 47 | sensor_msgs 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sonar_test) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | sensor_msgs 13 | std_msgs 14 | ) 15 | 16 | find_library(wiringPi wiringPi.so) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | #find_package(PkgConfig REQUIRED COMPONENTS system) 20 | #pkg_check_modules(wiringPi REQUIRED wiringPi) 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | #generate_messages( 74 | # DEPENDENCIES sensor_msgs std_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if you package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES sonar_test 109 | # CATKIN_DEPENDS roscpp sensor_msgs std_msgs 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/sonar_test.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | #add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | add_executable(${PROJECT_NAME}_node src/main.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | target_link_libraries(${PROJECT_NAME}_node 151 | ${catkin_LIBRARIES} 152 | wiringPi 153 | ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 171 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | # install(DIRECTORY include/${PROJECT_NAME}/ 178 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | # FILES_MATCHING PATTERN "*.h" 180 | # PATTERN ".svn" EXCLUDE 181 | # ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sonar_test.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "ros/ros.h" 7 | #include "ros/time.h" 8 | #include "sensor_msgs/Range.h" 9 | 10 | //assigned pins for ultrasonic sensor 11 | #define ECHO_0 0 12 | #define ECHO_1 3 13 | #define ECHO_2 22 14 | #define ECHO_3 24 15 | #define ECHO_4 29 16 | 17 | #define FOV 0.5 //fake FOV 18 | 19 | volatile double distance0 = 0.0; 20 | volatile double distance1 = 0.0; 21 | volatile double distance2 = 0.0; 22 | volatile double distance3 = 0.0; 23 | volatile double distance4 = 0.0; 24 | 25 | //volatile int x = 0; 26 | //volatile int y = 0; 27 | //volatile int z = 0; 28 | 29 | //initializing msg type to publish on ROS 30 | sensor_msgs::Range rangeMsg0; 31 | sensor_msgs::Range rangeMsg1; 32 | sensor_msgs::Range rangeMsg2; 33 | sensor_msgs::Range rangeMsg3; 34 | sensor_msgs::Range rangeMsg4; 35 | 36 | //to measure object distance in front of sonar0 37 | void sonarCheck0 (void) 38 | { 39 | volatile static double distance0_P = 0.30; 40 | volatile static long startTime0 = 0.0; 41 | if(digitalRead(ECHO_0) == HIGH){ 42 | startTime0 = micros(); 43 | return; 44 | } 45 | 46 | if(digitalRead(ECHO_0) == LOW){ 47 | long travelTime0 = micros() - startTime0; 48 | distance0 = travelTime0 / 1000.0; 49 | if (distance0 < 0.30) distance0 = 0.30; 50 | else if (distance0 > 5.00) distance0 = distance0_P; 51 | distance0_P = distance0; 52 | } 53 | } 54 | 55 | //to measure object distance in front of sonar1 56 | void sonarCheck1 (void) 57 | { 58 | volatile static double distance1_P = 0.30; 59 | volatile static long startTime1 = 0.0; 60 | if(digitalRead(ECHO_1) == HIGH){ 61 | startTime1 = micros(); 62 | return; 63 | } 64 | 65 | if(digitalRead(ECHO_1) == LOW){ 66 | long travelTime1 = micros() - startTime1; 67 | distance1 = travelTime1 / 1000.0; 68 | if (distance1 < 0.30) distance1 = 0.30; 69 | else if (distance1 > 5.00) distance1 = distance1_P; 70 | distance1_P =distance1; 71 | } 72 | } 73 | 74 | //to measure object distance in front of sonar2 75 | void sonarCheck2 (void) 76 | { 77 | volatile static double distance2_P = 0.30; 78 | volatile static long startTime2 = 0.0; 79 | if(digitalRead(ECHO_2) == HIGH){ 80 | startTime2 = micros(); 81 | return; 82 | } 83 | 84 | if(digitalRead(ECHO_2) == LOW){ 85 | long travelTime2 = micros() - startTime2; 86 | distance2 = travelTime2 / 1000.0; 87 | if (distance2 < 0.30) distance2 = 0.30; 88 | else if (distance2 > 5.00) distance2 = distance2_P; 89 | distance2_P = distance2; 90 | } 91 | } 92 | 93 | //to measure object distance in front of sonar3 94 | void sonarCheck3 (void) 95 | { 96 | volatile static double distance3_P = 0.30; 97 | volatile static long startTime3 = 0.0; 98 | if(digitalRead(ECHO_3) == HIGH){ 99 | startTime3 = micros(); 100 | return; 101 | } 102 | 103 | if(digitalRead(ECHO_3) == LOW){ 104 | long travelTime3 = micros() - startTime3; 105 | distance3 = travelTime3 / 1000.0; 106 | if (distance3 < 0.30) distance3 = 0.30; 107 | else if (distance3 > 5.00) distance3 = distance3_P; 108 | distance3_P = distance3; 109 | } 110 | } 111 | 112 | //to measure object distance in front of sonar4 113 | void sonarCheck4 (void) 114 | { 115 | volatile static double distance4_P = 0.30; 116 | volatile static long startTime4 = 0.0; 117 | if(digitalRead(ECHO_4) == HIGH){ 118 | startTime4 = micros(); 119 | return; 120 | } 121 | 122 | if(digitalRead(ECHO_4) == LOW){ 123 | long travelTime4 = micros() - startTime4; 124 | distance4 = travelTime4 / 1000.0; 125 | if (distance4 < 0.30) distance4 = 0.30; 126 | else if (distance4 > 5.00) distance4 = distance4_P; 127 | distance4_P = distance4; 128 | } 129 | } 130 | 131 | /*initialize every range msg*/ 132 | void setupRangeMsg0(void){ 133 | rangeMsg0.header.stamp = ros::Time::now(); 134 | rangeMsg0.header.frame_id = "/sonar0"; 135 | rangeMsg0.radiation_type = 0, //0=ultrasonic, 1=IR 136 | rangeMsg0.field_of_view = FOV; 137 | rangeMsg0.min_range = 0.0; 138 | rangeMsg0.max_range = 5.0; 139 | } 140 | 141 | void setupRangeMsg1(void){ 142 | rangeMsg1.header.stamp = ros::Time::now(); 143 | rangeMsg1.header.frame_id = "/sonar1"; 144 | rangeMsg1.radiation_type = 0, //0=ultrasonic, 1=IR 145 | rangeMsg1.field_of_view = FOV; 146 | rangeMsg1.min_range = 0.0; 147 | rangeMsg1.max_range = 5.0; 148 | } 149 | 150 | void setupRangeMsg2(void){ 151 | rangeMsg2.header.stamp = ros::Time::now(); 152 | rangeMsg2.header.frame_id = "/sonar2"; 153 | rangeMsg2.radiation_type = 0, //0=ultrasonic, 1=IR 154 | rangeMsg2.field_of_view = FOV; 155 | rangeMsg2.min_range = 0.0; 156 | rangeMsg2.max_range = 5.0; 157 | } 158 | 159 | void setupRangeMsg3(void){ 160 | rangeMsg3.header.stamp = ros::Time::now(); 161 | rangeMsg3.header.frame_id = "/sonar3"; 162 | rangeMsg3.radiation_type = 0, //0=ultrasonic, 1=IR 163 | rangeMsg3.field_of_view = FOV; 164 | rangeMsg3.min_range = 0.0; 165 | rangeMsg3.max_range = 5.0; 166 | } 167 | 168 | void setupRangeMsg4(void){ 169 | rangeMsg4.header.stamp = ros::Time::now(); 170 | rangeMsg4.header.frame_id = "/sonar4"; 171 | rangeMsg4.radiation_type = 0, //0=ultrasonic, 1=IR 172 | rangeMsg4.field_of_view = FOV; 173 | rangeMsg4.min_range = 0.0; 174 | rangeMsg4.max_range = 5.0; 175 | } 176 | 177 | //setting up threads for each ultrasonic sensor 178 | void setupWiringPi(void) { 179 | wiringPiSetup(); 180 | pinMode(ECHO_0, INPUT); 181 | pinMode(ECHO_1, INPUT); 182 | pinMode(ECHO_2, INPUT); 183 | pinMode(ECHO_3, INPUT); 184 | pinMode(ECHO_4, INPUT); 185 | 186 | wiringPiISR (ECHO_0, INT_EDGE_BOTH, &sonarCheck0); 187 | wiringPiISR (ECHO_1, INT_EDGE_BOTH, &sonarCheck1); 188 | wiringPiISR (ECHO_2, INT_EDGE_BOTH, &sonarCheck2); 189 | wiringPiISR (ECHO_3, INT_EDGE_BOTH, &sonarCheck3); 190 | wiringPiISR (ECHO_4, INT_EDGE_BOTH, &sonarCheck4); 191 | } 192 | 193 | //MAIN 194 | int main(int argc, char **argv) 195 | { 196 | setupWiringPi(); 197 | 198 | ros::init(argc, argv, "range_msg"); 199 | ros::NodeHandle nh; 200 | 201 | ros::Publisher sonar0_pub = nh.advertise("sonar0", 5); 202 | ros::Publisher sonar1_pub = nh.advertise("sonar1", 5); 203 | ros::Publisher sonar2_pub = nh.advertise("sonar2", 5); 204 | ros::Publisher sonar3_pub = nh.advertise("sonar3", 5); 205 | ros::Publisher sonar4_pub = nh.advertise("sonar4", 5); 206 | 207 | ros::Rate loop_rate(10); 208 | 209 | 210 | while (ros::ok()) 211 | { 212 | /* sensor_msgs::Range rangeMsg; 213 | rangeMsg.header.stamp = ros::Time::now(); 214 | rangeMsg.header.frame_id = "/sonar0"; 215 | rangeMsg.radiation_type = 0, //0=ultrasonic, 1=IR 216 | rangeMsg.field_of_view = 0.05; 217 | rangeMsg.min_range = 0.0; 218 | rangeMsg.max_range = 5.0; 219 | rangeMsg.range = distance0; 220 | //broadcastTransform(threadName) 221 | //RangeSensorPub.publish(rangeMsg) 222 | */ 223 | 224 | setupRangeMsg0(); 225 | setupRangeMsg1(); 226 | setupRangeMsg2(); 227 | setupRangeMsg3(); 228 | setupRangeMsg4(); 229 | 230 | rangeMsg0.range = distance0; 231 | rangeMsg1.range = distance1; 232 | rangeMsg2.range = distance2; 233 | rangeMsg3.range = distance3; 234 | rangeMsg4.range = distance4; 235 | 236 | sonar0_pub.publish(rangeMsg0); 237 | sonar1_pub.publish(rangeMsg1); 238 | sonar2_pub.publish(rangeMsg2); 239 | sonar3_pub.publish(rangeMsg3); 240 | sonar4_pub.publish(rangeMsg4); 241 | 242 | ros::spinOnce(); 243 | loop_rate.sleep(); 244 | } 245 | 246 | return 0; 247 | } 248 | --------------------------------------------------------------------------------