├── LICENSE ├── README.ja.md ├── README.md ├── docs ├── mrp.png └── whill_auto_stop_node_graph.png └── whill_auto_stop ├── CMakeLists.txt ├── README.md ├── config ├── VLP16-velodyne_driver_node-params.yaml ├── VLP16-velodyne_transform_node-params.yaml └── auto_stop_area.yaml ├── launch ├── bringup_launch.py └── velodyne_launch.py ├── package.xml ├── rviz └── debug.rviz └── src ├── laserscan_filter_node.cpp └── twist_filter_node.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 WHILL, Inc. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.ja.md: -------------------------------------------------------------------------------- 1 | # ros2_whill_applications 2 | 3 | WHILLの[電動モビリティプラットフォーム](https://whill-mrp.notion.site/WHILL-f975baf4015e4eebbb243a7d331efb0a)用ROS 2ドライバである[whill (whill-labs/ros2_whill)](https://github.com/whill-labs/ros2_whill/)を使ったアプリケーション実装例です。 4 | 5 | ![](./docs/mrp.png) 6 | 7 | 電動モビリティプラットフォーム以外にセンサ等の準備が必要になるサンプルもあります。 8 | 9 | ## Examples 10 | 11 | ### whill_auto_stop 12 | 13 | * 動作確認済みディストリビューション：Jazzy 14 | * 追加ハードウェア：あり 15 | 16 | TwistメッセージをSubscribeして走行します。その際、周囲の障害物に近づき過ぎたら停止するサンプルです。 17 | 18 | ロボット台車にOuster（旧Velodyne）社製 VLP-16を搭載しています。 19 | 20 | 詳細は[whill_auto_stop/README.md](whill_auto_stop/README.md)を参照ください。 21 | 22 | ## Usage 23 | 24 | ### 1. ROS 2のインストール 25 | 26 | 27 | #### 公式ドキュメントに沿って進める方法 28 | 29 | https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html を参考に、ROS 2 Jazzyをインストールします。 30 | 31 | また、 https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html を参考にビルドツールであるcolconをインストールします。 32 | 33 | #### インストールスクリプトを使って進める方法 34 | 35 | 有志が公開する（非公式の）インストールスクリプトでもインストールできます。 36 | その場合は下記のコマンドでROS 2 Jazzyをインストールします。 37 | 38 | ```sh 39 | git clone https://github.com/Tiryoh/ros2_setup_scripts_ubuntu.git 40 | cd ros2_setup_scripts_ubuntu 41 | ./ros2-jazzy-desktop-main.sh 42 | ``` 43 | 44 | ### 2. WHILLパッケージのインストール 45 | 46 | `$HOME/ros2_ws`を使う場合、下記のコマンドでインストールできます。 47 | 48 | ```sh 49 | # ダウンロード 50 | cd ~/ros2_ws/src 51 | git clone -b $ROS_DISTRO https://github.com/whill-labs/ros2_whill_interfaces.git 52 | git clone -b $ROS_DISTRO https://github.com/whill-labs/ros2_whill.git 53 | git clone https://github.com/whill-labs/ros2_whill_applications.git 54 | # ビルド 55 | cd ~/ros2_ws 56 | colcon build 57 | source install/setup.bash # zshの場合はsetup.zsh 58 | ``` 59 | 60 | ### 3. 各デバイスを接続する 61 | 62 | PCとロボット台車、LiDAR等を接続します。 63 | 64 | ### 4. launchファイルを起動する 65 | 66 | whill_auto_stopの場合は以下のコマンドを実行します。 67 | 68 | ``` 69 | ros2 launch whill_auto_stop bringup_launch.py 70 | ``` 71 | 72 | 詳細は[whill_auto_stop/README.md](whill_auto_stop/README.md)を参照ください。 73 | 74 | ## Contact 75 | 76 | Email: `mrp.contact[at]whill.inc` 77 | 78 | WHILLの電動モビリティプラットフォームについては下記サイトをご覧ください。 79 | 80 | [電動モビリティプラットフォーム](https://whill-mrp.notion.site/WHILL-f975baf4015e4eebbb243a7d331efb0a) 81 | 82 | ## LICENSE 83 | 84 | Copyright (c) 2024 WHILL, Inc. 85 | 86 | MITライセンスで公開しています。詳細は[LICENSE](./LICENSE)をご参照ください。 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | README.ja.md -------------------------------------------------------------------------------- /docs/mrp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whill-labs/ros2_whill_applications/1d8336219c7e54371f19e5f8ddc1520d7912210b/docs/mrp.png -------------------------------------------------------------------------------- /docs/whill_auto_stop_node_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whill-labs/ros2_whill_applications/1d8336219c7e54371f19e5f8ddc1520d7912210b/docs/whill_auto_stop_node_graph.png -------------------------------------------------------------------------------- /whill_auto_stop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(whill_auto_stop) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | find_package(tf2_geometry_msgs REQUIRED) 16 | find_package(tf2_ros REQUIRED) 17 | 18 | add_executable(twist_filter_node src/twist_filter_node.cpp) 19 | ament_target_dependencies(twist_filter_node 20 | geometry_msgs 21 | rclcpp 22 | std_msgs 23 | ) 24 | install(TARGETS twist_filter_node 25 | DESTINATION lib/${PROJECT_NAME} 26 | ) 27 | 28 | add_executable(laserscan_filter_node src/laserscan_filter_node.cpp) 29 | ament_target_dependencies(laserscan_filter_node 30 | geometry_msgs 31 | rclcpp 32 | sensor_msgs 33 | std_msgs 34 | tf2 35 | tf2_geometry_msgs 36 | tf2_ros 37 | ) 38 | install(TARGETS laserscan_filter_node 39 | DESTINATION lib/${PROJECT_NAME} 40 | ) 41 | 42 | install(DIRECTORY launch config rviz 43 | DESTINATION share/${PROJECT_NAME}) 44 | 45 | if(BUILD_TESTING) 46 | find_package(ament_lint_auto REQUIRED) 47 | set(ament_cmake_copyright_FOUND TRUE) 48 | set(ament_cmake_cpplint_FOUND TRUE) 49 | 50 | # uncurstifyの代わりにclang-formatを使う 51 | find_package(ament_cmake_clang_format REQUIRED) 52 | set(ament_cmake_uncrustify_FOUND TRUE) 53 | 54 | ament_lint_auto_find_test_dependencies() 55 | endif() 56 | 57 | ament_package() 58 | -------------------------------------------------------------------------------- /whill_auto_stop/README.md: -------------------------------------------------------------------------------- 1 | # whill_auto_stop 2 | 3 | * 動作確認済みディストリビューション：Jazzy 4 | * 追加ハードウェア：あり 5 | 6 | TwistメッセージをSubscribeして走行します。その際、周囲の障害物に近づき過ぎたら停止するサンプルです。 7 | 8 | ロボット台車にOuster（旧Velodyne）社製 VLP-16を搭載しています。 9 | 10 | https://github.com/user-attachments/assets/d6be0824-2cd3-4a8c-8105-cd8a80746ce9 11 | 12 | ## ノード構成 13 | 14 | ![](../docs/whill_auto_stop_node_graph.png) 15 | 16 | ## 使用方法 17 | 18 | VLP-16の設定を必要に応じて変更します 19 | 20 | * `config/VLP16-velodyne_driver_node-params.yaml` 21 | * `config/VLP16-velodyne_transform_node-params.yaml` 22 | 23 | `bringup_launch.py`を起動します 24 | 25 | ``` 26 | ros2 launch whill_auto_stop bringup_launch.py 27 | ``` 28 | -------------------------------------------------------------------------------- /whill_auto_stop/config/VLP16-velodyne_driver_node-params.yaml: -------------------------------------------------------------------------------- 1 | velodyne_driver_node: 2 | ros__parameters: 3 | device_ip: 192.168.67.202 4 | gps_time: false 5 | time_offset: 0.0 6 | enabled: true 7 | read_once: false 8 | read_fast: false 9 | repeat_delay: 0.0 10 | frame_id: velodyne 11 | model: VLP16 12 | rpm: 600.0 13 | port: 2268 14 | timestamp_first_packet: false 15 | -------------------------------------------------------------------------------- /whill_auto_stop/config/VLP16-velodyne_transform_node-params.yaml: -------------------------------------------------------------------------------- 1 | velodyne_transform_node: 2 | ros__parameters: 3 | calibration: VLP16db.yaml 4 | model: VLP16 5 | min_range: 0.4 6 | max_range: 130.0 7 | view_direction: 0.0 8 | fixed_frame: "" 9 | target_frame: "" 10 | organize_cloud: true 11 | -------------------------------------------------------------------------------- /whill_auto_stop/config/auto_stop_area.yaml: -------------------------------------------------------------------------------- 1 | /**/laserscan_filter_node: 2 | ros__parameters: 3 | reference_link: base_link 4 | forward: 5 | x_min: -0.3 6 | x_max: 1.0 7 | y_min: -0.5 8 | y_max: 0.5 9 | backward: 10 | x_min: -0.6 11 | x_max: 0.8 12 | y_min: -0.5 13 | y_max: 0.5 14 | -------------------------------------------------------------------------------- /whill_auto_stop/launch/bringup_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.conditions import IfCondition 8 | from launch.launch_description_sources import PythonLaunchDescriptionSource 9 | from launch.substitutions import LaunchConfiguration 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | declare_arg_rviz = DeclareLaunchArgument( 15 | 'rviz', default_value='true', description='Set true to launch RViz' 16 | ) 17 | 18 | whill_auto_stop_share_dir = get_package_share_directory( 19 | 'whill_auto_stop') 20 | 21 | whill_driver_launch = IncludeLaunchDescription( 22 | PythonLaunchDescriptionSource( 23 | [ 24 | os.path.join( 25 | get_package_share_directory('whill_bringup'), 'launch', 26 | 'whill_launch.py'), 27 | ] 28 | ), 29 | ) 30 | 31 | teleop_launch = IncludeLaunchDescription( 32 | PythonLaunchDescriptionSource( 33 | [ 34 | os.path.join( 35 | get_package_share_directory('teleop_twist_joy'), 'launch', 36 | 'teleop-launch.py'), 37 | ] 38 | ), 39 | launch_arguments={ 40 | 'joy_config': 'xbox', 41 | 'joy_vel': 'auto_stop/cmd_vel', 42 | }.items() 43 | ) 44 | 45 | velodyne_launch = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | [ 48 | os.path.join( 49 | whill_auto_stop_share_dir, 'launch', 50 | 'velodyne_launch.py'), 51 | ] 52 | ), 53 | ) 54 | 55 | laserscan_params_file = os.path.join( 56 | whill_auto_stop_share_dir, 'config', 'auto_stop_area.yaml') 57 | print(laserscan_params_file) 58 | laserscan_filter_node = Node( 59 | package='whill_auto_stop', 60 | executable='laserscan_filter_node', 61 | output='log', 62 | namespace='auto_stop', 63 | parameters=[laserscan_params_file], 64 | remappings=[ 65 | ('scan', '/scan'), 66 | ], 67 | ) 68 | 69 | twist_filter_node = Node( 70 | package='whill_auto_stop', 71 | executable='twist_filter_node', 72 | output='both', 73 | namespace='auto_stop', 74 | parameters=[{'negative_logic': True}], 75 | remappings=[ 76 | ('enable', 'scan_in_range'), 77 | ('output_twist', '/whill/controller/cmd_vel'), 78 | ('input_twist', 'cmd_vel'), 79 | ], 80 | ) 81 | 82 | rviz_node = Node( 83 | package='rviz2', 84 | executable='rviz2', 85 | output='screen', 86 | arguments=['-d', os.path.join( 87 | whill_auto_stop_share_dir, 'rviz', 'debug.rviz')], 88 | condition=IfCondition(LaunchConfiguration('rviz')) 89 | ) 90 | 91 | ld = LaunchDescription() 92 | 93 | ld.add_action(declare_arg_rviz) 94 | ld.add_action(teleop_launch) 95 | ld.add_action(velodyne_launch) 96 | ld.add_action(whill_driver_launch) 97 | ld.add_action(laserscan_filter_node) 98 | ld.add_action(twist_filter_node) 99 | ld.add_action(rviz_node) 100 | 101 | return ld 102 | -------------------------------------------------------------------------------- /whill_auto_stop/launch/velodyne_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import yaml 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | import launch 7 | from launch import LaunchDescription 8 | from launch.actions import RegisterEventHandler, EmitEvent, IncludeLaunchDescription 9 | from launch.event_handlers import OnProcessExit 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | from launch_ros.actions import Node 12 | 13 | 14 | def generate_launch_description(): 15 | whill_auto_stop_share_dir = get_package_share_directory( 16 | 'whill_auto_stop') 17 | 18 | driver_params_file = os.path.join( 19 | whill_auto_stop_share_dir, 'config', 'VLP16-velodyne_driver_node-params.yaml') 20 | velodyne_driver_node = Node( 21 | package='velodyne_driver', 22 | executable='velodyne_driver_node', 23 | output='both', 24 | parameters=[driver_params_file] 25 | ) 26 | 27 | convert_share_dir = get_package_share_directory( 28 | 'velodyne_pointcloud') 29 | convert_params_file = os.path.join( 30 | whill_auto_stop_share_dir, 'config', 'VLP16-velodyne_transform_node-params.yaml') 31 | with open(convert_params_file, 'r') as f: 32 | convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] 33 | convert_params['calibration'] = os.path.join( 34 | convert_share_dir, 'params', 'VLP16db.yaml') 35 | velodyne_transform_node = Node( 36 | package='velodyne_pointcloud', 37 | executable='velodyne_transform_node', 38 | output='both', 39 | parameters=[convert_params] 40 | ) 41 | 42 | laserscan_share_dir = get_package_share_directory( 43 | 'velodyne_laserscan') 44 | laserscan_params_file = os.path.join( 45 | laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml') 46 | velodyne_laserscan_node = Node( 47 | package='velodyne_laserscan', 48 | executable='velodyne_laserscan_node', 49 | output='both', 50 | parameters=[laserscan_params_file] 51 | ) 52 | 53 | display_robot_launch = IncludeLaunchDescription( 54 | PythonLaunchDescriptionSource( 55 | [ 56 | os.path.join(get_package_share_directory('whill_description'), 'launch', 57 | 'description_launch.py'), 58 | ] 59 | ), 60 | ) 61 | 62 | static_transform_publisher_node = Node( 63 | package='tf2_ros', 64 | executable='static_transform_publisher', 65 | output='screen', 66 | arguments=['0.18', '0', '0.35', '0', '3.14', '3.14', 'base_link', 'velodyne'], 67 | ) 68 | 69 | event_handler = RegisterEventHandler( 70 | event_handler=OnProcessExit( 71 | target_action=velodyne_driver_node, 72 | on_exit=[EmitEvent( 73 | event=launch.events.Shutdown())], 74 | ) 75 | ) 76 | 77 | ld = LaunchDescription() 78 | 79 | ld.add_action(velodyne_driver_node) 80 | ld.add_action(velodyne_transform_node) 81 | ld.add_action(velodyne_laserscan_node) 82 | 83 | ld.add_action(display_robot_launch) 84 | ld.add_action(static_transform_publisher_node) 85 | 86 | ld.add_action(event_handler) 87 | 88 | return ld 89 | -------------------------------------------------------------------------------- /whill_auto_stop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | whill_auto_stop 5 | 0.0.0 6 | Example of implementation of "auto stop" to stop driving when an obstacle is approaching 7 | Daisuke Sato 8 | George Mandokoro 9 | MIT 10 | 11 | ament_cmake 12 | rclcpp 13 | geometry_msgs 14 | std_msgs 15 | sensor_msgs 16 | tf2 17 | tf2_geometry_msgs 18 | tf2_ros 19 | velodyne_driver 20 | velodyne_laserscan 21 | velodyne_pointcloud 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | ament_cmake_clang_format 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /whill_auto_stop/rviz/debug.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 555 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: AutostopLaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Class: rviz_default_plugins/RobotModel 52 | Collision Enabled: false 53 | Description File: "" 54 | Description Source: Topic 55 | Description Topic: 56 | Depth: 5 57 | Durability Policy: Volatile 58 | History Policy: Keep Last 59 | Reliability Policy: Reliable 60 | Value: /robot_description 61 | Enabled: true 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: Links in Alphabetic Order 68 | backrest: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | base_floor: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | base_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | left_arm: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | left_front_wheel: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | left_sensor_arm: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | left_wheel: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | right_arm: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | right_front_wheel: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | right_sensor_arm: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | right_wheel: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | seat: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | Mass Properties: 129 | Inertia: false 130 | Mass: false 131 | Name: RobotModel 132 | TF Prefix: "" 133 | Update Interval: 0 134 | Value: true 135 | Visual Enabled: true 136 | - Class: rviz_default_plugins/TF 137 | Enabled: true 138 | Filter (blacklist): "" 139 | Filter (whitelist): "" 140 | Frame Timeout: 15 141 | Frames: 142 | All Enabled: true 143 | backrest: 144 | Value: true 145 | base_floor: 146 | Value: true 147 | base_link: 148 | Value: true 149 | left_arm: 150 | Value: true 151 | left_front_wheel: 152 | Value: true 153 | left_sensor_arm: 154 | Value: true 155 | left_wheel: 156 | Value: true 157 | right_arm: 158 | Value: true 159 | right_front_wheel: 160 | Value: true 161 | right_sensor_arm: 162 | Value: true 163 | right_wheel: 164 | Value: true 165 | seat: 166 | Value: true 167 | velodyne: 168 | Value: true 169 | Marker Scale: 1 170 | Name: TF 171 | Show Arrows: true 172 | Show Axes: true 173 | Show Names: false 174 | Tree: 175 | base_link: 176 | base_floor: 177 | backrest: 178 | {} 179 | left_front_wheel: 180 | {} 181 | left_wheel: 182 | {} 183 | right_front_wheel: 184 | {} 185 | right_wheel: 186 | {} 187 | seat: 188 | left_arm: 189 | {} 190 | left_sensor_arm: 191 | {} 192 | right_arm: 193 | {} 194 | right_sensor_arm: 195 | {} 196 | velodyne: 197 | {} 198 | Update Interval: 0 199 | Value: true 200 | - Alpha: 1 201 | Autocompute Intensity Bounds: true 202 | Autocompute Value Bounds: 203 | Max Value: 10 204 | Min Value: -10 205 | Value: true 206 | Axis: Z 207 | Channel Name: intensity 208 | Class: rviz_default_plugins/LaserScan 209 | Color: 255; 255; 255 210 | Color Transformer: Intensity 211 | Decay Time: 0 212 | Enabled: true 213 | Invert Rainbow: false 214 | Max Color: 255; 255; 255 215 | Max Intensity: -999999 216 | Min Color: 255; 0; 0 217 | Min Intensity: 999999 218 | Name: AutostopLaserScan 219 | Position Transformer: XYZ 220 | Selectable: true 221 | Size (Pixels): 3 222 | Size (m): 0.10000000149011612 223 | Style: Flat Squares 224 | Topic: 225 | Depth: 5 226 | Durability Policy: Volatile 227 | Filter size: 10 228 | History Policy: Keep Last 229 | Reliability Policy: Reliable 230 | Value: /auto_stop/detected_points 231 | Use Fixed Frame: true 232 | Use rainbow: false 233 | Value: true 234 | - Alpha: 1 235 | Class: rviz_default_plugins/Polygon 236 | Color: 25; 255; 0 237 | Enabled: true 238 | Name: Polygon 239 | Topic: 240 | Depth: 5 241 | Durability Policy: Volatile 242 | Filter size: 10 243 | History Policy: Keep Last 244 | Reliability Policy: Reliable 245 | Value: /auto_stop/scan_range_polygon 246 | Value: true 247 | - Alpha: 1 248 | Autocompute Intensity Bounds: true 249 | Autocompute Value Bounds: 250 | Max Value: 10 251 | Min Value: -10 252 | Value: true 253 | Axis: Z 254 | Channel Name: intensity 255 | Class: rviz_default_plugins/LaserScan 256 | Color: 255; 255; 255 257 | Color Transformer: Intensity 258 | Decay Time: 0 259 | Enabled: true 260 | Invert Rainbow: false 261 | Max Color: 255; 255; 255 262 | Max Intensity: 224 263 | Min Color: 0; 0; 0 264 | Min Intensity: 1 265 | Name: LaserScan 266 | Position Transformer: XYZ 267 | Selectable: true 268 | Size (Pixels): 3 269 | Size (m): 0.009999999776482582 270 | Style: Flat Squares 271 | Topic: 272 | Depth: 5 273 | Durability Policy: Volatile 274 | Filter size: 10 275 | History Policy: Keep Last 276 | Reliability Policy: Reliable 277 | Value: /scan 278 | Use Fixed Frame: true 279 | Use rainbow: true 280 | Value: true 281 | - Alpha: 1 282 | Autocompute Intensity Bounds: true 283 | Autocompute Value Bounds: 284 | Max Value: 10 285 | Min Value: -10 286 | Value: true 287 | Axis: Z 288 | Channel Name: intensity 289 | Class: rviz_default_plugins/PointCloud2 290 | Color: 255; 255; 255 291 | Color Transformer: Intensity 292 | Decay Time: 0 293 | Enabled: false 294 | Invert Rainbow: false 295 | Max Color: 255; 255; 255 296 | Max Intensity: 173 297 | Min Color: 0; 0; 0 298 | Min Intensity: 0 299 | Name: PointCloud2 300 | Position Transformer: XYZ 301 | Selectable: true 302 | Size (Pixels): 3 303 | Size (m): 0.009999999776482582 304 | Style: Flat Squares 305 | Topic: 306 | Depth: 5 307 | Durability Policy: Volatile 308 | History Policy: Keep Last 309 | Reliability Policy: Reliable 310 | Value: /velodyne_points 311 | Use Fixed Frame: true 312 | Use rainbow: true 313 | Value: false 314 | Enabled: true 315 | Global Options: 316 | Background Color: 48; 48; 48 317 | Fixed Frame: base_floor 318 | Frame Rate: 30 319 | Name: root 320 | Tools: 321 | - Class: rviz_default_plugins/Interact 322 | Hide Inactive Objects: true 323 | - Class: rviz_default_plugins/MoveCamera 324 | - Class: rviz_default_plugins/Select 325 | - Class: rviz_default_plugins/FocusCamera 326 | - Class: rviz_default_plugins/Measure 327 | Line color: 128; 128; 0 328 | - Class: rviz_default_plugins/SetInitialPose 329 | Covariance x: 0.25 330 | Covariance y: 0.25 331 | Covariance yaw: 0.06853891909122467 332 | Topic: 333 | Depth: 5 334 | Durability Policy: Volatile 335 | History Policy: Keep Last 336 | Reliability Policy: Reliable 337 | Value: /initialpose 338 | - Class: rviz_default_plugins/SetGoal 339 | Topic: 340 | Depth: 5 341 | Durability Policy: Volatile 342 | History Policy: Keep Last 343 | Reliability Policy: Reliable 344 | Value: /goal_pose 345 | - Class: rviz_default_plugins/PublishPoint 346 | Single click: true 347 | Topic: 348 | Depth: 5 349 | Durability Policy: Volatile 350 | History Policy: Keep Last 351 | Reliability Policy: Reliable 352 | Value: /clicked_point 353 | Transformation: 354 | Current: 355 | Class: rviz_default_plugins/TF 356 | Value: true 357 | Views: 358 | Current: 359 | Class: rviz_default_plugins/Orbit 360 | Distance: 6.196164608001709 361 | Enable Stereo Rendering: 362 | Stereo Eye Separation: 0.05999999865889549 363 | Stereo Focal Distance: 1 364 | Swap Stereo Eyes: false 365 | Value: false 366 | Focal Point: 367 | X: 0.381662517786026 368 | Y: -0.3795689344406128 369 | Z: 0.043379202485084534 370 | Focal Shape Fixed Size: true 371 | Focal Shape Size: 0.05000000074505806 372 | Invert Z Axis: false 373 | Name: Current View 374 | Near Clip Distance: 0.009999999776482582 375 | Pitch: 1.4697964191436768 376 | Target Frame: 377 | Value: Orbit (rviz) 378 | Yaw: 3.010402202606201 379 | Saved: ~ 380 | Window Geometry: 381 | Displays: 382 | collapsed: false 383 | Height: 846 384 | Hide Left Dock: false 385 | Hide Right Dock: true 386 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 387 | Selection: 388 | collapsed: false 389 | Time: 390 | collapsed: false 391 | Tool Properties: 392 | collapsed: false 393 | Views: 394 | collapsed: true 395 | Width: 1200 396 | X: 60 397 | Y: 249 398 | -------------------------------------------------------------------------------- /whill_auto_stop/src/laserscan_filter_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 WHILL, Inc. 2 | // Released under the MIT license 3 | // https://opensource.org/licenses/mit-license.php 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class LaserScanFilterNode : public rclcpp::Node { 20 | public: 21 | LaserScanFilterNode() : Node("laserscan_filter_node"), tf_buffer_(std::make_shared(RCL_ROS_TIME)), tf_listener_(tf_buffer_) { 22 | // Declare and get parameters 23 | this->declare_parameter("forward.x_min", -1.0); 24 | this->declare_parameter("forward.x_max", 1.0); 25 | this->declare_parameter("forward.y_min", -1.0); 26 | this->declare_parameter("forward.y_max", 1.0); 27 | this->declare_parameter("backward.x_min", -1.0); 28 | this->declare_parameter("backward.x_max", 1.0); 29 | this->declare_parameter("backward.y_min", -1.0); 30 | this->declare_parameter("backward.y_max", 1.0); 31 | this->declare_parameter("reference_link", "base_link"); 32 | 33 | this->get_parameter("reference_link", reference_link_); 34 | this->get_parameter("forward.x_min", forward_x_min_); 35 | this->get_parameter("forward.x_max", forward_x_max_); 36 | this->get_parameter("forward.y_min", forward_y_min_); 37 | this->get_parameter("forward.y_max", forward_y_max_); 38 | this->get_parameter("backward.x_min", backward_x_min_); 39 | this->get_parameter("backward.x_max", backward_x_max_); 40 | this->get_parameter("backward.y_min", backward_y_min_); 41 | this->get_parameter("backward.y_max", backward_y_max_); 42 | 43 | // Subscriber for LaserScan messages 44 | laser_scan_subscriber_ = this->create_subscription( 45 | "scan", 10, 46 | std::bind(&LaserScanFilterNode::laserScanCallback, this, std::placeholders::_1)); 47 | 48 | // Subscriber for velocity messages 49 | velocity_subscriber_ = this->create_subscription( 50 | "cmd_vel", 10, 51 | std::bind(&LaserScanFilterNode::velocityCallback, this, std::placeholders::_1)); 52 | 53 | // Publisher for Bool messages 54 | bool_publisher_ = this->create_publisher("scan_in_range", 10); 55 | 56 | // Publisher for detected points 57 | detected_points_publisher_ = this->create_publisher("detected_points", 10); 58 | 59 | // Publisher for polygon visualization 60 | polygon_publisher_ = this->create_publisher("scan_range_polygon", 10); 61 | 62 | RCLCPP_INFO(this->get_logger(), "Laser Scan Filter Node has been started."); 63 | RCLCPP_INFO(this->get_logger(), "Reference link is set to: %s", reference_link_.c_str()); 64 | 65 | } 66 | 67 | private: 68 | double getYawFromQuaternion(const geometry_msgs::msg::Quaternion &quat_msg) 69 | { 70 | tf2::Quaternion quat; 71 | tf2::fromMsg(quat_msg, quat); 72 | 73 | double roll, pitch, yaw; 74 | tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); 75 | 76 | return yaw; 77 | } 78 | 79 | void velocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { 80 | // Check the linear x velocity to determine forward or bakward direction 81 | is_forward_ = (msg->linear.x >= 0); 82 | linear_x_ = msg->linear.x; 83 | RCLCPP_DEBUG(this->get_logger(), "Direction: %s", is_forward_ ? "Forward" : "Backward"); 84 | } 85 | 86 | void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { 87 | bool in_range = false; 88 | 89 | // Select appropriate bounds based on direction 90 | double x_min = is_forward_ ? forward_x_min_ : backward_x_min_ + linear_x_; 91 | double x_max = is_forward_ ? forward_x_max_ + linear_x_ : backward_x_max_; 92 | double y_min = is_forward_ ? forward_y_min_ : backward_y_min_; 93 | double y_max = is_forward_ ? forward_y_max_ : backward_y_max_; 94 | 95 | publishPolygon(x_min, x_max, y_min, y_max); 96 | 97 | geometry_msgs::msg::TransformStamped transform_stamped; 98 | try { 99 | transform_stamped = tf_buffer_.lookupTransform(reference_link_, msg->header.frame_id, tf2::TimePointZero); 100 | } catch (tf2::TransformException &ex) { 101 | RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", msg->header.frame_id.c_str(), reference_link_.c_str(), ex.what()); 102 | return; 103 | } 104 | 105 | auto detected_points_msg = std::make_shared(); 106 | *detected_points_msg = *msg; 107 | detected_points_msg->ranges.clear(); 108 | detected_points_msg->intensities.clear(); 109 | 110 | // Iterate over the LaserScan ranges 111 | for (size_t i = 0; i < msg->ranges.size(); ++i) { 112 | // Skip invalid ranges 113 | if (std::isnan(msg->ranges[i]) || std::isinf(msg->ranges[i])) { 114 | detected_points_msg->ranges.push_back(std::numeric_limits::quiet_NaN()); 115 | detected_points_msg->intensities.push_back(0.0); 116 | continue; 117 | } 118 | 119 | // Compute angle and position in local frame 120 | double angle = msg->angle_min + i * msg->angle_increment; 121 | double x_local = msg->ranges[i] * std::cos(angle); 122 | double y_local = msg->ranges[i] * std::sin(angle); 123 | 124 | // Get yaw angle from quaternion 125 | double yaw = getYawFromQuaternion(transform_stamped.transform.rotation); 126 | 127 | // Transform to reference frame 128 | double x_ref = transform_stamped.transform.translation.x + 129 | x_local * std::cos(yaw) - y_local * std::sin(yaw); 130 | double y_ref = transform_stamped.transform.translation.y + 131 | x_local * std::sin(yaw) + y_local * std::cos(yaw); 132 | 133 | // Check if the point is within the defined range 134 | if (x_ref >= x_min && x_ref <= x_max && y_ref >= y_min && y_ref <= y_max) { 135 | in_range = true; 136 | detected_points_msg->ranges.push_back(msg->ranges[i]); 137 | detected_points_msg->intensities.push_back(msg->intensities[i]); 138 | } else { 139 | detected_points_msg->ranges.push_back(std::numeric_limits::quiet_NaN()); 140 | detected_points_msg->intensities.push_back(0.0); 141 | } 142 | } 143 | 144 | // Publish the result 145 | std_msgs::msg::Bool bool_msg; 146 | bool_msg.data = in_range; 147 | bool_publisher_->publish(bool_msg); 148 | 149 | detected_points_publisher_->publish(*detected_points_msg); 150 | 151 | RCLCPP_INFO(this->get_logger(), "Scan in range: %s", in_range ? "True" : "False"); 152 | } 153 | 154 | void publishPolygon(double x_min, double x_max, double y_min, double y_max) { 155 | auto polygon_msg = geometry_msgs::msg::PolygonStamped(); 156 | polygon_msg.header.frame_id = reference_link_; 157 | polygon_msg.header.stamp = this->now(); 158 | 159 | geometry_msgs::msg::Point32 p1, p2, p3, p4; 160 | 161 | // Define the corners of the rectangle 162 | p1.x = x_min; p1.y = y_min; p1.z = 0.0; 163 | p2.x = x_max; p2.y = y_min; p2.z = 0.0; 164 | p3.x = x_max; p3.y = y_max; p3.z = 0.0; 165 | p4.x = x_min; p4.y = y_max; p4.z = 0.0; 166 | 167 | polygon_msg.polygon.points.push_back(p1); 168 | polygon_msg.polygon.points.push_back(p2); 169 | polygon_msg.polygon.points.push_back(p3); 170 | polygon_msg.polygon.points.push_back(p4); 171 | polygon_msg.polygon.points.push_back(p1); // Close the loop 172 | 173 | polygon_publisher_->publish(polygon_msg); 174 | 175 | RCLCPP_DEBUG(this->get_logger(), "Published scan range polygon."); 176 | } 177 | 178 | rclcpp::Subscription::SharedPtr laser_scan_subscriber_; 179 | rclcpp::Subscription::SharedPtr velocity_subscriber_; 180 | rclcpp::Publisher::SharedPtr bool_publisher_; 181 | rclcpp::Publisher::SharedPtr detected_points_publisher_; 182 | rclcpp::Publisher::SharedPtr polygon_publisher_; 183 | 184 | tf2_ros::Buffer tf_buffer_; 185 | tf2_ros::TransformListener tf_listener_; 186 | 187 | bool is_forward_; 188 | double linear_x_; 189 | double forward_x_min_, forward_x_max_, forward_y_min_, forward_y_max_; 190 | double backward_x_min_, backward_x_max_, backward_y_min_, backward_y_max_; 191 | std::string reference_link_; 192 | }; 193 | 194 | int main(int argc, char **argv) { 195 | rclcpp::init(argc, argv); 196 | rclcpp::spin(std::make_shared()); 197 | rclcpp::shutdown(); 198 | return 0; 199 | } 200 | -------------------------------------------------------------------------------- /whill_auto_stop/src/twist_filter_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 WHILL, Inc. 2 | // Released under the MIT license 3 | // https://opensource.org/licenses/mit-license.php 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | class TwistFilterNode : public rclcpp::Node { 10 | public: 11 | TwistFilterNode() : Node("twist_filter_node"), enable_publish_(false), negative_logic_(false) { 12 | // Declare and get negative_logic parameter 13 | this->declare_parameter("negative_logic", false); 14 | this->get_parameter("negative_logic", negative_logic_); 15 | 16 | // Subscriber for Twist messages 17 | twist_subscriber_ = this->create_subscription( 18 | "input_twist", 10, 19 | std::bind(&TwistFilterNode::twistCallback, this, std::placeholders::_1)); 20 | 21 | // Subscriber for bool messages 22 | bool_subscriber_ = this->create_subscription( 23 | "enable", 10, 24 | std::bind(&TwistFilterNode::boolCallback, this, std::placeholders::_1)); 25 | 26 | // Publisher for filtered Twist messages 27 | twist_publisher_ = this->create_publisher("output_twist", 10); 28 | 29 | RCLCPP_INFO(this->get_logger(), "Twist Filter Node has been started."); 30 | RCLCPP_INFO(this->get_logger(), "Negative logic is %s", negative_logic_ ? "enabled" : "disabled"); 31 | } 32 | 33 | private: 34 | void twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { 35 | if ((enable_publish_ && !negative_logic_) || (!enable_publish_ && negative_logic_)) { 36 | twist_publisher_->publish(*msg); 37 | RCLCPP_INFO(this->get_logger(), "Publishing Twist message."); 38 | } else { 39 | RCLCPP_INFO(this->get_logger(), "Publishing disabled, Twist message discarded."); 40 | } 41 | } 42 | 43 | void boolCallback(const std_msgs::msg::Bool::SharedPtr msg) { 44 | enable_publish_ = msg->data; 45 | RCLCPP_INFO(this->get_logger(), "Enable flag set to: %s", negative_logic_ ? (enable_publish_ ? "False" : "True") : (enable_publish_ ? "True" : "False")); 46 | } 47 | 48 | rclcpp::Subscription::SharedPtr twist_subscriber_; 49 | rclcpp::Subscription::SharedPtr bool_subscriber_; 50 | rclcpp::Publisher::SharedPtr twist_publisher_; 51 | 52 | bool enable_publish_; 53 | bool negative_logic_; 54 | }; 55 | 56 | int main(int argc, char **argv) { 57 | rclcpp::init(argc, argv); 58 | rclcpp::spin(std::make_shared()); 59 | rclcpp::shutdown(); 60 | return 0; 61 | } 62 | --------------------------------------------------------------------------------