forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 31
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(control): add autonomous emergency braking module (backport auto…
…warefoundation#2793, autowarefoundation#3186, autowarefoundation#3248, autowarefoundation#3292) (#330) * feat(control): add autonomous emergency braking module (autowarefoundation#2793) * fix build error * ci(pre-commit): autofix * fix: fix lookup symbol error * fix(autonomous_emergency_braking): fix typo (autowarefoundation#3186) Update node.cpp * fix(autonomous_emergency_braking): publish debug marker on base_link frame (autowarefoundation#3248) * feat(autonomous_emergency_braking): keep collision information for user specified seconds (autowarefoundation#3292) * feat(autonomous_emergency_braking): keep collision information for user specified seconds * style(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Co-authored-by: Yutaka Shimizu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
- Loading branch information
1 parent
7123a7f
commit 2ed77fc
Showing
14 changed files
with
971 additions
and
0 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
34 changes: 34 additions & 0 deletions
34
common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.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,34 @@ | ||
// Copyright 2022 TIER IV, Inc. | ||
// | ||
// 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 TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ | ||
#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ | ||
|
||
#include "tier4_autoware_utils/geometry/boost_geometry.hpp" | ||
|
||
#include <geometry_msgs/msg/pose.hpp> | ||
|
||
#include <boost/geometry.hpp> | ||
#include <boost/optional.hpp> | ||
|
||
#include <vector> | ||
|
||
namespace tier4_autoware_utils | ||
{ | ||
bool isClockwise(const Polygon2d & polygon); | ||
Polygon2d inverseClockwise(const Polygon2d & polygon); | ||
|
||
} // namespace tier4_autoware_utils | ||
|
||
#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_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
44 changes: 44 additions & 0 deletions
44
common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp
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,44 @@ | ||
// Copyright 2022 TIER IV, Inc. | ||
// | ||
// 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. | ||
|
||
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" | ||
|
||
#include "tier4_autoware_utils/geometry/geometry.hpp" | ||
|
||
#include <tf2/utils.h> | ||
|
||
namespace tier4_autoware_utils | ||
{ | ||
bool isClockwise(const Polygon2d & polygon) | ||
{ | ||
const int n = polygon.outer().size(); | ||
const double x_offset = polygon.outer().at(0).x(); | ||
const double y_offset = polygon.outer().at(0).y(); | ||
double sum = 0.0; | ||
for (std::size_t i = 0; i < polygon.outer().size(); ++i) { | ||
sum += | ||
(polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - | ||
(polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); | ||
} | ||
|
||
return sum < 0.0; | ||
} | ||
|
||
Polygon2d inverseClockwise(const Polygon2d & polygon) | ||
{ | ||
auto output_polygon = polygon; | ||
boost::geometry::reverse(output_polygon); | ||
return output_polygon; | ||
} | ||
} // namespace tier4_autoware_utils |
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,26 @@ | ||
cmake_minimum_required(VERSION 3.14) | ||
project(autonomous_emergency_braking) | ||
|
||
find_package(autoware_cmake REQUIRED) | ||
autoware_package() | ||
|
||
set(AEB_NODE ${PROJECT_NAME}_node) | ||
ament_auto_add_library(${AEB_NODE} SHARED | ||
src/node.cpp | ||
) | ||
|
||
rclcpp_components_register_node(${AEB_NODE} | ||
PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" | ||
EXECUTABLE ${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 | ||
) |
17 changes: 17 additions & 0 deletions
17
control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.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,17 @@ | ||
/**: | ||
ros__parameters: | ||
use_predicted_trajectory: true | ||
use_imu_path: true | ||
voxel_grid_x: 0.05 | ||
voxel_grid_y: 0.05 | ||
voxel_grid_z: 100000.0 | ||
min_generated_path_length: 0.5 | ||
expand_width: 0.1 | ||
longitudinal_offset: 2.0 | ||
t_response: 1.0 | ||
a_ego_min: -3.0 | ||
a_obj_min: -1.0 | ||
prediction_time_horizon: 1.5 | ||
prediction_time_interval: 0.1 | ||
collision_keeping_sec: 0.0 | ||
aeb_hz: 10.0 |
190 changes: 190 additions & 0 deletions
190
control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.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,190 @@ | ||
// Copyright 2023 TIER IV, Inc. | ||
// | ||
// 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 AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ | ||
#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ | ||
|
||
#include <diagnostic_updater/diagnostic_updater.hpp> | ||
#include <pcl_ros/transforms.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
#include <tier4_autoware_utils/tier4_autoware_utils.hpp> | ||
#include <vehicle_info_util/vehicle_info_util.hpp> | ||
|
||
#include <autoware_auto_planning_msgs/msg/trajectory.hpp> | ||
#include <autoware_auto_system_msgs/msg/autoware_state.hpp> | ||
#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp> | ||
#include <nav_msgs/msg/odometry.hpp> | ||
#include <sensor_msgs/msg/imu.hpp> | ||
#include <sensor_msgs/msg/point_cloud2.hpp> | ||
#include <visualization_msgs/msg/marker.hpp> | ||
|
||
#include <boost/optional.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> | ||
|
||
#include <memory> | ||
#include <mutex> | ||
#include <string> | ||
#include <vector> | ||
|
||
namespace autoware::motion::control::autonomous_emergency_braking | ||
{ | ||
|
||
using autoware_auto_planning_msgs::msg::Trajectory; | ||
using autoware_auto_system_msgs::msg::AutowareState; | ||
using autoware_auto_vehicle_msgs::msg::VelocityReport; | ||
using nav_msgs::msg::Odometry; | ||
using sensor_msgs::msg::Imu; | ||
using sensor_msgs::msg::PointCloud2; | ||
using PointCloud = pcl::PointCloud<pcl::PointXYZ>; | ||
using diagnostic_updater::DiagnosticStatusWrapper; | ||
using diagnostic_updater::Updater; | ||
using tier4_autoware_utils::Point2d; | ||
using tier4_autoware_utils::Polygon2d; | ||
using vehicle_info_util::VehicleInfo; | ||
using visualization_msgs::msg::Marker; | ||
using visualization_msgs::msg::MarkerArray; | ||
using Path = std::vector<geometry_msgs::msg::Pose>; | ||
|
||
struct ObjectData | ||
{ | ||
rclcpp::Time stamp; | ||
geometry_msgs::msg::Point position; | ||
double velocity{0.0}; | ||
double rss{0.0}; | ||
double distance_to_object{0.0}; | ||
}; | ||
|
||
class CollisionDataKeeper | ||
{ | ||
public: | ||
explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } | ||
|
||
void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; } | ||
|
||
bool checkExpired() | ||
{ | ||
if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) { | ||
data_.reset(); | ||
} | ||
return (data_ == nullptr); | ||
} | ||
|
||
void update(const ObjectData & data) { data_.reset(new ObjectData(data)); } | ||
|
||
ObjectData get() | ||
{ | ||
if (data_) { | ||
return *data_; | ||
} else { | ||
return ObjectData(); | ||
} | ||
} | ||
|
||
private: | ||
std::unique_ptr<ObjectData> data_; | ||
double timeout_sec_{0.0}; | ||
rclcpp::Clock::SharedPtr clock_; | ||
}; | ||
|
||
class AEB : public rclcpp::Node | ||
{ | ||
public: | ||
explicit AEB(const rclcpp::NodeOptions & node_options); | ||
|
||
// subscriber | ||
rclcpp::Subscription<PointCloud2>::SharedPtr sub_point_cloud_; | ||
rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_; | ||
rclcpp::Subscription<Imu>::SharedPtr sub_imu_; | ||
rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_; | ||
rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_; | ||
|
||
// publisher | ||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_; | ||
rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_; // debug | ||
|
||
// timer | ||
rclcpp::TimerBase::SharedPtr timer_; | ||
|
||
// callback | ||
void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); | ||
void onVelocity(const VelocityReport::ConstSharedPtr input_msg); | ||
void onImu(const Imu::ConstSharedPtr input_msg); | ||
void onTimer(); | ||
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); | ||
void onAutowareState(const AutowareState::ConstSharedPtr input_msg); | ||
|
||
bool isDataReady(); | ||
|
||
// main function | ||
void onCheckCollision(DiagnosticStatusWrapper & stat); | ||
bool checkCollision(MarkerArray & debug_markers); | ||
bool hasCollision( | ||
const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects); | ||
|
||
void generateEgoPath( | ||
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons); | ||
void generateEgoPath( | ||
const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons); | ||
void createObjectData( | ||
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp, | ||
std::vector<ObjectData> & objects); | ||
|
||
void addMarker( | ||
const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons, | ||
const std::vector<ObjectData> & objects, const double color_r, const double color_g, | ||
const double color_b, const double color_a, const std::string & ns, | ||
MarkerArray & debug_markers); | ||
|
||
void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); | ||
|
||
PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; | ||
VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; | ||
Imu::ConstSharedPtr imu_ptr_{nullptr}; | ||
Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; | ||
AutowareState::ConstSharedPtr autoware_state_{nullptr}; | ||
|
||
tf2_ros::Buffer tf_buffer_{get_clock()}; | ||
tf2_ros::TransformListener tf_listener_{tf_buffer_}; | ||
|
||
// vehicle info | ||
VehicleInfo vehicle_info_; | ||
|
||
// diag | ||
Updater updater_{this}; | ||
|
||
// member variables | ||
bool use_predicted_trajectory_; | ||
bool use_imu_path_; | ||
double voxel_grid_x_; | ||
double voxel_grid_y_; | ||
double voxel_grid_z_; | ||
double min_generated_path_length_; | ||
double expand_width_; | ||
double longitudinal_offset_; | ||
double t_response_; | ||
double a_ego_min_; | ||
double a_obj_min_; | ||
double prediction_time_horizon_; | ||
double prediction_time_interval_; | ||
CollisionDataKeeper collision_data_keeper_; | ||
}; | ||
} // namespace autoware::motion::control::autonomous_emergency_braking | ||
|
||
#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ |
19 changes: 19 additions & 0 deletions
19
control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml
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,19 @@ | ||
<launch> | ||
<arg name="param_path" default="$(find-pkg-share autonomous_emergency_braking)/config/autonomous_emergency_braking.param.yaml"/> | ||
<arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/> | ||
<arg name="input_velocity" default="/vehicle/status/velocity_status"/> | ||
<arg name="input_imu" default="/sensing/imu/imu_data"/> | ||
<arg name="input_odometry" default="/localization/kinematic_state"/> | ||
<arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/> | ||
|
||
<node pkg="autonomous_emergency_braking" exec="autonomous_emergency_braking" name="autonomous_emergency_braking" output="screen"> | ||
<!-- load config files --> | ||
<param from="$(var param_path)"/> | ||
<!-- remap topic name --> | ||
<remap from="~/input/pointcloud" to="$(var input_pointcloud)"/> | ||
<remap from="~/input/velocity" to="$(var input_velocity)"/> | ||
<remap from="~/input/imu" to="$(var input_imu)"/> | ||
<remap from="~/input/odometry" to="$(var input_odometry)"/> | ||
<remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/> | ||
</node> | ||
</launch> |
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,41 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>autonomous_emergency_braking</name> | ||
<version>0.1.0</version> | ||
<description>Autonomous Emergency Braking package as a ROS2 node</description> | ||
<maintainer email="[email protected]">yutaka</maintainer> | ||
<license>Apache License 2.0</license> | ||
|
||
<buildtool_depend>ament_cmake</buildtool_depend> | ||
|
||
<build_depend>autoware_cmake</build_depend> | ||
|
||
<depend>autoware_auto_control_msgs</depend> | ||
<depend>autoware_auto_system_msgs</depend> | ||
<depend>autoware_auto_vehicle_msgs</depend> | ||
<depend>diagnostic_updater</depend> | ||
<depend>geometry_msgs</depend> | ||
<depend>nav_msgs</depend> | ||
<depend>pcl_conversions</depend> | ||
<depend>pcl_ros</depend> | ||
<depend>pointcloud_preprocessor</depend> | ||
<depend>rclcpp</depend> | ||
<depend>rclcpp_components</depend> | ||
<depend>sensor_msgs</depend> | ||
<depend>std_msgs</depend> | ||
<depend>tf2</depend> | ||
<depend>tf2_eigen</depend> | ||
<depend>tf2_geometry_msgs</depend> | ||
<depend>tf2_ros</depend> | ||
<depend>tier4_autoware_utils</depend> | ||
<depend>vehicle_info_util</depend> | ||
<depend>visualization_msgs</depend> | ||
|
||
<test_depend>ament_lint_auto</test_depend> | ||
<test_depend>autoware_lint_common</test_depend> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
Oops, something went wrong.