├── .gitignore ├── hc_sr04.jpg ├── src ├── echo_sonars.py ├── export_gpio_pins.sh └── hc_sr04_node.cpp ├── LICENSE ├── README.md ├── package.xml └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | 3 | -------------------------------------------------------------------------------- /hc_sr04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matpalm/ros-hc-sr04-node/HEAD/hc_sr04.jpg -------------------------------------------------------------------------------- /src/echo_sonars.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from sensor_msgs.msg import Range 4 | 5 | sonars = [0, 0, 0] 6 | lkg_sonars = [0, 0, 0] 7 | 8 | def callback(msg, idx): 9 | global sonars 10 | sonars[idx] = msg.range 11 | if lkg_sonars[idx] == 0 or msg.range != 30.0: 12 | lkg_sonars[idx] = msg.range 13 | 14 | rospy.init_node('sonar_subscriber') 15 | for i in range(3): 16 | rospy.Subscriber("/robotZero/sonar_%d" % i, Range, 17 | callback, i) 18 | 19 | r = rospy.Rate(5) 20 | while not rospy.is_shutdown(): 21 | s = ["%5.2f" % v for v in sonars] 22 | s += ["%5.2f" % v for v in lkg_sonars] 23 | print " ".join(s) 24 | r.sleep() 25 | 26 | -------------------------------------------------------------------------------- /src/export_gpio_pins.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | ### BEGIN INIT INFO 4 | # Provides: gpio_setup 5 | # Required-Start: $remote_fs $syslog 6 | # Required-Stop: $remote_fs $syslog 7 | # Default-Start: 2 3 4 5 8 | # Default-Stop: 0 1 6 9 | # Short-Description: exports gpio pins for 3 sonars. 10 | # Description: exports gpio pins for 3 sonars. 11 | ### END INIT INFO 12 | 13 | case "$1" in 14 | start) 15 | # to run as non run using wiringPiSetupSys we 16 | # need to export the gpio pins. 17 | # see http://wiringpi.com/reference/setup/ 18 | gpio export 24 out 19 | gpio export 25 in 20 | gpio export 22 out 21 | gpio export 23 in 22 | gpio export 18 out 23 | gpio export 27 in 24 | ;; 25 | stop) 26 | gpio unexportall 27 | ;; 28 | *) 29 | echo "unknown command" 30 | exit 1 31 | ;; 32 | esac 33 | 34 | -------------------------------------------------------------------------------- /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 a set of [hc-sr04](http://www.micropik.com/PDF/HCSR04.pdf) ultrasonic sonars. 2 | 3 | ![hc-sr04](hc_sr04.jpg) 4 | 5 | Publishes [sensor_msgs::Range](http://docs.ros.org/jade/api/sensor_msgs/html/msg/Range.html) messages at 10Hz to `/sonarN` topics. Specifically populates `range`. 6 | 7 | We include a node for a set of sonars, rather than a node per sonar, since we need to ensure only one sonar is measuring at a time (sonars can easily interfere with each other) 8 | 9 | on rasp pi 2 uses ~2% cpu and <1% mem for 3 sonars (capping at a distance of 20cm) 10 | 11 | REQUIRES running `src/export_gpio_pins.sh` to export pins otherwise need to run as root. (limitation of `wiringPiSetupSys`) 12 | 13 | ```` 14 | rostopic echo /sonar_0 15 | --- 16 | header: 17 | seq: 525 18 | stamp: 19 | secs: 1462946345 20 | nsecs: 463606605 21 | frame_id: '' 22 | radiation_type: 0 23 | field_of_view: 0.0 24 | min_range: 0.0 25 | max_range: 20.0 26 | range: 7.08620691299 27 | --- 28 | ```` 29 | 30 | TODOS 31 | * make hard coded pins in `hc_sr04_node.cpp` configurable 32 | * need some analysis on `field_of_view` -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hc_sr04 4 | 1.0.0 5 | The hc_sr04 package 6 | 7 | 8 | 9 | 10 | mat 11 | 12 | 13 | 14 | 15 | MIT 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | roscpp 41 | roscpp 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/hc_sr04_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace hc_sr04_node { 11 | 12 | // Maximum distance reported. Values over this distance 13 | // report MAX_DISTANCE. TODO make this a property. 14 | const static float MAX_DISTANCE = 30; 15 | const static float DIST_SCALE = 58.0; 16 | const static float TRAVEL_TIME_MAX = MAX_DISTANCE * DIST_SCALE; 17 | 18 | class Sonar { 19 | public: 20 | Sonar(int t, int e) : trigger_(t), echo_(e) { 21 | pinMode(trigger_, OUTPUT); 22 | pinMode(echo_, INPUT); 23 | digitalWrite(trigger_, LOW); 24 | delay(1000); 25 | } 26 | 27 | float distance(bool* error) { 28 | // Send trig pulse 29 | digitalWrite(trigger_, HIGH); 30 | delayMicroseconds(20); 31 | digitalWrite(trigger_, LOW); 32 | 33 | // Wait for echo. Very rarely (2 of 12K at 20Hz) 34 | // see ECHO never go HIGH so we include a way to 35 | // bail. 36 | int bail = 1000; 37 | while(digitalRead(echo_) == LOW) { 38 | if (--bail == 0) { 39 | *error = true; 40 | return 0; 41 | } 42 | } 43 | 44 | // Measure time for echo. Return early if the 45 | // pulse is appearing to take too long. Note: 46 | // error case of never going LOW results in 47 | // MAX reading :/ 48 | long startTime = micros(); 49 | long travelTime = 0; 50 | while(digitalRead(echo_) == HIGH) { 51 | travelTime = micros() - startTime; 52 | if (travelTime > TRAVEL_TIME_MAX) { 53 | travelTime = TRAVEL_TIME_MAX; 54 | break; 55 | } 56 | delayMicroseconds(100); 57 | } 58 | 59 | // Return distance in cm 60 | *error = false; 61 | return travelTime / 58.0; 62 | } 63 | 64 | private: 65 | int trigger_; 66 | int echo_; 67 | }; 68 | 69 | } // namespace hc_sr04_node 70 | 71 | int main(int argc, char **argv) { 72 | 73 | // Start ROS node. 74 | ROS_INFO("Starting node"); 75 | ros::init(argc, argv, "hc_sr04s"); 76 | ros::NodeHandle node; 77 | ros::Rate rate(10); // 10 hz 78 | 79 | // Build N sonars. 80 | wiringPiSetupSys(); // uses gpio pin numbering 81 | // TODO: config these 82 | vector sonars; 83 | sonars.push_back(hc_sr04_node::Sonar(24, 25)); 84 | sonars.push_back(hc_sr04_node::Sonar(22, 23)); 85 | sonars.push_back(hc_sr04_node::Sonar(18, 27)); 86 | 87 | // Build a publisher for each sonar. 88 | vector sonar_pubs; 89 | for (int i = 0; i < sonars.size(); ++i) { 90 | stringstream ss; 91 | ss << "sonar_" << i; 92 | sonar_pubs.push_back(node.advertise(ss.str(), 10)); 93 | } 94 | 95 | // Build base range message that will be used for 96 | // each time a msg is published. 97 | sensor_msgs::Range range; 98 | range.radiation_type = sensor_msgs::Range::ULTRASOUND; 99 | range.min_range = 0.0; 100 | range.max_range = hc_sr04_node::MAX_DISTANCE; 101 | 102 | float distance; 103 | bool error; 104 | while(ros::ok()) { 105 | for (int i = 0; i < sonars.size(); ++i) { 106 | range.header.stamp = ros::Time::now(); 107 | range.range = sonars[i].distance(&error); 108 | if (error) 109 | ROS_WARN("Error on sonar %d", i); 110 | else 111 | sonar_pubs[i].publish(range); 112 | } 113 | ros::spinOnce(); 114 | rate.sleep(); 115 | } 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hc_sr04) 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 | sensor_msgs 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES hc_sr04 104 | # CATKIN_DEPENDS roscpp 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(hc_sr04 121 | # src/${PROJECT_NAME}/hc_sr04.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(hc_sr04 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | add_executable(hc_sr04_node src/hc_sr04_node.cpp) 131 | 132 | ## Add cmake target dependencies of the executable 133 | ## same as for the library above 134 | # add_dependencies(hc_sr04_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | target_link_libraries(hc_sr04_node 138 | ${catkin_LIBRARIES} 139 | wiringPi 140 | ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS hc_sr04 hc_sr04_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hc_sr04.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | --------------------------------------------------------------------------------