-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
ab372cd
commit 4ea576b
Showing
8 changed files
with
522 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| cmake_minimum_required(VERSION 3.5) | ||
| project(navyu_safety_limiter) | ||
|
|
||
| if(NOT CMAKE_CXX_STANDARD) | ||
| set(CMAKE_CXX_STANDARD 17) | ||
| set(CMAKE_CXX_STANDARD_REQUIRED ON) | ||
| set(CMAKE_CXX_EXTENSIONS OFF) | ||
| endif() | ||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic ) | ||
| endif() | ||
|
|
||
| find_package(backward_ros REQUIRED) | ||
| find_package(ament_cmake_auto REQUIRED) | ||
| ament_auto_find_build_dependencies() | ||
|
|
||
| ament_auto_add_library(${PROJECT_NAME} SHARED | ||
| src/${PROJECT_NAME}.cpp | ||
| ) | ||
|
|
||
| rclcpp_components_register_node(${PROJECT_NAME} | ||
| PLUGIN "NavyuSafetyLimiter" | ||
| EXECUTABLE ${PROJECT_NAME}_node | ||
| ) | ||
|
|
||
| add_backward(${PROJECT_NAME}) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() | ||
|
|
||
| ament_auto_package( | ||
| INSTALL_TO_SHARE | ||
| launch | ||
| config | ||
| ) | ||
|
|
||
| install(FILES DESTINATION share/${PROJECT_NAME}) |
11 changes: 11 additions & 0 deletions
11
navyu_safety_limiter/config/navyu_safety_limiter_params.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,11 @@ | ||
| safety_limiter_node: | ||
| ros__parameters: | ||
| base_frame: base_link | ||
| slowdown_ratio: 0.45 | ||
| collision_points_threshold_in_polygon: 6 | ||
| scan_timeout: 1.0 | ||
| robot_radius: 0.5 | ||
| length: 0.5 | ||
| scale: 1.5 | ||
| slowdown_polygon: [-0.4, 0.5, -0.4, -0.5, 0.7, -0.5, 0.7, 0.5] | ||
| stop_polygon: [-0.3, 0.4, -0.3, -0.4, 0.5, -0.4, 0.5, 0.4] |
121 changes: 121 additions & 0 deletions
121
navyu_safety_limiter/include/navyu_safety_limiter/navyu_safety_limiter.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,121 @@ | ||
| // Copyright 2024 RyuYamamoto. | ||
| // | ||
| // 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 | ||
|
|
||
| #ifndef NAVYU_SAFETY_LIMITER__NAVYU_SAFETY_LIMITER_HPP_ | ||
| #define NAVYU_SAFETY_LIMITER__NAVYU_SAFETY_LIMITER_HPP_ | ||
|
|
||
| #include <laser_geometry/laser_geometry.hpp> | ||
| #include <rclcpp/rclcpp.hpp> | ||
| #include <tf2_eigen/tf2_eigen.hpp> | ||
|
|
||
| #include <geometry_msgs/msg/polygon.hpp> | ||
| #include <geometry_msgs/msg/polygon_stamped.hpp> | ||
| #include <geometry_msgs/msg/twist.hpp> | ||
| #include <sensor_msgs/msg/laser_scan.hpp> | ||
| #include <visualization_msgs/msg/marker.hpp> | ||
| #include <visualization_msgs/msg/marker_array.hpp> | ||
|
|
||
| #include <pcl/common/transforms.h> | ||
| #include <pcl/point_cloud.h> | ||
| #include <pcl/point_types.h> | ||
| #include <pcl_conversions/pcl_conversions.h> | ||
| #include <tf2_ros/buffer.h> | ||
| #include <tf2_ros/transform_listener.h> | ||
|
|
||
| enum Type { | ||
| STOP = 0, | ||
| SLOWDOWN, | ||
| }; | ||
|
|
||
| struct Polygon | ||
| { | ||
| std::string name_; | ||
| bool update_dynamic_{false}; | ||
| double length_; | ||
| double scale_; | ||
| double robot_radius_; | ||
| geometry_msgs::msg::Polygon polygon_; | ||
| Type type_; | ||
|
|
||
| geometry_msgs::msg::Polygon update_polygon(geometry_msgs::msg::Twist twist) | ||
| { | ||
| if (!update_dynamic_) return polygon_; | ||
|
|
||
| geometry_msgs::msg::Polygon polygon; | ||
|
|
||
| geometry_msgs::msg::Point32 p1, p2, p3, p4; | ||
| p1.x = 0.0; | ||
| p1.y = robot_radius_; | ||
| p2.x = 0.0; | ||
| p2.y = -1 * robot_radius_; | ||
| p3.x = length_ + twist.linear.x * scale_; | ||
| p3.y = -1 * robot_radius_; | ||
| p4.x = length_ + twist.linear.x * scale_; | ||
| p4.y = robot_radius_; | ||
|
|
||
| polygon.points.emplace_back(p1); | ||
| polygon.points.emplace_back(p2); | ||
| polygon.points.emplace_back(p3); | ||
| polygon.points.emplace_back(p4); | ||
|
|
||
| return polygon; | ||
| } | ||
| }; | ||
|
|
||
| using PolygonPublisher = rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr; | ||
|
|
||
| class NavyuSafetyLimiter : public rclcpp::Node | ||
| { | ||
| public: | ||
| explicit NavyuSafetyLimiter(const rclcpp::NodeOptions & node_options); | ||
| ~NavyuSafetyLimiter(); | ||
|
|
||
| void callback_laser_scan(const sensor_msgs::msg::LaserScan::SharedPtr msg); | ||
| void callback_cmd_vel(const geometry_msgs::msg::Twist msg); | ||
|
|
||
| geometry_msgs::msg::Polygon create_polygon(std::vector<double> vertex); | ||
|
|
||
| bool check_collision_points_in_polygon( | ||
| const geometry_msgs::msg::Polygon polygon, | ||
| const pcl::PointCloud<pcl::PointXYZ>::Ptr collison_points); | ||
|
|
||
| bool get_transform( | ||
| const std::string target_frame, const std::string source_frame, | ||
| geometry_msgs::msg::TransformStamped & frame); | ||
|
|
||
| private: | ||
| rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_subscriber_; | ||
| rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_subscriber_; | ||
| rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_publisher_; | ||
| rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr current_state_marker_publisher_; | ||
| std::map<std::string, PolygonPublisher> polygon_pubilsher_; | ||
|
|
||
| tf2_ros::Buffer tf_buffer_{get_clock()}; | ||
| tf2_ros::TransformListener listener_{tf_buffer_}; | ||
|
|
||
| laser_geometry::LaserProjection projection_; | ||
|
|
||
| geometry_msgs::msg::Twist cmd_vel_in_; | ||
| geometry_msgs::msg::Twist current_control_twist_; | ||
|
|
||
| std::vector<Polygon> polygons_; | ||
|
|
||
| std::string base_frame_; | ||
| double slowdown_ratio_; | ||
| int collision_points_threshold_in_polygon_; | ||
|
|
||
| std::string current_state_{"NONE"}; | ||
| }; | ||
|
|
||
| #endif |
43 changes: 43 additions & 0 deletions
43
navyu_safety_limiter/launch/navyu_safety_limiter.launch.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,43 @@ | ||
| # Copyright 2024 RyuYamamoto. | ||
| # | ||
| # 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 launch_ros.actions import Node | ||
|
|
||
| from launch import LaunchDescription | ||
| from launch.actions import DeclareLaunchArgument | ||
| from launch.substitutions import LaunchConfiguration | ||
| from launch.substitutions import PathJoinSubstitution | ||
| from launch_ros.substitutions import FindPackageShare | ||
|
|
||
|
|
||
| def generate_launch_description(): | ||
| use_sim_time = LaunchConfiguration("use_sim_time", default="true") | ||
|
|
||
| safety_limiter_config_path = PathJoinSubstitution( | ||
| [FindPackageShare("navyu_safety_limiter"), "config", "navyu_safety_limiter_params.yaml"] | ||
| ) | ||
|
|
||
| return LaunchDescription( | ||
| [ | ||
| DeclareLaunchArgument("use_sim_time", default_value="true"), | ||
| Node( | ||
| package="navyu_safety_limiter", | ||
| executable="navyu_safety_limiter_node", | ||
| name="safety_limiter_node", | ||
| output="screen", | ||
| remappings=[("/cmd_vel_out", "cmd_vel")], | ||
| parameters=[safety_limiter_config_path, {"use_sim_time": use_sim_time}], | ||
| ), | ||
| ] | ||
| ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,31 @@ | ||
| <?xml version="1.0"?> | ||
| <package format="3"> | ||
| <name>navyu_safety_limiter</name> | ||
| <version>0.1.0</version> | ||
| <description>The navyu_safety_limiter package</description> | ||
| <maintainer email="[email protected]">RyuYamamoto</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
| <buildtool_depend>ament_cmake_auto</buildtool_depend> | ||
|
|
||
| <depend>geometry_msgs</depend> | ||
| <depend>nav_msgs</depend> | ||
| <depend>rclcpp</depend> | ||
| <depend>rclcpp_components</depend> | ||
| <depend>sensor_msgs</depend> | ||
| <depend>laser_geometry</depend> | ||
| <depend>tf2</depend> | ||
| <depend>tf2_eigen</depend> | ||
| <depend>tf2_geometry_msgs</depend> | ||
| <depend>tf2_ros</depend> | ||
| <depend>pcl_ros</depend> | ||
| <depend>visualization_msgs</depend> | ||
|
|
||
| <exec_depend>launch_ros</exec_depend> | ||
| <test_depend>ament_lint_auto</test_depend> | ||
| <test_depend>ament_lint_common</test_depend> | ||
|
|
||
| <export> | ||
| <build_type>ament_cmake</build_type> | ||
| </export> | ||
| </package> |
Oops, something went wrong.