├── .gitignore ├── LICENSE ├── README.md ├── launch ├── __pycache__ │ ├── mapping_controller_launch.cpython-38.pyc │ ├── mapping_controller_launch_prob.cpython-38.pyc │ └── mapping_controller_with_tools.cpython-38.pyc └── mapping_controller_launch.py ├── mapping_controller ├── __init__.py ├── mission_controller.py ├── probability_mapper.py └── random_bounce.py ├── package.xml ├── resource ├── mapping_controller ├── prob_map.png └── random_bounce.yaml ├── setup.cfg ├── setup.py └── test ├── __pycache__ ├── test_copyright.cpython-38-PYTEST.pyc ├── test_flake8.cpython-38-PYTEST.pyc └── test_pep257.cpython-38-PYTEST.pyc ├── test_copyright.py ├── test_flake8.py └── test_pep257.py /.gitignore: -------------------------------------------------------------------------------- 1 | /.vscode 2 | /log -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mapping_controller 2 | Webots ROS2 Mapping Controller 3 | 4 | ## Description 5 | The Mapping Controller package starts e-puck robot on Webots Simulator to bounce for mapping the environment. 6 | The maps can be the simple mapping from webots_ros2 or probability mapping. 7 | The maps will be saved at the given path. 8 | 9 | ## Build 10 | The following instructions assume that a ROS2 workspace is already set up. 11 | ```commandline 12 | rosdep update 13 | rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y 14 | colcon build --packages-select mapping_controller 15 | . install/setup.bash 16 | ``` 17 | 18 | ## Execution 19 | After building and installing the package, you can launch the simulation. 20 | 21 | ### Simple Mapping with rviz2 22 | ```commandline 23 | ros2 launch mapping_controller mapping_controller_launch.py mapper:=true rviz:=true mapping_time:=3 24 | ``` 25 | mapping_time is the time for the robot to bounce in minute. 26 | 27 | ### Probability Mapping with rviz2 28 | ```commandline 29 | ros2 launch mapping_controller mapping_controller_launch.py probability:=true rviz:=true mapping_time:=3 30 | ``` 31 | 32 | ### Specifying path 33 | By default the map is saved at home directory, to change it use path parameter. 34 | ```commandline 35 | ros2 launch mapping_controller mapping_controller_launch.py probability:=true rviz:=true mapping_time:=3 path:=path:="/home/$USER/Documents" 36 | ``` 37 | 38 | ### Map File Example 39 | the map file looks like: 40 | 41 | ![alt text](resource/prob_map.png) 42 | 43 | ## Dependencies 44 | Webots ROS2 package 45 | 46 | ## Documentation 47 | If you're interested in understanding the details, please read my post [here](https://towardsdatascience.com/occupancy-grid-mapping-with-webots-and-ros2-a6162a644fab) 48 | 49 | ## Issues 50 | Please report issues if you found bugs or raise a Pull Request. 51 | -------------------------------------------------------------------------------- /launch/__pycache__/mapping_controller_launch.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/launch/__pycache__/mapping_controller_launch.cpython-38.pyc -------------------------------------------------------------------------------- /launch/__pycache__/mapping_controller_launch_prob.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/launch/__pycache__/mapping_controller_launch_prob.cpython-38.pyc -------------------------------------------------------------------------------- /launch/__pycache__/mapping_controller_with_tools.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/launch/__pycache__/mapping_controller_with_tools.cpython-38.pyc -------------------------------------------------------------------------------- /launch/mapping_controller_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | from launch.substitutions import LaunchConfiguration 8 | from launch.actions import IncludeLaunchDescription 9 | from ament_index_python.packages import get_package_share_directory 10 | from pathlib import Path 11 | 12 | 13 | def generate_launch_description(): 14 | package_dir = get_package_share_directory('webots_ros2_epuck') 15 | 16 | use_probability = LaunchConfiguration('probability', default=False) 17 | use_nav = LaunchConfiguration('nav', default=False) 18 | use_rviz = LaunchConfiguration('rviz', default=False) 19 | use_mapper = LaunchConfiguration('mapper', default=False) 20 | synchronization = LaunchConfiguration('synchronization', default=False) 21 | fill_map = LaunchConfiguration('fill_map', default=True) 22 | use_sim_time = LaunchConfiguration('use_sim_time', default=True) 23 | world = LaunchConfiguration('world', default='epuck_world.wbt') 24 | mapping_time = LaunchConfiguration('mapping_time', default=5) 25 | map_path = LaunchConfiguration('path', default=str(Path.home())) 26 | 27 | # Webots 28 | webots_launch = IncludeLaunchDescription( 29 | PythonLaunchDescriptionSource( 30 | os.path.join(package_dir, 'robot_launch.py') 31 | ), 32 | launch_arguments={ 33 | 'synchronization': synchronization, 34 | 'use_sim_time': use_sim_time 35 | }.items() 36 | ) 37 | 38 | # Base configuration 39 | tools_launch = IncludeLaunchDescription( 40 | PythonLaunchDescriptionSource( 41 | os.path.join(package_dir, 'robot_tools_launch.py') 42 | ), 43 | launch_arguments={ 44 | 'nav': use_nav, 45 | 'rviz': use_rviz, 46 | 'mapper': use_mapper, 47 | 'use_sim_time': use_sim_time, 48 | 'world': world 49 | }.items() 50 | ) 51 | 52 | return LaunchDescription([ 53 | webots_launch, 54 | tools_launch, 55 | Node( 56 | package='mapping_controller', 57 | executable='random_bounce', 58 | output='log' 59 | ), 60 | Node( 61 | package='mapping_controller', 62 | executable='mission_controller', 63 | output='screen', 64 | parameters=[{'use_probability': use_probability, 65 | 'mapping_time': mapping_time, 66 | 'mapper': use_mapper, 67 | 'path': map_path}] 68 | ), 69 | Node( 70 | package='mapping_controller', 71 | executable='probability_mapper', 72 | output='log', 73 | parameters=[{'use_sim_time': use_sim_time, 'fill_map': fill_map}], 74 | condition=launch.conditions.IfCondition(use_probability) 75 | ), 76 | ]) 77 | -------------------------------------------------------------------------------- /mapping_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/mapping_controller/__init__.py -------------------------------------------------------------------------------- /mapping_controller/mission_controller.py: -------------------------------------------------------------------------------- 1 | """Mission Controller 2 | Description: 3 | This ros2 node commands the robot bounce until timeout and save 4 | the map to the given path. 5 | License: 6 | Copyright 2021 Debby Nirwan 7 | Licensed under the Apache License, Version 2.0 (the "License"); 8 | you may not use this file except in compliance with the License. 9 | You may obtain a copy of the License at 10 | http://www.apache.org/licenses/LICENSE-2.0 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | """ 17 | import rclpy 18 | import threading 19 | import numpy 20 | import cv2 21 | import copy 22 | import os 23 | 24 | from rclpy.node import Node 25 | from nav_msgs.msg import OccupancyGrid 26 | from std_srvs.srv import SetBool 27 | from pathlib import Path 28 | 29 | UNOBSERVED = -1 30 | UNOCCUPIED = 0 31 | MINUTE = 60 32 | DEFAULT_MAPPING_TIME = 5 33 | MAX_CONFIDENCE = 100.0 34 | MIN_LIKELIHOOD_THRES = 50 35 | PNG_OCCUPIED = 0.0 36 | PNG_UNOCCUPIED = 255.0 37 | PNG_UNOBSERVED = 128.0 38 | 39 | 40 | class MissionController(Node): 41 | 42 | def __init__(self): 43 | super().__init__('mission_controller') 44 | 45 | self._start_bouncing_srv = self.create_client(SetBool, 46 | '/start_stop_bouncing') 47 | self._use_probability = self.declare_parameter('use_probability', 48 | False) 49 | self._use_mapper = self.declare_parameter('mapper', False) 50 | self._mapping_time = self.declare_parameter('mapping_time', 51 | DEFAULT_MAPPING_TIME) 52 | default_path = str(Path.home()) 53 | self._path = self.declare_parameter('path', default_path) 54 | self._bounce = True 55 | self._map_lock = threading.Lock() 56 | self._prob_map_lock = threading.Lock() 57 | self._occ_cache = [] 58 | self._occ_cache_prob = [] 59 | self._occ_cache_width = 0 60 | self._occ_cache_width_prob = 0 61 | 62 | if self._use_probability.value: 63 | self.create_subscription(OccupancyGrid, '/prob_map', 64 | self.occupancy_prob_grid_callback, 1) 65 | if self._use_mapper.value: 66 | self.create_subscription(OccupancyGrid, '/map', 67 | self.occupancy_grid_callback, 1) 68 | self.timer = self.create_timer(self._mapping_time.value * MINUTE, 69 | self.timer_callback) 70 | 71 | while not self._start_bouncing_srv.wait_for_service(timeout_sec=1.0): 72 | self.get_logger().info('service not available, waiting again...') 73 | request = SetBool.Request() 74 | request.data = self._bounce 75 | self._bounce = not self._bounce 76 | self._start_bouncing_srv.call_async(request) 77 | 78 | def timer_callback(self): 79 | request = SetBool.Request() 80 | request.data = self._bounce 81 | self._bounce = not self._bounce 82 | self._start_bouncing_srv.call_async(request) 83 | 84 | map_to_save_prob = [] 85 | map_to_save_max_likelihood = [] 86 | if self._use_probability.value: 87 | for occ in self._occ_cache_prob: 88 | occ_reversed = MAX_CONFIDENCE - occ 89 | confidence_reversed = int(occ_reversed/MAX_CONFIDENCE 90 | * PNG_UNOCCUPIED) 91 | map_to_save_prob.append(confidence_reversed) 92 | if confidence_reversed < MIN_LIKELIHOOD_THRES: 93 | map_to_save_max_likelihood.append(PNG_OCCUPIED) 94 | else: 95 | map_to_save_max_likelihood.append(confidence_reversed) 96 | 97 | prob_file = os.path.join(self._path.value, 'prob_map.png') 98 | self.get_logger().info(f"saving prob map to: {prob_file}") 99 | cv2.imwrite(prob_file, 100 | numpy.reshape(numpy.array(map_to_save_prob), 101 | (-1, self._occ_cache_width_prob))) 102 | 103 | max_likelihood_file = os.path.join(self._path.value, 104 | 'prob_map_max_likelihood.png') 105 | self.get_logger().info( 106 | f"saving max likelihood map to: {max_likelihood_file}") 107 | cv2.imwrite(max_likelihood_file, 108 | numpy.reshape(numpy.array(map_to_save_max_likelihood), 109 | (-1, self._occ_cache_width_prob))) 110 | 111 | map_to_save = [] 112 | if self._use_mapper.value: 113 | for occ in self._occ_cache: 114 | if occ == UNOBSERVED: 115 | map_to_save.append(int(PNG_UNOBSERVED)) 116 | elif occ == UNOCCUPIED: 117 | map_to_save.append(int(PNG_UNOCCUPIED)) 118 | else: 119 | map_to_save.append(int(PNG_OCCUPIED)) 120 | map_file = os.path.join(self._path.value, 'map.png') 121 | self.get_logger().info(f"saving map to: {map_file}") 122 | cv2.imwrite(map_file, 123 | numpy.reshape(numpy.array(map_to_save), 124 | (-1, self._occ_cache_width))) 125 | 126 | self.timer.cancel() 127 | self.get_logger().info("mapping completed") 128 | 129 | def occupancy_grid_callback(self, msg): 130 | self._map_lock.acquire() 131 | self._occ_cache = copy.deepcopy(msg.data) 132 | self._occ_cache_width = msg.info.width 133 | self._map_lock.release() 134 | 135 | def occupancy_prob_grid_callback(self, msg): 136 | self._prob_map_lock.acquire() 137 | self._occ_cache_prob = copy.deepcopy(msg.data) 138 | self._occ_cache_width_prob = msg.info.width 139 | self._prob_map_lock.release() 140 | 141 | 142 | def main(args=None): 143 | rclpy.init(args=args) 144 | mission_controller = MissionController() 145 | rclpy.spin(mission_controller) 146 | mission_controller.destroy_node() 147 | rclpy.shutdown() 148 | 149 | 150 | if __name__ == '__main__': 151 | main() 152 | -------------------------------------------------------------------------------- /mapping_controller/probability_mapper.py: -------------------------------------------------------------------------------- 1 | """Probability Mapper 2 | Description: 3 | This ros2 node gets the laser scan readings and create and publish 4 | a probability occupancy map. 5 | License: 6 | Copyright 2021 Debby Nirwan 7 | Licensed under the Apache License, Version 2.0 (the "License"); 8 | you may not use this file except in compliance with the License. 9 | You may obtain a copy of the License at 10 | http://www.apache.org/licenses/LICENSE-2.0 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | """ 17 | import rclpy 18 | import numpy as np 19 | import threading 20 | 21 | from rclpy.node import Node 22 | from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile 23 | from nav_msgs.msg import OccupancyGrid 24 | from sensor_msgs.msg import LaserScan 25 | from geometry_msgs.msg import TransformStamped 26 | from tf2_ros import StaticTransformBroadcaster, TransformListener, Buffer 27 | from tf2_ros import LookupException, ConnectivityException 28 | from tf2_ros import ExtrapolationException 29 | from scipy.stats import norm 30 | from scipy.interpolate import interp1d 31 | from math import sin, cos, atan2, floor, degrees, isnan 32 | 33 | 34 | ROBOT_DIAMETER = 7 # cm 35 | WORLD_WIDTH = 3 # m 36 | WORLD_HEIGHT = 3 # m 37 | RESOLUTION = 0.01 # 1 cm 38 | 39 | WORLD_ORIGIN_X = - WORLD_WIDTH / 2.0 40 | WORLD_ORIGIN_Y = - WORLD_HEIGHT / 2.0 41 | MAP_WIDTH = int(WORLD_WIDTH / RESOLUTION) 42 | MAP_HEIGHT = int(WORLD_HEIGHT / RESOLUTION) 43 | 44 | MIN_PROB = 0.01 45 | MAX_PROB = 0.99 46 | INITIAL_PROBABILITY = 0.50 47 | L0 = 0.10 48 | 49 | INFRARED_MAX = 0.07 50 | TOF_MAX = 2.00 51 | 52 | INFRARED_READING = [ 53 | 0.000, 54 | 0.005, 55 | 0.010, 56 | 0.015, 57 | 0.020, 58 | 0.030, 59 | 0.040, 60 | 0.050, 61 | 0.060, 62 | 0.070 63 | ] 64 | 65 | INFRARED_STD_DEV = [ 66 | 0.000, 67 | 0.003, 68 | 0.007, 69 | 0.0406, 70 | 0.01472, 71 | 0.0241, 72 | 0.0287, 73 | 0.04225, 74 | 0.03065, 75 | 0.04897 76 | ] 77 | 78 | TOF_READING = [ 79 | 0.00, 80 | 0.05, 81 | 0.10, 82 | 0.20, 83 | 0.50, 84 | 1.00, 85 | 1.70, 86 | 2.00 87 | ] 88 | 89 | TOF_STD_DEV = [ 90 | 0.126, 91 | 0.032, 92 | 0.019, 93 | 0.009, 94 | 0.007, 95 | 0.010, 96 | 0.013, 97 | 0.000 98 | ] 99 | 100 | 101 | class ProbabilityMapper(Node): 102 | def __init__(self, name): 103 | super().__init__(name) 104 | 105 | self._infra_red_interpolation = interp1d(INFRARED_READING, 106 | INFRARED_STD_DEV) 107 | self._tof_interpolation = interp1d(TOF_READING, TOF_STD_DEV) 108 | self._map_lock = threading.Lock() 109 | 110 | fill_map_param = self.declare_parameter('fill_map', True) 111 | 112 | # Init map related elements 113 | self._probability_map = [self._log_odd(INITIAL_PROBABILITY)] \ 114 | * MAP_WIDTH * MAP_HEIGHT 115 | self.map_publisher = self.create_publisher( 116 | OccupancyGrid, 117 | '/prob_map', 118 | qos_profile=QoSProfile( 119 | depth=1, 120 | durability=DurabilityPolicy.TRANSIENT_LOCAL, 121 | history=HistoryPolicy.KEEP_LAST, 122 | ) 123 | ) 124 | self.tf_publisher = StaticTransformBroadcaster(self) 125 | tf = TransformStamped() 126 | tf.header.stamp = self.get_clock().now().to_msg() 127 | tf.header.frame_id = 'map' 128 | tf.child_frame_id = 'odom' 129 | tf.transform.translation.x = 0.0 130 | tf.transform.translation.y = 0.0 131 | tf.transform.translation.z = 0.0 132 | self.tf_publisher.sendTransform(tf) 133 | 134 | # Init laser related elements 135 | if fill_map_param.value: 136 | self.tf_buffer = Buffer() 137 | self.tf_listener = TransformListener(self.tf_buffer, self) 138 | self.scanner_subscriber = self.create_subscription(LaserScan, 139 | '/scan', 140 | self.update_map, 141 | 1) 142 | 143 | # Publish initial map 144 | self.publish_map() 145 | 146 | # Publish map every second 147 | self.create_timer(1, self.publish_map) 148 | 149 | def publish_map(self): 150 | self._map_lock.acquire() 151 | now = self.get_clock().now() 152 | prob_map = [-1] * MAP_WIDTH * MAP_HEIGHT 153 | idx = 0 154 | for cell in self._probability_map: 155 | cell_prob = round(self._prob(cell), 2) 156 | prob_map[idx] = int(cell_prob*100) 157 | idx += 1 158 | 159 | msg = OccupancyGrid() 160 | msg.header.stamp = now.to_msg() 161 | msg.header.frame_id = 'map' 162 | msg.info.resolution = RESOLUTION 163 | msg.info.width = MAP_WIDTH 164 | msg.info.height = MAP_HEIGHT 165 | msg.info.origin.position.x = WORLD_ORIGIN_X 166 | msg.info.origin.position.y = WORLD_ORIGIN_Y 167 | msg.data = prob_map 168 | self.map_publisher.publish(msg) 169 | self._map_lock.release() 170 | 171 | def update_map(self, msg): 172 | if self._map_lock.locked(): 173 | return 174 | else: 175 | self._map_lock.acquire() 176 | 177 | # Determine transformation of laser and robot in respect to odometry 178 | laser_rotation = None 179 | laser_translation = None 180 | try: 181 | tf = self.tf_buffer.lookup_transform('odom', 182 | msg.header.frame_id, 183 | msg.header.stamp) 184 | q = tf.transform.rotation 185 | laser_rotation = atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * 186 | (q.y * q.y + q.z * q.z)) 187 | laser_translation = tf.transform.translation 188 | except (LookupException, ConnectivityException, 189 | ExtrapolationException): 190 | self._map_lock.release() 191 | return 192 | 193 | # Determine position of robot and laser 194 | # in map coordinate frame (in meter) 195 | world_robot_x = laser_translation.x + WORLD_ORIGIN_X # robot's center 196 | world_robot_y = laser_translation.y + WORLD_ORIGIN_Y # robot's center 197 | world_laser_xs = [] 198 | world_laser_ys = [] 199 | world_laser_prob = [] 200 | laser_range_angle = msg.angle_min + laser_rotation 201 | sensor_angle = msg.angle_min 202 | for laser_range in msg.ranges: 203 | if laser_range < msg.range_max and laser_range > msg.range_min: 204 | world_laser_x = world_robot_x + laser_range * \ 205 | cos(laser_range_angle) 206 | world_laser_y = world_robot_y + laser_range * \ 207 | sin(laser_range_angle) 208 | tof = (round(degrees(sensor_angle)) == 0) 209 | if tof and laser_range > TOF_MAX: 210 | laser_range = TOF_MAX 211 | elif not tof and laser_range > INFRARED_MAX: 212 | laser_range = INFRARED_MAX 213 | _, log_odd = self._get_prob(laser_range, tof) 214 | world_laser_xs.append(world_laser_x) 215 | world_laser_ys.append(world_laser_y) 216 | world_laser_prob.append(log_odd) 217 | laser_range_angle += msg.angle_increment 218 | sensor_angle += msg.angle_increment 219 | 220 | # Determine position on map (in cm) 221 | robot_x = int(world_robot_x / RESOLUTION) 222 | robot_y = int(world_robot_y / RESOLUTION) 223 | laser_xs = [] 224 | laser_ys = [] 225 | laser_prob = world_laser_prob 226 | for world_laser_x, world_laser_y in \ 227 | zip(world_laser_xs, world_laser_ys): 228 | laser_xs.append(int(world_laser_x / RESOLUTION)) 229 | laser_ys.append(int(world_laser_y / RESOLUTION)) 230 | 231 | # Fill the map based on known readings 232 | for laser_x, laser_y, prob in zip(laser_xs, laser_ys, laser_prob): 233 | index = laser_y * MAP_WIDTH + laser_x 234 | self.plot_bresenham_line(robot_x, laser_x, robot_y, laser_y, index) 235 | current_prob = self._probability_map[index] 236 | new_prob = current_prob + prob - self._log_odd(L0) 237 | self._probability_map[index] = new_prob 238 | 239 | # robot footprint 240 | start_x = robot_x - (floor(ROBOT_DIAMETER/2)) 241 | start_y = robot_y - (floor(ROBOT_DIAMETER/2)) 242 | for x in range(ROBOT_DIAMETER): 243 | for y in range(ROBOT_DIAMETER): 244 | index = (start_y + y) * MAP_WIDTH + (start_x + x) 245 | if (index >= 0 and index < len(self._probability_map)) or \ 246 | (index < 0 and index >= -len(self._probability_map)): 247 | self._probability_map[index] = self._log_odd(MIN_PROB) 248 | self._map_lock.release() 249 | 250 | def plot_bresenham_line(self, x0, x1, y0, y1, idx): 251 | # Bresenham's line algorithm 252 | # (https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm) 253 | dx = abs(x1 - x0) 254 | sx = 1 if x0 < x1 else -1 255 | dy = -abs(y1 - y0) 256 | sy = 1 if y0 < y1 else -1 257 | err = dx + dy 258 | while True: 259 | index = y0 * MAP_WIDTH + x0 260 | if index != idx: 261 | current_prob = self._probability_map[index] 262 | new_prob = current_prob + self._log_odd(MIN_PROB) - \ 263 | self._log_odd(L0) 264 | self._probability_map[index] = new_prob 265 | if x0 == x1 and y0 == y1: 266 | break 267 | e2 = 2 * err 268 | if e2 >= dy: 269 | err += dy 270 | x0 += sx 271 | if e2 <= dx: 272 | err += dx 273 | y0 += sy 274 | 275 | @staticmethod 276 | def _prob(log_odd: float) -> float: 277 | result = np.exp(log_odd) / (1.0 + np.exp(log_odd)) 278 | if isnan(result): 279 | result = 0.0 280 | return result 281 | 282 | @staticmethod 283 | def _log_odd(prob: float) -> float: 284 | return np.log(prob / (1.0 - prob)) 285 | 286 | def _get_mean_and_std_dev(self, reading: float, tof: bool) -> tuple: 287 | assert reading >= 0.0, f"reading={reading}" 288 | 289 | if tof: 290 | lookup_table = self._tof_interpolation 291 | else: 292 | lookup_table = self._infra_red_interpolation 293 | 294 | return reading, lookup_table(reading) 295 | 296 | def _get_prob(self, reading: float, tof: bool) -> tuple: 297 | mean, std_dev = self._get_mean_and_std_dev(reading, tof) 298 | normal_dist = norm(loc=mean, scale=std_dev) 299 | probability = normal_dist.pdf(reading) 300 | 301 | if probability >= 1.0: 302 | probability = MAX_PROB 303 | elif probability == 0: 304 | probability = MIN_PROB 305 | 306 | log_odd = self._log_odd(probability) 307 | 308 | return probability, log_odd 309 | 310 | 311 | def main(args=None): 312 | rclpy.init(args=args) 313 | epuck_mapper = ProbabilityMapper('probability_mapper') 314 | rclpy.spin(epuck_mapper) 315 | epuck_mapper.destroy_node() 316 | rclpy.shutdown() 317 | 318 | 319 | if __name__ == '__main__': 320 | main() 321 | -------------------------------------------------------------------------------- /mapping_controller/random_bounce.py: -------------------------------------------------------------------------------- 1 | """Random Bounce 2 | Description: 3 | This ros2 node once commanded to start will control the robot 4 | to bounce until commanded to stop. 5 | License: 6 | Copyright 2021 Debby Nirwan 7 | Licensed under the Apache License, Version 2.0 (the "License"); 8 | you may not use this file except in compliance with the License. 9 | You may obtain a copy of the License at 10 | http://www.apache.org/licenses/LICENSE-2.0 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | """ 17 | import rclpy 18 | import threading 19 | import random 20 | import copy 21 | import yaml 22 | import os 23 | 24 | from rclpy.node import Node 25 | from geometry_msgs.msg import Twist, Vector3 26 | from sensor_msgs.msg import LaserScan 27 | from math import degrees, radians 28 | from std_srvs.srv import SetBool 29 | from ament_index_python.packages import get_package_share_directory 30 | 31 | MILLISECONDS = 0.001 32 | ONE_SECOND_PERIOD = 5 33 | ROBOT_DIAMETER = 0.07 34 | ROBOT_RADIUS = ROBOT_DIAMETER / 2.0 35 | 36 | TOF_MAX_READING = 2.000 37 | TOF_MIN_READING = 0.005 38 | 39 | IR_MAX_READING = 0.070 40 | IR_MIN_READING = 0.005 41 | 42 | 43 | class RandomBounce(Node): 44 | 45 | def __init__(self): 46 | super().__init__('random_bounce') 47 | self._velocity_cmd = self.create_publisher(Twist, '/cmd_vel', 10) 48 | self.create_subscription(LaserScan, '/scan', 49 | self.laser_scan_callback, 1) 50 | self.srv = self.create_service(SetBool, '/start_stop_bouncing', 51 | self.start_stop) 52 | self.timer = self.create_timer(200 * MILLISECONDS, self.timer_callback) 53 | self.velocity_lock = threading.Lock() 54 | self.velocity = Twist() 55 | self.prev_velocity = Twist() 56 | self.time = 0 57 | self.turn_right = True 58 | self.change_turn = False 59 | self.enabled = False 60 | self.speed_zero = Vector3(x=0.0, y=0.0, z=0.0) 61 | self.twist_zero = Twist(linear=self.speed_zero, 62 | angular=self.speed_zero) 63 | self.stopped = False 64 | self.config = None 65 | package_dir = get_package_share_directory('mapping_controller') 66 | cfg_file = 'resource/random_bounce.yaml' 67 | with open(os.path.join(package_dir, cfg_file)) as cfg_file: 68 | self.config = yaml.load(cfg_file, Loader=yaml.FullLoader) 69 | 70 | def start_stop(self, request, response): 71 | self.velocity_lock.acquire() 72 | self.enabled = request.data 73 | self.velocity_lock.release() 74 | response.success = True 75 | 76 | return response 77 | 78 | def timer_callback(self): 79 | self.velocity_lock.acquire() 80 | velocity = self.twist_zero 81 | if self.enabled: 82 | velocity = copy.deepcopy(self.velocity) 83 | else: 84 | self.time = 0 85 | self.velocity_lock.release() 86 | 87 | if (velocity != self.prev_velocity): 88 | self._velocity_cmd.publish(velocity) 89 | self.prev_velocity = copy.deepcopy(velocity) 90 | if velocity == self.twist_zero: 91 | self.stopped = True 92 | else: 93 | self.stopped = False 94 | 95 | if self.time != 0: 96 | self.time -= 1 97 | 98 | def laser_scan_callback(self, msg): 99 | sensor_angle = msg.angle_min 100 | for laser_range in msg.ranges: 101 | tof = (round(degrees(sensor_angle)) == 0) 102 | if tof: 103 | max = TOF_MAX_READING + ROBOT_RADIUS 104 | min = TOF_MIN_READING + ROBOT_RADIUS 105 | else: 106 | max = IR_MAX_READING + ROBOT_RADIUS 107 | min = IR_MIN_READING + ROBOT_RADIUS 108 | if laser_range <= max and laser_range >= min: 109 | distance = abs(laser_range - ROBOT_RADIUS) 110 | if (distance <= self.config['object_distance']) and \ 111 | (abs(round(degrees(sensor_angle))) <= 112 | self.config['object_angle']): 113 | if self.time == 0: 114 | self.velocity_lock.acquire() 115 | if not self.stopped: 116 | self.velocity = copy.deepcopy(self.twist_zero) 117 | else: 118 | self.velocity.linear = Vector3(x=0.0, y=0.0, z=0.0) 119 | angular_spd = radians(self.config['angular_speed']) 120 | self.velocity.angular = Vector3(x=0.0, 121 | y=0.0, 122 | z=angular_spd) 123 | if not self.turn_right: 124 | self.velocity.angular = Vector3(x=0.0, 125 | y=0.0, 126 | z=-angular_spd) 127 | self.time = self.config['rotation_in_second'] * \ 128 | ONE_SECOND_PERIOD 129 | self.change_turn = random.choice([True, False]) 130 | self.velocity_lock.release() 131 | return 132 | sensor_angle += msg.angle_increment 133 | 134 | # not blocked 135 | if self.time == 0: 136 | self.velocity_lock.acquire() 137 | self.velocity.linear = Vector3(x=self.config['linear_speed'], 138 | y=0.0, z=0.0) 139 | self.velocity.angular = Vector3(x=0.0, y=0.0, z=0.0) 140 | self.velocity_lock.release() 141 | if self.change_turn: 142 | self.change_turn = False 143 | self.turn_right = random.choice([True, False]) 144 | 145 | 146 | def main(args=None): 147 | rclpy.init(args=args) 148 | random_bounce = RandomBounce() 149 | rclpy.spin(random_bounce) 150 | random_bounce.destroy_node() 151 | rclpy.shutdown() 152 | 153 | 154 | if __name__ == '__main__': 155 | main() 156 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mapping_controller 5 | 0.0.1 6 | Mapping Controller for epuck on Webots Simulator 7 | Debby Nirwan 8 | Apache License 2.0 9 | 10 | rclpy 11 | geometry_msgs 12 | sensor_msgs 13 | python3-opencv 14 | python-numpy 15 | python3-scipy 16 | webots_ros2 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | 23 | 24 | ament_python 25 | 26 | 27 | -------------------------------------------------------------------------------- /resource/mapping_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/resource/mapping_controller -------------------------------------------------------------------------------- /resource/prob_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/resource/prob_map.png -------------------------------------------------------------------------------- /resource/random_bounce.yaml: -------------------------------------------------------------------------------- 1 | linear_speed: 0.1 2 | angular_speed: 15 3 | rotation_in_second: 6 4 | object_distance: 0.05 5 | object_angle: 90 6 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/mapping_controller 3 | [install] 4 | install-scripts=$base/lib/mapping_controller 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'mapping_controller' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.1', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ('share/' + package_name, ['launch/mapping_controller_launch.py']), 14 | ('share/' + package_name + '/resource', 15 | ['resource/random_bounce.yaml']), 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | author='Debby Nirwan', 20 | author_email='debby_nirwan@yahoo.com', 21 | maintainer='Debby Nirwan', 22 | maintainer_email='debby_nirwan@yahoo.com', 23 | keywords=['ROS2', 'Webots', 'Probability Mapper', 24 | 'Simulation', 'Examples'], 25 | classifiers=[ 26 | 'Intended Audience :: Developers', 27 | 'License :: OSI Approved :: Apache Software License', 28 | 'Programming Language :: Python', 29 | 'Topic :: Software Development', 30 | ], 31 | description='Mapping Controller for epuck on Webots Simulator', 32 | license='Apache License 2.0', 33 | tests_require=['pytest'], 34 | entry_points={ 35 | 'console_scripts': [ 36 | 'random_bounce = mapping_controller.random_bounce:main', 37 | 'mission_controller = mapping_controller.mission_controller:main', 38 | 'probability_mapper = mapping_controller.probability_mapper:main', 39 | ], 40 | }, 41 | ) 42 | -------------------------------------------------------------------------------- /test/__pycache__/test_copyright.cpython-38-PYTEST.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/test/__pycache__/test_copyright.cpython-38-PYTEST.pyc -------------------------------------------------------------------------------- /test/__pycache__/test_flake8.cpython-38-PYTEST.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/test/__pycache__/test_flake8.cpython-38-PYTEST.pyc -------------------------------------------------------------------------------- /test/__pycache__/test_pep257.cpython-38-PYTEST.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/debbynirwan/mapping_controller/88cb7fe32f3c37b8ef02d2dbb527db70d0f5e20c/test/__pycache__/test_pep257.cpython-38-PYTEST.pyc -------------------------------------------------------------------------------- /test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | --------------------------------------------------------------------------------