diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..bb834c8 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,13 @@ +Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` line to commit messages to certify that they have the right to submit the code they are contributing to the project according to the [Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f49a4e1 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/README.md b/README.md index d3dc0af..9e3c94e 100644 --- a/README.md +++ b/README.md @@ -1 +1,61 @@ -# Turtlebot Mapper \ No newline at end of file +# Turtlebot Mapper + +The package's goal is to use the robot [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) with a laser scan in a partially known environment to count the number of obstacles in this environment. The proposed solution aims to be as generic as possible. Given a closed environment, the robot must continue exploring until the number of frontier points (intersection between unknown regions and free regions) reaches a threshold. + +* [turtlebot3_occupancy_grid](turtlebot3_mapper/turtlebot3_occupancy_grid/turtlebot3_occupancy_grid.py): it subscribes to receive messages from the laser sensor and updates the occupancy grid map for each message received. Initially, all points on the occupancy map have probability equal to 50%. As messages are received from the laser sensor, occupied points will have probability above 50% and free points on the map will have probability below 50%. This probabilistic occupancy grid is published at a fixed rate in the `custom_map` topic. + +* [turtlebot3_object_detector](turtlebot3_mapper/turtlebot3_object_detector/turtlebot3_object_detector.py): it subscribes to receive messages from `turtlebot3_occupancy_grid` with the probabilistic occupancy grid. For each message received, the map is segmented into a threshold ensuring that only points with a probability greater than this threshold are 1. Then, [OpenCV's connected components](https://docs.opencv.org/3.4/d3/dc0/group__imgproc__shape.html#ga107a78bf7cd25dec05fb4dfc5c9e765f) approach is used these determine occupied regions. If this region's area is between a minimum and maximum value, then it publish the [BoundingBox2DArray](http://docs.ros.org/en/api/vision_msgs/html/msg/BoundingBox2DArray.html) and an [Image](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html) with everything rendered to visualize the results. + +* [turtlebot3_explorer](turtlebot3_mapper/turtlebot3_explorer/turtlebot3_explorer.py): it subscribes to receive messages from the laser sensor and publishes velocity commands. If any obstacle is detected in front of the robot, it then rotates until it finds a free path again. Also has a service that allows to enable or disable this behavior. + +* [turtlebot3_mission_controller](turtlebot3_mapper/turtlebot3_mission_controller/turtlebot3_mission_controller.py): the main service of this node is an action server responsible for determining when the exploration starts and ends. To do so, it uses the concept of fronteirs (intersection between unknown regions and free regions). Through the goal sent to the action server, where a user can specify the number of remaining fronteir points, it will enable the `turtlebot3_explorer` to explore the enviromment until it reaches the specified numbers of frontiers points in the occupancy grid. + +* [turtlebot3_mission_client](turtlebot3_mapper/turtlebot3_mission_client/turtlebot3_mission_client.py): action client responsible for sending the exploration task to the action server and saving the results to a file. + +Below you can see a graph of the entire system, +![ROS graph](resource/rosgraph.png) + + +## Building package + +If you don't already have a workspace where you want to add this package, then create one: +```bash +mkdir -p ros2_ws/src +cd ros2_ws +``` + +Then, add this repository in `src` folder. Note that you also need to add a repository with custom action that it is used here. +```bash +cd ./src +git clone git@github.com:autonomous-robots/turtlebot3_interfaces.git +git clone git@github.com:autonomous-robots/turtlebot3_mapper.git +cd .. +``` + +Now build all packages: +```bash +colcon build +``` + +### Running +For each terminal you open, remember to run: +```bash +source install/setup.bash +``` + +To run all nodes (with the exception of the `turtlebot3_mission_client`), run the following launch script: +```bash +ros2 launch turtlebot3_mapper turtlebot3_mapper_launch.py +``` + +Then, open a new terminal and make the environment exploration request: +```bash +source install/setup.bash +ros2 run turtlebot3_mapper turtlebot3_mission_client -f 200 +``` + +## References + +* [AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics), python sample codes for robotics algorithms; +* [ROBOTIS-GIT/turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3), ROS packages for Turtlebot3; +* [ROBOTIS-GIT/turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations), simulations for TurtleBot3; \ No newline at end of file diff --git a/launch/turtlebot3_mapper_launch.py b/launch/turtlebot3_mapper_launch.py new file mode 100644 index 0000000..9649836 --- /dev/null +++ b/launch/turtlebot3_mapper_launch.py @@ -0,0 +1,166 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + turtlebot3_mapper_rviz_dir = os.path.join( + get_package_share_directory('turtlebot3_mapper'), + 'rviz', + ) + turtlebot3_gazebo_launch_dir = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'launch', + ) + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='-2.0') + y_pose = LaunchConfiguration('y_pose', default='-0.5') + turtlebot3_world_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + turtlebot3_gazebo_launch_dir, + 'turtlebot3_world.launch.py', + ), + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose, + 'use_sim_time': use_sim_time, + }.items(), + ) + nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') + rviz_config_file = LaunchConfiguration( + 'rviz_config_file', + default=os.path.join( + turtlebot3_mapper_rviz_dir, + 'config.rviz', + ), + ) + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + nav2_launch_file_dir, + 'rviz_launch.py', + )), + launch_arguments={'rviz_config': rviz_config_file}.items(), + ) + turtlebot3_occupancy_grid_node = Node( + package='turtlebot3_mapper', + namespace='', + executable='turtlebot3_occupancy_grid', + name='turtlebot3_occupancy_grid', + parameters=[ + { + "width": 8.0 + }, + { + "height": 8.0 + }, + { + "resolution": 0.03, + }, + { + "min_prob": 0.01 + }, + { + "max_prob": 0.99, + }, + { + "prob_occupied": 0.6, + }, + { + "prob_free": 0.4, + }, + { + "prob_priori": 0.5, + }, + ], + ) + turtlebot3_object_detector_node = Node( + package='turtlebot3_mapper', + namespace='', + executable='turtlebot3_object_detector', + name='turtlebot3_object_detector', + parameters=[ + { + "threshold": 150, + }, + { + "min_area": 30, + }, + { + "max_area": 100, + }, + { + "connectivity": 4, + }, + ], + ) + turtlebot3_explorer_node = Node( + package='turtlebot3_mapper', + namespace='', + executable='turtlebot3_explorer', + name='turtlebot3_explorer', + parameters=[ + { + "view_angle": 30.0, + }, + { + "min_rand": 10, + }, + { + "max_rand": 20, + }, + { + "safety_distance": 0.5, + }, + { + "linear_speed": 0.15, + }, + { + "angular_speed": 0.25, + }, + { + "update_rate": 0.5, + }, + ], + ) + turtlebot3_mission_controller_node = Node( + package='turtlebot3_mapper', + namespace='', + executable='turtlebot3_mission_controller', + name='turtlebot3_mission_controller', + parameters=[ + { + "free_limit": 30, + }, + ], + ) + ld = LaunchDescription() + ld.add_action(rviz_cmd) + ld.add_action(turtlebot3_world_cmd) + ld.add_action(turtlebot3_explorer_node) + ld.add_action(turtlebot3_occupancy_grid_node) + ld.add_action(turtlebot3_object_detector_node) + ld.add_action(turtlebot3_mission_controller_node) + return ld diff --git a/package.xml b/package.xml index eeee586..9a18ebe 100644 --- a/package.xml +++ b/package.xml @@ -3,20 +3,24 @@ turtlebot3_mapper 0.1.0 - Simple implementation of an occupancy map using laser scan + Set of packages developed to explore, map and detect obstacles in an enviromment autonomous-robots - MIT + Apache License 2.0 rclpy nav_msgs - sensors_msgs + vision_msgs + sensor_msgs + rcl_interfaces + tf2_ros + turtlebot3_interfaces python3-numpy ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_pycodestyle + ament_cmake + ament_python diff --git a/resource/rosgraph.png b/resource/rosgraph.png new file mode 100644 index 0000000..2f73ba1 Binary files /dev/null and b/resource/rosgraph.png differ diff --git a/rviz/config.rviz b/rviz/config.rviz new file mode 100644 index 0000000..dc8b8a4 --- /dev/null +++ b/rviz/config.rviz @@ -0,0 +1,268 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5323529243469238 + Tree Height: 719 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /custom_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /custom_map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rendered + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + wheel_left_link: + Value: true + wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.713586807250977 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000025e0000035afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065010000003d000001c90000002800fffffffb0000000a00560069006500770073010000020c0000018b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 27 diff --git a/setup.py b/setup.py index e2e828f..6478773 100644 --- a/setup.py +++ b/setup.py @@ -1,3 +1,5 @@ +from os import path +from glob import glob from setuptools import setup package_name = 'turtlebot3_mapper' @@ -5,23 +7,49 @@ setup( name=package_name, version='0.1.0', - packages=[package_name], + packages=[ + package_name, + path.join(package_name, "turtlebot3_explorer"), + path.join(package_name, "turtlebot3_occupancy_grid"), + path.join(package_name, "turtlebot3_object_detector"), + path.join(package_name, "turtlebot3_mission_controller"), + path.join(package_name, "turtlebot3_mission_client"), + ], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (path.join('share', package_name, 'launch'), glob('launch/*launch.[pxy][yma]*')), + (path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), ], install_requires=['setuptools'], zip_safe=True, - maintainer='autonomous-robots', - description='Simple implementation of an occupancy map using laser scan', - license='MIT', + author=["Luiz Carlos Cosmi Filho"], + author_email=["luizcarloscosmifilho@gmail.com"], + maintainer="Luiz Carlos Cosmi Filho", + maintainer_email="luizcarloscosmifilho@gmail.com", + keywords=['ROS', 'ROS2', 'mapping', 'perception', 'kinematics'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='Set of packages developed to explore, map and detect obstacles in an \ + enviromment', + license='Apache License, Version 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'mapper=turtlebot3_mapper.mapper:main', - 'viewer=turtlebot3_mapper.viewer:main', - 'explorer=turtlebot3_mapper.explorer:main', + 'turtlebot3_explorer = \ + turtlebot3_mapper.turtlebot3_explorer.main:main', + 'turtlebot3_occupancy_grid = \ + turtlebot3_mapper.turtlebot3_occupancy_grid.main:main', + 'turtlebot3_object_detector = \ + turtlebot3_mapper.turtlebot3_object_detector.main:main', + 'turtlebot3_mission_controller = \ + turtlebot3_mapper.turtlebot3_mission_controller.main:main', + 'turtlebot3_mission_client = \ + turtlebot3_mapper.turtlebot3_mission_client.main:main', ], }, ) diff --git a/turtlebot3_mapper/explorer.py b/turtlebot3_mapper/explorer.py deleted file mode 100644 index 483d94e..0000000 --- a/turtlebot3_mapper/explorer.py +++ /dev/null @@ -1,163 +0,0 @@ -import rclpy -import threading -import numpy as np - -from enum import Enum -from rclpy.node import Node -from rclpy.executors import ExternalShutdownException - -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Twist -from sensor_msgs.msg import LaserScan - -from turtlebot3_mapper.utils import euler_from_quaternion - - -class State(Enum): - FORWARD = 1 - ROTATE = 2 - UNKOWN = 3 - -class TurtlebotExplorer(Node): - - - def __init__(self, node_name: str = 'turtlebot_explorer'): - super(TurtlebotExplorer, - self).__init__(node_name=node_name) - self._subscriber_scan = self.create_subscription( - msg_type=LaserScan, - topic="/scan", - callback=self._scan_callback, - qos_profile=10, - ) - self._subscriber_odom = self.create_subscription( - msg_type=Odometry, - topic="/odom", - callback=self._odom_callback, - qos_profile=10, - ) - self._publisher = self.create_publisher( - msg_type=Twist, - topic="/cmd_vel", - qos_profile=10, - ) - self.update_timer = self.create_timer( - timer_period_sec=1, - callback=self.update_callback, - ) - self.view_angle = 90 - self._odom_init = False - self._scan_init = False - self.count = 0 - self.multiplier = 1.0 - self._update = threading.Lock() - - self.state = State.UNKOWN - self.get_logger().info("Init turtlebot_explorer") - - def _scan_callback(self, message: LaserScan): - if not self._update.locked(): - self._scan = message - self._scan_init = True - - def _odom_callback(self, message: Odometry): - self._odom = message - if not self._update.locked(): - self._odom = message - self._odom_init = True - - def update_callback(self): - if self._scan_init and self._odom_init: - if self._update.locked(): - return - else: - self._update.acquire() - self.get_logger().info("updating...") - self.detect_obstacle() - self._update.release() - - def detect_obstacle(self): - # unit: m - safety_distance = 0.5 - - _, _, theta = euler_from_quaternion(self._odom.pose.pose.orientation) - - min_ang = (-140) * (np.pi/180) - max_ang = (140) * (np.pi/180) - - increm = self._scan.angle_increment - size = len(self._scan.ranges) - - start = int(size/2 + min_ang/increm) - end = int(size/2 + max_ang/increm) - - # scan_range = [self.scan.ranges[i] for i in range(90, 270)] - scan_range = [self._scan.ranges[i] for i in range(end, len(self._scan.ranges))] - for i in range(0, start): - scan_range.append(self._scan.ranges[i]) - - obstacle_distance = min(scan_range) - - if self.state == State.UNKOWN: - if obstacle_distance < safety_distance: - if self.state != State.ROTATE: - twist = Twist() - twist.linear.x = 0.0 - twist.angular.z = 0.25*self.multiplier - if self.count >= 3: - self.count = 0 - self.multiplier *= -1.0 - self.count += 1 - self.state = State.ROTATE - self._publisher.publish(twist) - else: - self.state = State.FORWARD - twist = Twist() - twist.linear.x = 0.25 - twist.angular.z = 0.0 - self._publisher.publish(twist) - - elif self.state == State.FORWARD: - if obstacle_distance < safety_distance: - if self.state != State.ROTATE: - twist = Twist() - twist.linear.x = 0.0 - twist.angular.z = 0.25*self.multiplier - if self.count >= 3: - self.count = 0 - self.multiplier *= -1.0 - self.count += 1 - self.state = State.ROTATE - self._publisher.publish(twist) - else: - twist = Twist() - twist.linear.x = 0.25 - twist.angular.z = 0.0 - self.state = State.FORWARD - self._publisher.publish(twist) - - else: - if obstacle_distance < safety_distance: - pass - else: - twist = Twist() - twist.linear.x = 0.25 - twist.angular.z = 0.0 - self.state = State.FORWARD - self._publisher.publish(twist) - - -def main(*args, **kwargs): - rclpy.init(*args, **kwargs) - node = TurtlebotExplorer() - try: - rclpy.spin(node=node) - except (KeyboardInterrupt, ExternalShutdownException): - pass - finally: - node.destroy_node() - rclpy.try_shutdown() - - -if __name__ == "__main__": - main() diff --git a/turtlebot3_mapper/mapper.py b/turtlebot3_mapper/mapper.py deleted file mode 100644 index 9c348f2..0000000 --- a/turtlebot3_mapper/mapper.py +++ /dev/null @@ -1,240 +0,0 @@ -import rclpy -import threading -import numpy as np - -from rclpy.node import Node -from rclpy.executors import ExternalShutdownException - -from sensor_msgs.msg import LaserScan -from nav_msgs.msg import Odometry, OccupancyGrid, MapMetaData - -from tf2_ros.buffer import Buffer -from tf2_ros import LookupException, TransformException -from tf2_ros.transform_listener import TransformListener - -from turtlebot3_mapper.utils import euler_from_quaternion - -class TurtlebotMapper(Node): - - def __init__(self, - node_name: str = 'turtlebot_mapper', - width: int = 8, - height: int = 8, - resolution: float = 0.03): - - super(TurtlebotMapper, self).__init__(node_name=node_name) - self.width = width - self.height = height - self.resolution = resolution - - N = int(1/resolution) - shape = (width*N, height*N) - self.grid = -1*np.ones(shape, dtype=np.int8) - - self._scan_subscriber = self.create_subscription( - msg_type=LaserScan, - topic="/scan", - callback=self._scan_callback, - qos_profile=10, - ) - self._map_publisher = self.create_publisher( - msg_type=OccupancyGrid, - topic="/custom_map", - qos_profile=10, - ) - self._update_timer = self.create_timer( - timer_period_sec=0.1, - callback=self._update_callback, - ) - self._scan_init = False - self._tf_buffer = Buffer() - self._tf_listener = TransformListener(self._tf_buffer, self) - self._update = threading.Lock() - self.get_logger().info(f"Init {node_name}") - - def _scan_callback(self, message: LaserScan): - self._scan_init = True - if not self._update.locked(): - self._scan = message - - def _update_callback(self): - if self._scan_init: - if self._update.locked(): - return - else: - self._update.acquire() - self.get_logger().info("updating...") - self.update( - message_laser=self._scan, - ) - self._map_publisher.publish(self.occupancy_grid) - self._update.release() - - - def update(self, message_laser: LaserScan): - try: - tf = self._tf_buffer.lookup_transform('odom', - message_laser.header.frame_id, - message_laser.header.stamp) - q = tf.transform.rotation - except (TransformException, LookupException): - self.get_logger().info("could not get tf") - return - _, _, theta = euler_from_quaternion(quaternion=q) - if theta < 0.0: - theta += 2*np.pi - distances = self.scan_to_distances(message=message_laser) - xy = self.distances_to_xy( - distances=distances, - x=tf.transform.translation.x, - y=tf.transform.translation.y, - theta=theta, - ) - xy[:,0] = (xy[:,0] + self.width//2)/self.resolution - xy[:,1] = (xy[:,1] + self.height//2)/self.resolution - xy = xy.astype(int) - xo = int((tf.transform.translation.x + self.width//2)/self.resolution) - yo = int((tf.transform.translation.y + self.height//2)/self.resolution) - - for i in range(xy.shape[0]): - points = self.bresenham( - x1=xo, - y1=yo, - x2=xy[i,0], - y2=xy[i,1], - ) - - for j in range(points.shape[0] - 1): - x = points[j,0] - y = points[j,1] - if self.grid[y, x] == 100: - continue - self.grid[y, x] = 0 - - x = xy[i,0] - y = xy[i,1] - if distances[i,0] < message_laser.range_max: - self.grid[y, x] = 100 - self.grid[y+1, x] = 100 - self.grid[y-1, x] = 100 - self.grid[y, x-1] = 100 - self.grid[y, x+1] = 100 - else: - if self.grid[y, x] != 100: - self.grid[y, x] = 0 - - @property - def occupancy_grid(self): - return self.numpy_to_occupancy_grid(arr=self.grid) - - @staticmethod - def scan_to_distances(message: LaserScan) -> np.ndarray: - # N samples - N = len(message.ranges) - array = np.zeros((N, 2)) - for i in range(len(message.ranges)): - angle = i * message.angle_increment - if message.ranges[i] > message.range_max: - distance = message.range_max - elif message.ranges[i] < message.range_min: - distance = message.range_min - else: - distance = message.ranges[i] - # distances in [m] - array[i,0] = distance - # angles in [radians] - array[i,1] = angle - return array - - @staticmethod - def distances_to_xy(distances: np.ndarray, x: float, y: float, theta: float) -> np.ndarray: - # N samples - N = distances.shape[0] - array = np.zeros((N,2)) - array[:,0] = x + distances[:,0]*np.cos(distances[:,1] + theta) - array[:,1] = y + distances[:,0]*np.sin(distances[:,1] + theta) - return array - - - @staticmethod - def numpy_to_occupancy_grid(arr, info=None): - """ - Source: http://docs.ros.org/en/jade/api/ros_numpy/html/occupancy__grid_8py_source.html - """ - if not len(arr.shape) == 2: - raise TypeError('Array must be 2D') - if not arr.dtype == np.int8: - raise TypeError('Array must be of int8s') - grid = OccupancyGrid() - if isinstance(arr, np.ma.MaskedArray): - # We assume that the masked value are already -1, for speed - arr = arr.data - grid.data = arr.ravel().tolist() - grid.info = info or MapMetaData() - grid.info.height = arr.shape[0] - grid.info.width = arr.shape[1] - return grid - - @staticmethod - def bresenham(x1: int, y1: int, x2: int, y2: int) -> np.ndarray: - """ - Implementation of Bresenham's line drawing algorithm. - >>> points1 = bresenham((4, 4), (6, 10)) - >>> print(points1) - np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]]) - - Source: https://github.com/AtsushiSakai/PythonRobotics - """ - # setup initial conditions - dx = x2 - x1 - dy = y2 - y1 - # determine how steep the line is - is_steep = abs(dy) > abs(dx) - # rotate line - if is_steep: - x1, y1 = y1, x1 - x2, y2 = y2, x2 - # swap start and end points if necessary and store swap state - swapped = False - if x1 > x2: - x1, x2 = x2, x1 - y1, y2 = y2, y1 - swapped = True - # recalculate differentials - dx = x2 - x1 - # recalculate differentials - dy = y2 - y1 - # calculate error - error = int(dx / 2.0) - y_step = 1 if y1 < y2 else -1 - # iterate over bounding box generating points between start and end - y = y1 - points = [] - for x in range(x1, x2 + 1): - coord = [y, x] if is_steep else (x, y) - points.append(coord) - error -= abs(dy) - if error < 0: - y += y_step - error += dx - # reverse the list if the coordinates were swapped - if swapped: - points.reverse() - return np.array(points) - - - -def main(*args, **kwargs): - rclpy.init(*args, **kwargs) - node = TurtlebotMapper() - try: - rclpy.spin(node=node) - except (KeyboardInterrupt, ExternalShutdownException): - pass - finally: - node.destroy_node() - rclpy.try_shutdown() - - -if __name__ == "__main__": - main() diff --git a/turtlebot3_mapper/turtlebot3_explorer/__init__.py b/turtlebot3_mapper/turtlebot3_explorer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_mapper/turtlebot3_explorer/main.py b/turtlebot3_mapper/turtlebot3_explorer/main.py new file mode 100644 index 0000000..6fc7d6f --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_explorer/main.py @@ -0,0 +1,35 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.executors import ExternalShutdownException + +from turtlebot3_mapper.turtlebot3_explorer.turtlebot3_explorer \ + import Turtlebot3Explorer + + +def main(*args, **kwargs): + rclpy.init(*args, **kwargs) + node = Turtlebot3Explorer() + try: + rclpy.spin(node=node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/turtlebot3_mapper/turtlebot3_explorer/turtlebot3_explorer.py b/turtlebot3_mapper/turtlebot3_explorer/turtlebot3_explorer.py new file mode 100644 index 0000000..65ed72c --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_explorer/turtlebot3_explorer.py @@ -0,0 +1,237 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import random +from enum import Enum +from typing import List + +import numpy as np + +from rclpy.node import Node +from rclpy.parameter import Parameter + +from std_srvs.srv import SetBool + +from geometry_msgs.msg import Twist +from sensor_msgs.msg import LaserScan +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult + +from tf2_ros.buffer import Buffer +from tf2_ros import LookupException, TransformException +from tf2_ros.transform_listener import TransformListener + +from turtlebot3_mapper.utils import ( + euler_from_quaternion, + laser_scan_to_polar, +) + + +class State(Enum): + FORWARD = 1 + ROTATE = 2 + STOP = 3 + UNKOWN = 4 + + +class Turtlebot3Explorer(Node): + + def parameter_callback(self, parameters: List[Parameter]) -> SetParametersResult: + for param in parameters: + if param.name == "view_angle": + self.view_angle = param.value + self.get_logger().info("Updated parameter view_angle=%.2f" % self.view_angle) + elif param.name == "min_rand": + self.min_rand = param.value + self.get_logger().info("Updated parameter min_rand=%.2f" % self.min_rand) + elif param.name == "max_rand": + self.max_rand = param.value + self.get_logger().info("Updated parameter max_rand=%.2f" % self.max_rand) + elif param.name == "safety_distance": + self.safety_distance = param.value + self.get_logger().info("Updated parameter safety_distance=%.2f" % + self.safety_distance) + elif param.name == "linear_speed": + self.linear_speed = param.value + self.get_logger().info("Updated parameter linear_speed=%.2f" % self.linear_speed) + elif param.name == "angular_speed": + self.angular_speed = param.value + self.get_logger().info("Updated parameter angular_speed=%.2f" % self.angular_speed) + else: + return SetParametersResult(successful=False) + return SetParametersResult(successful=True) + + def __init__(self, node_name: str = 'turtlebot_explorer'): + super(Turtlebot3Explorer, self).__init__(node_name=node_name) + self.declare_parameters( + namespace="", + parameters=[ + ( + "view_angle", + 30.0, + ParameterDescriptor(description="View angle"), + ), + ( + "min_rand", + 1, + ParameterDescriptor(description="Min range random limit"), + ), + ( + "max_rand", + 10, + ParameterDescriptor(description="Max range random limit"), + ), + ( + "safety_distance", + 0.5, + ParameterDescriptor(description="Safety distance"), + ), + ( + "linear_speed", + 0.15, + ParameterDescriptor(description="Linear speed"), + ), + ( + "angular_speed", + 0.15, + ParameterDescriptor(description="Angular speed"), + ), + ( + "update_rate", + 0.5, + ParameterDescriptor(description="Update rate"), + ), + ], + ) + self.view_angle = float(self.get_parameter("view_angle").value) + self.min_rand = float(self.get_parameter("min_rand").value) + self.max_rand = float(self.get_parameter("max_rand").value) + self.safety_distance = float(self.get_parameter("safety_distance").value) + self.linear_speed = float(self.get_parameter("linear_speed").value) + self.angular_speed = float(self.get_parameter("angular_speed").value) + self.add_on_set_parameters_callback(self.parameter_callback) + self._subscriber_scan = self.create_subscription( + msg_type=LaserScan, + topic="/scan", + callback=self._scan_callback, + qos_profile=10, + ) + self._publisher = self.create_publisher( + msg_type=Twist, + topic="/cmd_vel", + qos_profile=10, + ) + self.update_timer = self.create_timer( + timer_period_sec=(1 / self.get_parameter("update_rate").value), + callback=self._update_callback, + ) + self.srv = self.create_service( + SetBool, + 'enable', + self._enable_callback, + ) + self._scan_init = False + + self.count = 0 + self.multiplier = 1.0 + self.limit = random.randint(self.min_rand, self.max_rand) + + self.state = State.UNKOWN + self.enable = False + + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self) + self.get_logger().info("Init {}".format(node_name)) + + def _enable_callback(self, request, response): + self.enable = request.data + response.success = True + response.message = "Configured" + return response + + def _scan_callback(self, message: LaserScan): + self._scan = message + self._scan_init = True + + def _update_callback(self): + if self._scan_init: + self.get_logger().info("updating...") + self.detect_obstacle() + + def set_robot(self, state: State): + twist = Twist() + twist.linear.y = 0.0 + twist.linear.z = 0.0 + twist.angular.x = 0.0 + twist.angular.y = 0.0 + if state == State.STOP: + twist.linear.x = 0.0 + twist.angular.z = 0.0 + self._publisher.publish(twist) + elif state == State.FORWARD: + twist.linear.x = self.linear_speed + twist.angular.z = 0.0 + self._publisher.publish(twist) + elif state == State.ROTATE: + twist.linear.x = 0.0 + twist.angular.z = self.angular_speed * self.multiplier + self.count += 1 + if self.count >= self.limit: + self.limit = random.randint(self.min_rand, self.max_rand) + self.count = 0 + self.multiplier *= -1.0 + self._publisher.publish(twist) + + def detect_obstacle(self): + scan = self._scan + try: + trans = self._tf_buffer.lookup_transform(scan.header.frame_id, "base_link", + scan.header.stamp) + except (TransformException, LookupException) as ex: + self.get_logger().warn(f'Could not transform "odom" to {scan.header.frame_id}: {ex}') + return + + _, _, theta = euler_from_quaternion(trans.transform.rotation) + min_angle = (-self.view_angle + theta) * (np.pi / 180) + (2 * np.pi) + max_angle = (self.view_angle + theta) * (np.pi / 180) + + polar = laser_scan_to_polar(message=scan) + mask = (polar[:, 1] >= (min_angle)) | (polar[:, 1] <= max_angle) + distance_range = polar[mask, :] + + obstacle_distance = np.min(distance_range[:, 0]) + self.get_logger().info("Closest obstacle={}".format(obstacle_distance)) + + if not self.enable: + if self.state != State.STOP: + self.state = State.STOP + self.set_robot(state=self.state) + else: + if self.state == State.UNKOWN or self.state == State.STOP: + if obstacle_distance < self.safety_distance: + self.state = State.ROTATE + self.set_robot(state=self.state) + else: + self.state = State.FORWARD + self.set_robot(state=self.state) + elif self.state == State.FORWARD: + if obstacle_distance < self.safety_distance: + self.state = State.ROTATE + self.set_robot(state=self.state) + else: + self.state = State.FORWARD + self.set_robot(state=self.state) + elif self.state == State.ROTATE: + if obstacle_distance > self.safety_distance: + self.state = State.FORWARD + self.set_robot(state=self.state) diff --git a/turtlebot3_mapper/turtlebot3_mission_client/__init__.py b/turtlebot3_mapper/turtlebot3_mission_client/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_mapper/turtlebot3_mission_client/main.py b/turtlebot3_mapper/turtlebot3_mission_client/main.py new file mode 100644 index 0000000..10f7750 --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_mission_client/main.py @@ -0,0 +1,73 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +import argparse + +from rclpy.executors import ExternalShutdownException + +from turtlebot3_mapper.turtlebot3_mission_client.turtlebot3_mission_client \ + import Turtlebot3MissionClient + + +def main(*args, **kwargs): + parser = argparse.ArgumentParser(description='Explore enviromment') + parser.add_argument('-f', + action='store', + dest='frontiers', + type=int, + default=10, + required=True, + help='Minimal number of frontiers to reach') + parser.add_argument('--width', + action='store', + dest='width', + type=int, + default=8, + required=False, + help='Width of map') + parser.add_argument('--height', + action='store', + dest='height', + type=int, + default=8, + required=False, + help='Height of map') + parser.add_argument('--resolution', + action='store', + dest='resolution', + type=float, + default=0.03, + required=False, + help='Resolution of map') + args = parser.parse_args() + rclpy.init(args=None) + node = Turtlebot3MissionClient( + resolution=args.resolution, + width=args.width, + height=args.height, + node_name="turtlebot3_mission_client", + ) + node.send_goal(frontiers=args.frontiers) + try: + rclpy.spin(node=node) + except (KeyboardInterrupt, ExternalShutdownException): + node.cancel() + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/turtlebot3_mapper/turtlebot3_mission_client/turtlebot3_mission_client.py b/turtlebot3_mapper/turtlebot3_mission_client/turtlebot3_mission_client.py new file mode 100644 index 0000000..4e7ba6c --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_mission_client/turtlebot3_mission_client.py @@ -0,0 +1,98 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient + +from action_msgs.msg import GoalStatus + +from turtlebot3_interfaces.action import Mission + + +class Turtlebot3MissionClient(Node): + + def __init__(self, + resolution: float, + width: int, + height: int, + node_name: str = "turtlebot3_mission_client"): + super().__init__(node_name=node_name) + self._action_client = ActionClient( + self, + Mission, + 'mission', + ) + self.resolution = resolution + self.width = width + self.height = height + self.world = (-self.width // 2, -self.height // 2) + + def cancel_done(self, future): + cancel_response = future.result() + if len(cancel_response.goals_canceling) > 0: + self.get_logger().info('Goal successfully canceled') + else: + self.get_logger().info('Goal failed to cancel') + rclpy.shutdown() + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + self._goal_handle = goal_handle + self.get_logger().info('Goal accepted :)') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + status = future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Goal succeeded! Result: {0}'.format(result.frontiers)) + else: + self.get_logger().info('Goal failed with status: {0}'.format(status)) + with open("results.txt", "w") as file: + file.write("x;y;width;height\n") + for bbox in result.bbox.boxes: + x = round((bbox.center.position.x * self.resolution) + self.world[0], 2) + y = round((bbox.center.position.y * self.resolution) + self.world[0], 2) + size_x = round(bbox.size_x * self.resolution, 2) + size_y = round(bbox.size_y * self.resolution, 2) + file.write(f"{x};{y};{size_x};{size_y}\n") + + rclpy.shutdown() + + def feedback_callback(self, feedback): + self.get_logger().info('Received feedback, frontiers={0}'.format( + feedback.feedback.frontiers)) + + def cancel(self): + self.get_logger().info('Canceling goal') + future = self._goal_handle.cancel_goal_async() + future.add_done_callback(self.cancel_done) + + def send_goal(self, frontiers: int): + self.get_logger().info('Waiting for action server...') + self._action_client.wait_for_server() + + goal_msg = Mission.Goal() + goal_msg.frontiers = frontiers + self.get_logger().info('Sending goal request...') + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) diff --git a/turtlebot3_mapper/turtlebot3_mission_controller/__init__.py b/turtlebot3_mapper/turtlebot3_mission_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_mapper/turtlebot3_mission_controller/main.py b/turtlebot3_mapper/turtlebot3_mission_controller/main.py new file mode 100644 index 0000000..3aebc0e --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_mission_controller/main.py @@ -0,0 +1,36 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor + +from turtlebot3_mapper.turtlebot3_mission_controller.turtlebot3_mission_controller \ + import Turtlebot3MissionController + + +def main(*args, **kwargs): + rclpy.init(*args, **kwargs) + executor = MultiThreadedExecutor() + node = Turtlebot3MissionController() + try: + rclpy.spin(node=node, executor=executor) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/turtlebot3_mapper/turtlebot3_mission_controller/turtlebot3_mission_controller.py b/turtlebot3_mapper/turtlebot3_mission_controller/turtlebot3_mission_controller.py new file mode 100644 index 0000000..d530ba6 --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_mission_controller/turtlebot3_mission_controller.py @@ -0,0 +1,197 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import threading +from functools import partial +from typing import Tuple, List + +import numpy as np + +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.action import ActionServer, CancelResponse, GoalResponse + +from std_srvs.srv import SetBool +from nav_msgs.msg import OccupancyGrid +from vision_msgs.msg import BoundingBox2DArray +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult + +from turtlebot3_interfaces.action import Mission +from turtlebot3_mapper.utils import occupancy_grid_to_numpy + + +class Turtlebot3MissionController(Node): + + def parameter_callback(self, parameters: List[Parameter]) -> SetParametersResult: + for param in parameters: + if param.name == "free_limit": + self.free_limit = param.value + self.get_logger().info("Updated parameter free_limit=%.2f" % self.free_limit) + else: + return SetParametersResult(successful=False) + return SetParametersResult(successful=True) + + def __init__(self, node_name: str = 'turtlebot_mission_controller'): + super(Turtlebot3MissionController, self).__init__(node_name=node_name) + self.declare_parameters( + namespace="", + parameters=[ + ( + "free_limit", + 30, + ParameterDescriptor(description="Max probability to consider cell free"), + ), + ], + ) + self.add_on_set_parameters_callback(self.parameter_callback) + self.free_limit = int(self.get_parameter("free_limit").value) + action_callback_group = ReentrantCallbackGroup() + service_callback_group = ReentrantCallbackGroup() + subscription_callback_group = ReentrantCallbackGroup() + self._goal_handle = None + self._goal_lock = threading.Lock() + self._subscriber_scan = self.create_subscription( + msg_type=OccupancyGrid, + topic="/custom_map", + callback=self._grid_callback, + qos_profile=10, + callback_group=subscription_callback_group, + ) + self._subscriber_bbox = self.create_subscription( + msg_type=BoundingBox2DArray, + topic="/detections", + callback=self._bbox_callback, + qos_profile=10, + callback_group=subscription_callback_group, + ) + self._action_server = ActionServer( + node=self, + action_type=Mission, + action_name='mission', + execute_callback=self._execute_callback, + goal_callback=self._goal_callback, + handle_accepted_callback=self._handle_accepted_callback, + cancel_callback=self._cancel_callback, + callback_group=action_callback_group, + ) + self.cli = self.create_client( + SetBool, + 'enable', + callback_group=service_callback_group, + ) + self.grid = None + self.bbox = None + self.get_logger().info("Init {}".format(node_name)) + + def _goal_callback(self, goal_request: Mission.Goal): + self.get_logger().info("Received goal request") + return GoalResponse.ACCEPT + + def _handle_accepted_callback(self, goal_handle: ServerGoalHandle) -> None: + with self._goal_lock: + if self._goal_handle is not None and self._goal_handle.is_active: + self.get_logger().info('Aborting previous goal') + self._goal_handle.abort() + self._goal_handle = goal_handle + goal_handle.execute() + + def _cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse.ACCEPT: + self.get_logger().info('Received cancel request') + return CancelResponse.ACCEPT + + def _execute_callback(self, goal_handle: ServerGoalHandle) -> Mission.Result: + frontiers = np.inf + self._send_request(enable=True) + while frontiers > goal_handle.request.frontiers: + if not goal_handle.is_active: + self._send_request(enable=False) + self.get_logger().info('Goal aborted') + response = Mission.Result() + response.frontiers = int(frontiers) + response.bbox = self.bbox + return response + if goal_handle.is_cancel_requested: + self._send_request(enable=False) + goal_handle.canceled() + self.get_logger().info('Goal canceled') + response = Mission.Result() + response.frontiers = int(frontiers) + response.bbox = self.bbox + return response + + frontiers = self.count_frontiers(message=self.grid, free_limit=self.free_limit) + feedback_msg = Mission.Feedback() + feedback_msg.frontiers = int(frontiers) + goal_handle.publish_feedback(feedback_msg) + self.get_logger().info("Frontiers={}".format(frontiers)) + time.sleep(1) + self._send_request(enable=False) + goal_handle.succeed() + self.get_logger().info("Reached goal! :)") + response = Mission.Result() + response.frontiers = int(frontiers) + response.bbox = self.bbox + return response + + def _grid_callback(self, message: OccupancyGrid): + self.grid = message + self.get_logger().info("Received grid") + + def _bbox_callback(self, message: BoundingBox2DArray): + self.bbox = message + self.get_logger().info("Received grid") + + def _send_request(self, enable: bool): + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + request = SetBool.Request() + request.data = enable + future = self.cli.call_async(request) + future.add_done_callback(partial(self._callback_set)) + + def _callback_set(self, future): + try: + _ = future.result() + except Exception as ex: + self.get_logger().warn("Service call failed: %r" % (ex, )) + + def count_frontiers(self, message: OccupancyGrid, free_limit: int = 30): + count = 0 + array = occupancy_grid_to_numpy(msg=message) + free = np.argwhere(array < free_limit) + for n in range(free.shape[0]): + item = free[n, :] + neighboors = self.get_neighboors(x=item[0], y=item[1], shape=array.shape) + for i in range(neighboors.shape[0]): + coord = neighboors[i, :] + value = array[coord[0], coord[1]] + if value == 50: + count += 1 + return count + + @staticmethod + def get_neighboors(x: int, y: int, shape: Tuple[int, int]): + neighboors = [] + if (x + 1) < shape[1]: + neighboors.append([x + 1, y]) + if (x - 1) >= 0: + neighboors.append([x - 1, y]) + if (y + 1) < shape[0]: + neighboors.append([x, y + 1]) + if (y - 1) >= 0: + neighboors.append([x, y - 1]) + return np.array(neighboors) diff --git a/turtlebot3_mapper/turtlebot3_object_detector/__init__.py b/turtlebot3_mapper/turtlebot3_object_detector/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_mapper/turtlebot3_object_detector/main.py b/turtlebot3_mapper/turtlebot3_object_detector/main.py new file mode 100644 index 0000000..adc50c9 --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_object_detector/main.py @@ -0,0 +1,35 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.executors import ExternalShutdownException + +from turtlebot3_mapper.turtlebot3_object_detector.turtlebot3_object_detector \ + import Turtlebot3ObjectDetector + + +def main(*args, **kwargs): + rclpy.init(*args, **kwargs) + node = Turtlebot3ObjectDetector(node_name="turtlebot3_object_detector") + try: + rclpy.spin(node=node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/turtlebot3_mapper/turtlebot3_object_detector/turtlebot3_object_detector.py b/turtlebot3_mapper/turtlebot3_object_detector/turtlebot3_object_detector.py new file mode 100644 index 0000000..5051661 --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_object_detector/turtlebot3_object_detector.py @@ -0,0 +1,146 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import List + +import cv2 +import numpy as np +from cv_bridge import CvBridge + +from rclpy.node import Node +from rclpy.parameter import Parameter + +from sensor_msgs.msg import Image +from nav_msgs.msg import OccupancyGrid +from vision_msgs.msg import BoundingBox2DArray, BoundingBox2D +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult + +from turtlebot3_mapper.utils import occupancy_grid_to_numpy + + +class Turtlebot3ObjectDetector(Node): + + def parameter_callback(self, parameters: List[Parameter]) -> SetParametersResult: + for param in parameters: + if param.name == "threshold": + self.threshold = param.value + self.get_logger().info("Updated parameter threshold=%.2f" % self.threshold) + elif param.name == "connectivity": + self.connectivity = param.value + self.get_logger().info("Updated parameter connectivity=%.2f" % self.connectivity) + elif param.name == "min_area": + self.min_area = param.value + self.get_logger().info("Updated parameter min_area=%.2f" % self.min_area) + elif param.name == "max_area": + self.max_area = param.value + self.get_logger().info("Updated parameter max_area=%.2f" % self.max_area) + else: + return SetParametersResult(successful=False) + return SetParametersResult(successful=True) + + def __init__(self, node_name: str = 'turtlebot3_object_detector'): + + super(Turtlebot3ObjectDetector, self).__init__(node_name=node_name) + self.declare_parameters( + namespace="", + parameters=[ + ( + "threshold", + 150, + ParameterDescriptor(description="Threshold to binary image"), + ), + ( + "connectivity", + 4, + ParameterDescriptor(description="4-way or 8-way connectivity"), + ), + ( + "min_area", + 30, + ParameterDescriptor(description="Min area of each connected component"), + ), + ( + "max_area", + 100, + ParameterDescriptor(description="Max area of each connected component"), + ), + ], + ) + self.threshold = float(self.get_parameter("threshold").value) + self.connectivity = float(self.get_parameter("connectivity").value) + self.min_area = float(self.get_parameter("min_area").value) + self.max_area = float(self.get_parameter("max_area").value) + self.add_on_set_parameters_callback(self.parameter_callback) + + self._map_subscriber = self.create_subscription( + msg_type=OccupancyGrid, + topic="/custom_map", + callback=self._map_callback, + qos_profile=10, + ) + self._detections_publisher = self.create_publisher( + msg_type=BoundingBox2DArray, + topic="/detections", + qos_profile=10, + ) + self._result_publisher = self.create_publisher( + msg_type=Image, + topic="/rendered", + qos_profile=10, + ) + self._bridge = CvBridge() + self.get_logger().info(f"Init {node_name}") + + def _map_callback(self, message: OccupancyGrid): + # get occupancy grid as a numpy array int8 + array = occupancy_grid_to_numpy(message) + # convert int8 array to float32 + input = array.astype("float32") + # remap to 0-255 interval + input = 255 * (input - np.min(input)) / (np.max(input) - np.min(input)) + # if it is unkown or free, mark as 0 + input[input <= self.threshold] = 0 + # if it is occupied, mark as 255 + input[input > self.threshold] = 255 + # get array as uint8 + input = input.astype('uint8') + # get component connected information + analysis = cv2.connectedComponentsWithStats(input, self.connectivity, cv2.CV_32S) + # unpack information + (totalLabels, label_ids, values, centroid) = analysis + # get RGB image to show + output = cv2.cvtColor(input, cv2.COLOR_GRAY2BGR) + # Loop through each component + detections = BoundingBox2DArray() + detections.header = message.header + for i in range(1, totalLabels): + x = values[i, cv2.CC_STAT_LEFT] + y = values[i, cv2.CC_STAT_TOP] + w = values[i, cv2.CC_STAT_WIDTH] + h = values[i, cv2.CC_STAT_HEIGHT] + area = values[i, cv2.CC_STAT_AREA] + (cX, cY) = x + (w // 2), y + (h // 2) + if (area > self.min_area) and (area < self.max_area): + bbox = BoundingBox2D() + bbox.center.position.x = float(cX) + bbox.center.position.y = float(cY) + bbox.size_x = float(w) + bbox.size_y = float(h) + detections.boxes.append(bbox) + cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 1) + cv2.circle(output, (int(cX), int(cY)), 1, (0, 0, 255), -1) + self._detections_publisher.publish(detections) + image_ros = self._bridge.cv2_to_imgmsg(cvim=np.flip(output, axis=0)) + image_ros.header = message.header + self._result_publisher.publish(image_ros) diff --git a/turtlebot3_mapper/turtlebot3_occupancy_grid/__init__.py b/turtlebot3_mapper/turtlebot3_occupancy_grid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_mapper/turtlebot3_occupancy_grid/main.py b/turtlebot3_mapper/turtlebot3_occupancy_grid/main.py new file mode 100644 index 0000000..13497fc --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_occupancy_grid/main.py @@ -0,0 +1,35 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor + +from turtlebot3_mapper.turtlebot3_occupancy_grid.turtlebot3_occupancy_grid \ + import Turtlebot3OccupancyGrid + + +def main(*args, **kwargs): + rclpy.init(*args, **kwargs) + node = Turtlebot3OccupancyGrid(node_name="turtlebot3_occupancy_grid") + executor = MultiThreadedExecutor() + try: + rclpy.spin(node=node, executor=executor) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/turtlebot3_mapper/turtlebot3_occupancy_grid/turtlebot3_occupancy_grid.py b/turtlebot3_mapper/turtlebot3_occupancy_grid/turtlebot3_occupancy_grid.py new file mode 100644 index 0000000..8b6c354 --- /dev/null +++ b/turtlebot3_mapper/turtlebot3_occupancy_grid/turtlebot3_occupancy_grid.py @@ -0,0 +1,402 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import threading +from enum import Enum +from typing import List + +import numpy as np + +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.parameter import Parameter + +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import TransformStamped +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult + +from tf2_ros.buffer import Buffer +from tf2_ros import LookupException, TransformException +from tf2_ros.transform_listener import TransformListener +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +from turtlebot3_mapper.utils import ( + numpy_to_occupancy_grid, + euler_from_quaternion, + laser_scan_to_polar, + polar_to_cartesian, +) + + +class State(Enum): + FREE = 1 + OCCUPIED = 2 + UNKOWN = 3 + + +class Turtlebot3OccupancyGrid(Node): + + def parameter_callback(self, parameters: List[Parameter]) -> SetParametersResult: + for param in parameters: + if param.name == "min_prob": + self.min_prob = param.value + self.get_logger().info("Updated parameter min_prob=%.2f" % self.min_prob) + elif param.name == "max_prob": + self.max_prob = param.value + self.get_logger().info("Updated parameter max_prob=%.2f" % self.max_prob) + else: + return SetParametersResult(successful=False) + return SetParametersResult(successful=True) + + def __init__(self, node_name: str = 'turtlebot_occupancy_grid'): + + super(Turtlebot3OccupancyGrid, self).__init__(node_name=node_name) + self.declare_parameters( + namespace="", + parameters=[ + ( + "width", + 8.0, + ParameterDescriptor(description="Map's width"), + ), + ( + "height", + 8.0, + ParameterDescriptor(description="Map's height"), + ), + ( + "resolution", + 0.03, + ParameterDescriptor(description="Map's resolution"), + ), + ( + "min_prob", + 0.01, + ParameterDescriptor(description="Map's min probability value"), + ), + ( + "max_prob", + 0.99, + ParameterDescriptor(description="Map's max probability value"), + ), + ( + "prob_occupied", + 0.6, + ParameterDescriptor(description="Occupied probability increment"), + ), + ( + "prob_free", + 0.4, + ParameterDescriptor(description="Free probability increment"), + ), + ( + "prob_priori", + 0.5, + ParameterDescriptor(description="Priori probability increment"), + ), + ( + "rate", + 1.0, + ParameterDescriptor(description="Update rate"), + ), + ], + ) + self.width = float(self.get_parameter("width").value) + self.height = float(self.get_parameter("height").value) + self.resolution = float(self.get_parameter("resolution").value) + self.min_prob = float(self.get_parameter("min_prob").value) + self.max_prob = float(self.get_parameter("max_prob").value) + self.prob_occupied = float(self.get_parameter("prob_occupied").value) + self.prob_free = float(self.get_parameter("prob_free").value) + self.prob_priori = float(self.get_parameter("prob_priori").value) + self.rate = float(self.get_parameter("rate").value) + self.add_on_set_parameters_callback(self.parameter_callback) + # create 2D-array to holds occupancy grid + N = int(1 / self.resolution) + shape = (int(self.width * N), int(self.height * N)) + self.grid = self.prob_priori * np.ones(shape, dtype=float) + # subscriber to receive laser messages + self._scan_subscriber = self.create_subscription( + msg_type=LaserScan, + topic="/scan", + callback=self._scan_callback, + qos_profile=10, + ) + # publisher to send probability map + self._map_publisher = self.create_publisher( + msg_type=OccupancyGrid, + topic="/custom_map", + qos_profile=10, + ) + # timer to update map + self._update_timer = self.create_timer( + timer_period_sec=(1 / self.rate), + callback=self._timer_callback, + ) + self._scan_init = False + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self) + self._tf_publisher = StaticTransformBroadcaster(self) + tf = TransformStamped() + tf.header.stamp = self.get_clock().now().to_msg() + tf.header.frame_id = 'map' + tf.child_frame_id = 'odom' + tf.transform.translation.x = 0.0 + tf.transform.translation.y = 0.0 + tf.transform.translation.z = 0.0 + self._tf_publisher.sendTransform(tf) + self._update = threading.Lock() + self.get_logger().info(f"Init {node_name}") + + def _scan_callback(self, message: LaserScan): + """Callback executed when node receives messages from laser scan in robot. + + Parameters + ---------- + message_laser: :py:class:`LaserScan ` + message with laser scan received from robot. + """ + self._scan_init = True + self._scan = message + self.update_map(message_laser=self._scan) + + def _timer_callback(self): + """Timer callback used to publish occupancy grid. Only starts publishing after received + message from laser scan in robot. + """ + if self._scan_init: + self._update.acquire() + self._map_publisher.publish(self.occupancy_grid) + self._update.release() + self.get_logger().info("Sending occupancy grid...") + + def update_map(self, message_laser: LaserScan): + """Utility look for transformation between laser scan frame and odometry frame, compute + laser samples in odometry referential and mark as free or occupied. Note that when it finds + an occupied sample, it also mark 4-components around it as occupied. + + Parameters + ---------- + message_laser: :py:class:`LaserScan ` + message with laser scan received from robot. + """ + if not self._update.locked(): + self._update.acquire() + try: + tf = self._tf_buffer.lookup_transform( + target_frame='odom', + source_frame=message_laser.header.frame_id, + time=message_laser.header.stamp, + timeout=Duration(seconds=(1 / (self.rate))), + ) + quaternion = tf.transform.rotation + except (TransformException, LookupException) as ex: + self.get_logger().warn( + f'Could not transform "odom" to {message_laser.header.frame_id}: {ex}') + self._update.release() + return + _, _, theta = euler_from_quaternion(quaternion=quaternion) + if theta < 0.0: + theta += 2 * np.pi + polar = laser_scan_to_polar(message=message_laser) + xy = polar_to_cartesian( + coordinates=polar, + x=tf.transform.translation.x, + y=tf.transform.translation.y, + theta=theta, + ) + xy[:, 0] = (xy[:, 0] + self.width // 2) / self.resolution + xy[:, 1] = (xy[:, 1] + self.height // 2) / self.resolution + xy = xy.astype(int) + xo = int((tf.transform.translation.x + self.width // 2) / self.resolution) + yo = int((tf.transform.translation.y + self.height // 2) / self.resolution) + for i in range(xy.shape[0]): + points = self.bresenham( + x1=xo, + y1=yo, + x2=xy[i, 0], + y2=xy[i, 1], + ) + for j in range(points.shape[0] - 1): + x = points[j, 0] + y = points[j, 1] + self.update_cell(x=x, y=y, state=State.FREE) + x = xy[i, 0] + y = xy[i, 1] + if polar[i, 0] < message_laser.range_max: + self.update_cell(x=x, y=y, state=State.OCCUPIED) + self.update_cell(x=x - 1, y=y, state=State.OCCUPIED) + self.update_cell(x=x + 1, y=y, state=State.OCCUPIED) + self.update_cell(x=x, y=y + 1, state=State.OCCUPIED) + self.update_cell(x=x, y=y - 1, state=State.OCCUPIED) + else: + self.update_cell(x=x, y=y, state=State.FREE) + self.get_logger().info("Updated occupancy grid...") + self._update.release() + + def update_cell(self, x: int, y: int, state: State): + """Given a state observed given laser measurements, update occupancy grid using log-odds + and retrive probability to be store at this position. + + Parameters + ---------- + x: int + X-axis position in occupancy grid. + y: int + Y-axis position in occupancy grid. + state: :py:class:`State ` + Flag used to indicate the status observed at this position in the occupancy grid. + """ + if state == State.FREE: + log_prob = self.log_odd(probability=self.prob_free) + elif state == State.OCCUPIED: + log_prob = self.log_odd(probability=self.prob_occupied) + else: + log_prob = self.log_odd(probability=self.prob_priori) + current_prob = self.grid[x, y] + current_prob_log_odd = self.log_odd(probability=current_prob) + current_prob_log_odd += log_prob + new_prob = self.probability(log_odd=current_prob_log_odd) + if new_prob < self.min_prob: + new_prob = self.min_prob + elif new_prob > self.max_prob: + new_prob = self.max_prob + self.grid[x, y] = new_prob + + @property + def occupancy_grid(self): + # copy data from grid + grid = np.zeros_like(self.grid) + # multiple by 100, to give probabilities between 0 and 100 + grid[:, :] = self.grid[:, :] * 100 + # grid = np.rot90(grid) + grid = np.transpose(grid) + # convert to int8 and returns as occupancy grid + grid = grid.astype("int8") + return numpy_to_occupancy_grid( + array=grid, + resolution=self.resolution, + world=(-self.width // 2, -self.height // 2), + frame_id="map", + timestamp=self.get_clock().now().to_msg(), + ) + + @staticmethod + def log_odd(probability: float) -> float: + """A log odds in statistics is the logarithm of the odds ratio. The advantage of the + log-odds over the probability representation is that we can avoid numerical instabilities + for probabilities near zero or one. + + Parameters + ---------- + probability: float + Given probability to estimate log-odds. + + Returns + ------- + float + Return log-odds of a given probability. + """ + return np.log(probability / (1.0 - probability)) + + @staticmethod + def probability(log_odd: float) -> float: + """Retrieve probability from a log-odds representation. It will always be a value between 0 + and 1. + + Parameters + ---------- + log_odd: float + Given log-odd to estimate probability. + + Returns + ------- + float + Return probability between 0 and 1. + """ + result = 1 - (1.0 / (1 + np.exp(log_odd))) + if np.isnan(result): + result = 0.0 + return result + + @staticmethod + def bresenham(x1: int, y1: int, x2: int, y2: int) -> np.ndarray: + """Implementation of Bresenham's line drawing algorithm. The first point can be thought of + as the source point and the second point as the target point. + + Parameters + ---------- + x1: int + X-axis position in occupancy grid of first point. + y1: int + Y-axis position in occupancy grid of first point. + x2: int + X-axis position in occupancy grid of second point. + y2: int + Y-axis position in occupancy grid of second point. + Returns + ------- + :py:class:`ndarray ` of shape `(N,2)` + The first column corresponds to the X's and the second column corresponds to the Y's + occupancy grid indexes that the line covers. + + Examples + -------- + >>> points1 = bresenham((4, 4), (6, 10)) + >>> print(points1) + np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]]) + + Reference + --------- + Code inspired by a collection of sample codes for robotics algorithms. + Source code at: + * https://github.com/AtsushiSakai/PythonRobotics + """ + # setup initial conditions + dx = x2 - x1 + dy = y2 - y1 + # determine how steep the line is + is_steep = abs(dy) > abs(dx) + # rotate line + if is_steep: + x1, y1 = y1, x1 + x2, y2 = y2, x2 + # swap start and end points if necessary and store swap state + swapped = False + if x1 > x2: + x1, x2 = x2, x1 + y1, y2 = y2, y1 + swapped = True + # recalculate differentials + dx = x2 - x1 + # recalculate differentials + dy = y2 - y1 + # calculate error + error = int(dx / 2.0) + y_step = 1 if y1 < y2 else -1 + # iterate over bounding box generating points between start and end + y = y1 + points = [] + for x in range(x1, x2 + 1): + coord = [y, x] if is_steep else (x, y) + points.append(coord) + error -= abs(dy) + if error < 0: + y += y_step + error += dx + # reverse the list if the coordinates were swapped + if swapped: + points.reverse() + return np.array(points) diff --git a/turtlebot3_mapper/utils.py b/turtlebot3_mapper/utils.py index 859946d..94c2465 100644 --- a/turtlebot3_mapper/utils.py +++ b/turtlebot3_mapper/utils.py @@ -1,27 +1,175 @@ +# Copyright 2022 Luiz Carlos Cosmi Filho and others. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import Tuple + import numpy as np +from rclpy.time import Time + +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import Quaternion + + +def euler_from_quaternion(quaternion: Quaternion): + """ Convert quaternion (w in last place) to euler roll, pitch, yaw. + + Parameters + ---------- + quaternion: :py:class:`Quarternion ` + Orientation as a quaternion. + + Returns + ------- + Tuple[float,float,float] + Return roll, pitch, yaw euler angles. + + References + ---------- + Source: https://github.com/ROBOTIS-GIT/turtlebot3/blob/humble-devel/turtlebot3_example + """ + x = quaternion.x + y = quaternion.y + z = quaternion.z + w = quaternion.w + + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.arcsin(sinp) + + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + + +def laser_scan_to_polar(message: LaserScan) -> np.ndarray: + """Convert a LaserScan message to a numpy array with points polar coordinates obtained. If + the message indicates infinity, set the distance as the sensor's maximum value. In the + same way, if the message indicates a distance value smaller than the minimum distance, set + the sensor's minimum distance. + + Parameters + ---------- + message: :py:class:`LaserScan ` + Message received from laser sensor. + + Returns + ------- + :py:class:`ndarray ` of shape `(N,2)` + Points sampled in polar coordinates. The first column corresponds to the distance (in + meters) and the second column corresponds to the angle (in radians). + """ + N = len(message.ranges) + array = np.zeros((N, 2)) + for i in range(len(message.ranges)): + angle = i * message.angle_increment + if message.ranges[i] > message.range_max: + distance = message.range_max + elif message.ranges[i] < message.range_min: + distance = message.range_min + else: + distance = message.ranges[i] + array[i, 0] = distance + array[i, 1] = angle + return array + + +def polar_to_cartesian(coordinates: np.ndarray, x: float, y: float, theta: float) -> np.ndarray: + """Convert points in polar coordinates in the laser reference to cartesian coordinates in + the odometry reference. + + Parameters + ---------- + coordinates: :py:class:`ndarray ` of shape `(N,2)` + Points sampled in polar coordinates. The first column corresponds to the distance (in + meters) and the second column corresponds to the angle (in radians). + x: float + Laser position on the X-axis in the odometry referential. + y: float + Laser position on the Y-axis in the odometry referential. + theta: float + Laser rotation in the odometry referential. + + Returns + ------- + :py:class:`ndarray ` of shape `(N,2)` + Points sample in cartesian coordinates at odometry referential. The first column + corresponds to the X's and the second column corresponds to the Y's. + """ + N = coordinates.shape[0] + array = np.zeros((N, 2)) + array[:, 0] = x + coordinates[:, 0] * np.cos(coordinates[:, 1] + theta) + array[:, 1] = y + coordinates[:, 0] * np.sin(coordinates[:, 1] + theta) + return array + + +def numpy_to_occupancy_grid(array: np.ndarray, resolution: float, world: Tuple[float, float], + frame_id: str, timestamp: Time): + """Utility to converts an :py:class:`ndarray ` to an + :py:class:`OccupancyGrid `. -def euler_from_quaternion(quaternion): - """ - Convert quaternion (w in last place) to euler roll, pitch, yaw. - quat = [x, y, z, w] - - Source: https://github.com/ROBOTIS-GIT/turtlebot3/blob/humble-devel/turtlebot3_example - """ - x = quaternion.x - y = quaternion.y - z = quaternion.z - w = quaternion.w + Parameters + ---------- + array: :py:class:`ndarray ` of shape `(N,M)` + Two dimentional numpy array to convert in a occupancy grid message. + resolution: float + Occupancy grid resolution. + world: Tuple[float,float] + `(x,y)` world position. + frame_id: str + Referential frame name. + timestamp: Time + Timestamp info. - sinr_cosp = 2 * (w * x + y * z) - cosr_cosp = 1 - 2 * (x * x + y * y) - roll = np.arctan2(sinr_cosp, cosr_cosp) + Returns + ------- + :py:class:`OccupancyGrid ` + Occupancy grid message. - sinp = 2 * (w * y - z * x) - pitch = np.arcsin(sinp) + Reference + --------- + Code inspired by `ros_numpy` package, a collection of conversion function for extracting + numpy arrays from messages. Source code at: + * http://docs.ros.org/en/jade/api/ros_numpy/html/occupancy__grid_8py_source.html + """ + if not len(array.shape) == 2: + raise TypeError('Array must be 2D') + if not array.dtype == np.int8: + raise TypeError('Array must be of int8s') + grid = OccupancyGrid() + if isinstance(array, np.ma.MaskedArray): + array = array.data + grid.header.stamp = timestamp + grid.header.frame_id = frame_id + grid.data = array.ravel().tolist() + grid.info.resolution = resolution + grid.info.height = array.shape[0] + grid.info.width = array.shape[1] + grid.info.origin.position.x = float(world[0]) + grid.info.origin.position.y = float(world[1]) + return grid - siny_cosp = 2 * (w * z + x * y) - cosy_cosp = 1 - 2 * (y * y + z * z) - yaw = np.arctan2(siny_cosp, cosy_cosp) - return roll, pitch, yaw \ No newline at end of file +def occupancy_grid_to_numpy(msg): + """ + Source: http://docs.ros.org/en/jade/api/ros_numpy/html/occupancy__grid_8py_source.html + """ + data = np.asarray(msg.data, dtype=np.int8).reshape(msg.info.height, msg.info.width) + return data diff --git a/turtlebot3_mapper/viewer.py b/turtlebot3_mapper/viewer.py deleted file mode 100644 index e82e240..0000000 --- a/turtlebot3_mapper/viewer.py +++ /dev/null @@ -1,97 +0,0 @@ -import sys -import cv2 -import rclpy -import numpy as np - -from rclpy.node import Node -from rclpy.executors import ExternalShutdownException - -from nav_msgs.msg import OccupancyGrid - -class MapViewer(Node): - - def __init__(self, - node_name: str = 'map_viewer'): - - super(MapViewer, self).__init__(node_name=node_name) - - self._map_subscriber = self.create_subscription( - msg_type=OccupancyGrid, - topic="/custom_map", - callback=self._map_callback, - qos_profile=10, - ) - self._init = False - self.get_logger().info(f"Init {node_name}") - - def _map_callback(self, message: OccupancyGrid): - self._init = True - array = self.occupancygrid_to_numpy(message) - array[array == -1] = 57 - array[array == 0] = 127 - array[array == 100] = 0 - input = array.astype('uint8') - # Applying 7x7 Gaussian Blur - blurred = cv2.GaussianBlur(input, (7, 7), 0) - # Applying threshold - threshold = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY_INV | cv2.THRESH_OTSU)[1] - # Apply the Component analysis function - analysis = cv2.connectedComponentsWithStats(threshold, 4, cv2.CV_32S) - (totalLabels, label_ids, values, centroid) = analysis - - # Initialize a new image to store all the output components - input = array.astype('uint8') - output = cv2.cvtColor(input, cv2.COLOR_GRAY2BGR) - - - # Loop through each component - count = 0 - for i in range(1, totalLabels): - # Area of the component - x = values[i, cv2.CC_STAT_LEFT] - y = values[i, cv2.CC_STAT_TOP] - w = values[i, cv2.CC_STAT_WIDTH] - h = values[i, cv2.CC_STAT_HEIGHT] - area = values[i, cv2.CC_STAT_AREA] - (cX, cY) = centroid[i] - if (area > 100) and (area < 1000): - count += 1 - cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 1) - cv2.circle(output, (int(cX), int(cY)), 1, (0, 0, 255), -1) - # componentMask = (label_ids == i).astype("uint8") * 255 - # output = cv2.bitwise_or(output, componentMask) - - # resized = cv2.resize(output, (500, 500)) - image = cv2.flip(output, 0) - rotated_image = cv2.rotate(src = image, rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) - - cv2.namedWindow("output", cv2.WINDOW_NORMAL) - cv2.imshow("output", rotated_image) - if cv2.waitKey(1) == ord('q'): - sys.exit(1) - - - @staticmethod - def occupancygrid_to_numpy(msg, info=None): - """ - Source: http://docs.ros.org/en/jade/api/ros_numpy/html/occupancy__grid_8py_source.html - """ - data = np.asarray(msg.data, dtype=np.int8).reshape(msg.info.height, msg.info.width) - return data - - - -def main(*args, **kwargs): - rclpy.init(*args, **kwargs) - node = MapViewer() - try: - rclpy.spin(node=node) - except (KeyboardInterrupt, ExternalShutdownException): - pass - finally: - node.destroy_node() - rclpy.try_shutdown() - - -if __name__ == "__main__": - main()