├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── package.xml └── src └── sonar_to_laserscan_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sonar_to_laserscan) 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 roscpp) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 29 | ## pulled in transitively but can be declared for certainty nonetheless: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################### 70 | ## catkin specific configuration ## 71 | ################################### 72 | ## The catkin_package macro generates cmake config files for your package 73 | ## Declare things to be passed to dependent projects 74 | ## INCLUDE_DIRS: uncomment this if you package contains header files 75 | ## LIBRARIES: libraries you create in this project that dependent projects also need 76 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 77 | ## DEPENDS: system dependencies of this project that dependent projects also need 78 | catkin_package( 79 | # INCLUDE_DIRS include 80 | # LIBRARIES sonar_to_laserscan 81 | CATKIN_DEPENDS roscpp 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | # include_directories(include) 92 | 93 | ## Declare a cpp library 94 | # add_library(sonar_to_laserscan 95 | # src/sonar_to_laserscan.cpp 96 | # ) 97 | 98 | ## Declare a cpp executable 99 | add_executable(sonar_to_laserscan_node src/sonar_to_laserscan_node.cpp) 100 | 101 | ## Add cmake target dependencies of the executable/library 102 | ## as an example, message headers may need to be generated before nodes 103 | # add_dependencies(sonar_to_laserscan_node sonar_to_laserscan_generate_messages_cpp) 104 | 105 | ## Specify libraries to link a library or executable target against 106 | target_link_libraries(sonar_to_laserscan_node 107 | ${catkin_LIBRARIES} 108 | ) 109 | 110 | ############# 111 | ## Install ## 112 | ############# 113 | 114 | # all install targets should use catkin DESTINATION variables 115 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 116 | 117 | ## Mark executable scripts (Python etc.) for installation 118 | ## in contrast to setup.py, you can choose the destination 119 | # install(PROGRAMS 120 | # scripts/my_python_script 121 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 122 | # ) 123 | 124 | ## Mark executables and/or libraries for installation 125 | # install(TARGETS sonar_to_laserscan sonar_to_laserscan_node 126 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark cpp header files for installation 132 | # install(DIRECTORY include/${PROJECT_NAME}/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # FILES_MATCHING PATTERN "*.h" 135 | # PATTERN ".svn" EXCLUDE 136 | # ) 137 | 138 | ## Mark other files for installation (e.g. launch and bag files, etc.) 139 | # install(FILES 140 | # # myfile1 141 | # # myfile2 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | # ) 144 | 145 | ############# 146 | ## Testing ## 147 | ############# 148 | 149 | ## Add gtest based cpp test target and link libraries 150 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sonar_to_laserscan.cpp) 151 | # if(TARGET ${PROJECT_NAME}-test) 152 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 153 | # endif() 154 | 155 | ## Add folders to be run by python nosetests 156 | # catkin_add_nosetests(test) 157 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | sonar-to-laserscan 2 | ================== 3 | 4 | ROS node to convert the amigobot sonar scan depthcloud data to laserscan data for use by amcl 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sonar_to_laserscan 4 | 0.0.0 5 | The sonar_to_laserscan package converts amigobot sonar scan depthcloud data to laserscan data for use by amcl. 6 | 7 | Sam 8 | 9 | Apache v2 license 10 | 11 | https://github.com/SquadCollaborativeRobotics/sonar_to_laserscan 12 | 13 | Sam Ansari 14 | 15 | catkin 16 | 17 | roscpp 18 | roscpp 19 | 20 | -------------------------------------------------------------------------------- /src/sonar_to_laserscan_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #define PI 3.14159265 7 | #define DEG_TO_RAD(x) (x * PI / 180.0) 8 | #define MAX_RANGE 3.0 // meters 9 | #define MIN_ANGLE -144 // degrees 10 | #define MAX_ANGLE 144 // degrees 11 | #define DEG_INCREMENT 2 // degrees 12 | #define NUM_READINGS ((MAX_ANGLE-MIN_ANGLE) / DEG_INCREMENT + 1) 13 | 14 | #define NUMBER_OF_SONAR_SENSORS 8 15 | #define magnitude(x,y) sqrt((x)*(x) + (y)*(y)) 16 | 17 | // Amigobot sonar directions in degrees 18 | // TODO : Move to cfg file 19 | // static int sonar_directions[] = {90, 44, 12, -12, -44, -90, -144, 144}; 20 | 21 | // p3dx sonar directions in degrees 22 | static int sonar_directions[] = {90, 67.5, 45, 22.5, -22.5, -45, -67.5, -90}; 23 | 24 | // Initialize sonar data with default values 25 | double sonar_data[NUMBER_OF_SONAR_SENSORS] = {1, 1, 1, 1, 1, 1, 1, 1}; 26 | ros::Time sonar_timestamp; 27 | 28 | // -144 to 144 degrees in 2 deg increments, plus 1 to include angle max in array 29 | 30 | // 25 Hz for sonar 31 | double sonar_frequency = 25; 32 | 33 | double ranges[NUM_READINGS]; 34 | double intensities[NUM_READINGS]; 35 | 36 | ros::Publisher scan_pub; 37 | 38 | // Transform prefix, set as param 39 | std::string tf_prefix; 40 | 41 | // Uses global sonar_data to generate laserscan data and publish 42 | // Uses global sonar_timestamp to generate timestamp 43 | void publishLaserFromSonar() { 44 | // Generate empty data outside of the range for our laser scan 45 | for(unsigned int i = 0; i < NUM_READINGS; ++i){ 46 | ranges[i] = MAX_RANGE + 1.0; 47 | intensities[i] = 0; 48 | } 49 | 50 | // Generate actual sonar data in correct positions 51 | for (unsigned int i = 0; i < NUMBER_OF_SONAR_SENSORS; ++i) 52 | { 53 | // Place sonar data into correct position in ranges array 54 | int index = (sonar_directions[i] - MIN_ANGLE)/DEG_INCREMENT; 55 | ranges[index] = sonar_data[i]; 56 | intensities[index] = 0.5; 57 | } 58 | 59 | // Populate the LaserScan message 60 | sensor_msgs::LaserScan scan; 61 | scan.header.stamp = sonar_timestamp; 62 | scan.header.frame_id = tf_prefix + "/base_link"; 63 | scan.angle_min = DEG_TO_RAD(MIN_ANGLE); 64 | scan.angle_max = DEG_TO_RAD(MAX_ANGLE); 65 | scan.angle_increment = DEG_TO_RAD(DEG_INCREMENT); 66 | scan.time_increment = (1.0 / sonar_frequency) / (NUM_READINGS); 67 | scan.range_min = 0.0; 68 | scan.range_max = MAX_RANGE; 69 | 70 | scan.ranges.resize(NUM_READINGS); 71 | scan.intensities.resize(NUM_READINGS); 72 | for(unsigned int i = 0; i < NUM_READINGS; ++i){ 73 | scan.ranges[i] = ranges[i]; 74 | scan.intensities[i] = intensities[i]; 75 | } 76 | scan_pub.publish(scan); 77 | } 78 | 79 | // Store most recent sonar data 80 | void sonarCallback(const sensor_msgs::PointCloud::ConstPtr& msg){ 81 | // Extract distances from depth cloud 82 | for (unsigned int i = 0; i < NUMBER_OF_SONAR_SENSORS; ++i) { 83 | sonar_data[i] = magnitude(msg->points[i].x, msg->points[i].y); 84 | } 85 | // Get time of scan 86 | sonar_timestamp = msg->header.stamp; 87 | 88 | publishLaserFromSonar(); 89 | } 90 | 91 | 92 | int main(int argc, char** argv){ 93 | // Make sure that the number of values in sonar_direction matches number of sensors 94 | assert(sizeof(sonar_directions)/sizeof(int) == NUMBER_OF_SONAR_SENSORS); 95 | 96 | 97 | ros::init(argc, argv, "laser_scan_publisher"); 98 | ros::NodeHandle n; 99 | 100 | // Get TF prefix 101 | tf_prefix = ""; 102 | if (n.getParam("tf_prefix", tf_prefix)) 103 | ROS_INFO_STREAM("Read tf prefix: " << tf_prefix); 104 | else 105 | ROS_ERROR_STREAM("Did not read tf_prefix: default = " << tf_prefix); 106 | 107 | // Publish generated laserscan data with a queue size of 50 108 | scan_pub = n.advertise("scan", 50); 109 | 110 | // Subscribe to sonar data with queue size of 100 111 | ros::Subscriber goal_sub = n.subscribe("p3_node/sonar", 100, sonarCallback); 112 | 113 | ros::spin(); 114 | } --------------------------------------------------------------------------------