From 8ba8585a2983d48f1500a813f668f6161b1ebbc8 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 19:25:10 +0100 Subject: [PATCH] GameElementLayer prototype --- toid_costmaps/CMakeLists.txt | 69 ++++++ toid_costmaps/LICENSE | 202 ++++++++++++++++++ .../toid_costmaps/game_elements_layer.hpp | 39 ++++ toid_costmaps/package.xml | 30 +++ toid_costmaps/src/game_elements_layer.cpp | 38 ++++ toid_costmaps/toid_costmaps.xml | 7 + toid_lidar/CMakeLists.txt | 8 + toid_lidar/launch/launch.py | 68 ++++++ toid_lidar/params/lidar.yaml | 11 + toid_lidar/rviz/conf.rviz | 175 +++++++++++++++ 10 files changed, 647 insertions(+) create mode 100644 toid_costmaps/CMakeLists.txt create mode 100644 toid_costmaps/LICENSE create mode 100644 toid_costmaps/include/toid_costmaps/game_elements_layer.hpp create mode 100644 toid_costmaps/package.xml create mode 100644 toid_costmaps/src/game_elements_layer.cpp create mode 100644 toid_costmaps/toid_costmaps.xml create mode 100644 toid_lidar/launch/launch.py create mode 100644 toid_lidar/params/lidar.yaml create mode 100644 toid_lidar/rviz/conf.rviz diff --git a/toid_costmaps/CMakeLists.txt b/toid_costmaps/CMakeLists.txt new file mode 100644 index 0000000..9f31edc --- /dev/null +++ b/toid_costmaps/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_costmaps) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(library_name toid_costmaps) + +set( + PACKAGE_DEPS + + rclcpp + angles + geometry_msgs + pluginlib + nav_msgs + nav2_core + nav2_costmap_2d + nav2_util + tf2 + tf2_geometry_msgs + tf2_ros + Eigen3 + toid_msgs +) + +set( + SOURCES + src/game_elements_layer.cpp +) + +find_package(ament_cmake REQUIRED) +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +add_library(${library_name} SHARED ${SOURCES}) + +target_include_directories( + ${library_name} + PRIVATE + + include +) + +ament_target_dependencies( + ${library_name} + ${PACKAGE_DEPS} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${PACKAGE_DEPS}) + +pluginlib_export_plugin_description_file(nav2_costmap_2d toid_costmaps.xml) + +ament_package() diff --git a/toid_costmaps/LICENSE b/toid_costmaps/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_costmaps/LICENSE @@ -0,0 +1,202 @@ + + 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. diff --git a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp new file mode 100644 index 0000000..de71151 --- /dev/null +++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace toid +{ + +class GameElementLayer : public nav2_costmap_2d::CostmapLayer +{ +public: + GameElementLayer(){ + costmap_ = NULL; + } + ~GameElementLayer(){} + + virtual void onInitialize(); + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y); + + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + + virtual void reset() { return; } + + virtual void onFootprintChanged() {} + + virtual bool isClearable() { return false; } + +private: + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + bool need_recalculation_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/package.xml b/toid_costmaps/package.xml new file mode 100644 index 0000000..98952d0 --- /dev/null +++ b/toid_costmaps/package.xml @@ -0,0 +1,30 @@ + + + + toid_costmaps + 0.0.0 + TODO: Package description + Pimpest + Apache-2.0 + + ament_cmake + + angles + geometry_msgs + nav_msgs + nav2_costmap_2d + nav2_util + nav2_core + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + toid_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_costmaps/src/game_elements_layer.cpp b/toid_costmaps/src/game_elements_layer.cpp new file mode 100644 index 0000000..aecba67 --- /dev/null +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -0,0 +1,38 @@ +#include "toid_costmaps/game_elements_layer.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace toid +{ + +void GameElementLayer::onInitialize() {} + +void GameElementLayer::updateBounds( + double, double, double, double * min_x, double * min_y, double * max_x, double * max_y) +{ + touch(1.0 - 0.1, 0.5 - 0.1, min_x, min_y, max_x, max_y); + touch(1.0 + 0.1, 0.5 + 0.1, min_x, min_y, max_x, max_y); +} + +void GameElementLayer::updateCosts( + nav2_costmap_2d::Costmap2D & grid, int min_i, int min_j, int max_i, int max_j) +{ + bool in_bounds = true; + uint mx, my; + in_bounds &= worldToMap(1.0 - 0.1, 0.5 - 0.1, mx, my); + uint mmx, mmy; + in_bounds &= worldToMap(1.0 + 0.1, 0.5 + 0.1, mmx, mmy); + + if (in_bounds) { + for (uint j = my; j < mmy; j++) { + for (uint i = mx; i < mmx; i++) { + grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + } +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer); \ No newline at end of file diff --git a/toid_costmaps/toid_costmaps.xml b/toid_costmaps/toid_costmaps.xml new file mode 100644 index 0000000..bead28e --- /dev/null +++ b/toid_costmaps/toid_costmaps.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt index 41afecf..1e83fe0 100644 --- a/toid_lidar/CMakeLists.txt +++ b/toid_lidar/CMakeLists.txt @@ -39,6 +39,14 @@ target_include_directories( include ) +install( + DIRECTORY + launch + params + rviz + DESTINATION share/${PROJECT_NAME}/ +) + ament_target_dependencies(toid_lidar ${PACKAGE_DEPS}) install(TARGETS toid_lidar DESTINATION lib/${PROJECT_NAME}) diff --git a/toid_lidar/launch/launch.py b/toid_lidar/launch/launch.py new file mode 100644 index 0000000..a78e77e --- /dev/null +++ b/toid_lidar/launch/launch.py @@ -0,0 +1,68 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from pathlib import Path + +def generate_launch_description(): + + basedir = FindPackageShare("").find("toid_lidar") + + lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"]) + rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"]) + + visualize = LaunchConfiguration("visualize") + draw_markers = LaunchConfiguration("draw_markers") + use_closest = LaunchConfiguration("use_closest") + lidar_frame = LaunchConfiguration("lidar_frame") + + return LaunchDescription([ + DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ), + DeclareLaunchArgument( + 'draw_markers', + default_value='False', + description="Draw markers" + ), + DeclareLaunchArgument( + 'use_closest', + default_value='True', + description="Use closest point for calibration" + ), + DeclareLaunchArgument( + 'lidar_frame', + default_value='lidar_frame', + description="TF frame of the lidar" + ), + Node( + package='toid_lidar', + executable='toid_lidar', + output="screen", + parameters=[ + lidar_config, + { + 'closest': use_closest, + 'draw_markers': draw_markers + }] + ), + Node( + package='rplidar_ros', + executable='rplidar_composition', + output="screen", + parameters=[lidar_config, {'frame_id': lidar_frame}] + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config], + condition=IfCondition(visualize) + ) + ]) diff --git a/toid_lidar/params/lidar.yaml b/toid_lidar/params/lidar.yaml new file mode 100644 index 0000000..e6db752 --- /dev/null +++ b/toid_lidar/params/lidar.yaml @@ -0,0 +1,11 @@ +rplidar_node: + ros__parameters: + frame_id: "lidar_frame" + +toid_lidar: + ros__parameters: + map_width: 3.0 + map_height: 2.0 + draw_makers: false + closest: true + \ No newline at end of file diff --git a/toid_lidar/rviz/conf.rviz b/toid_lidar/rviz/conf.rviz new file mode 100644 index 0000000..2c0ce6e --- /dev/null +++ b/toid_lidar/rviz/conf.rviz @@ -0,0 +1,175 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.71659916639328 + Tree Height: 1144 + - 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 + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rivalMaker + 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: 47 + Min Color: 0; 0; 0 + Min Intensity: 47 + 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 + 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: + Angle: -0.24500009417533875 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 152.6725311279297 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1662 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f000000574fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000005740000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000574fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000005740000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000005efc0100000002fb0000000800540069006d0065010000000000000b400000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000007dd0000057400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2880 + X: 0 + Y: 64