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 (autowarefound…
- Loading branch information
1 parent
8d49017
commit d409e60
Showing
10 changed files
with
801 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
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 | ||
) |
16 changes: 16 additions & 0 deletions
16
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,16 @@ | ||
/**: | ||
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 | ||
aeb_hz: 10.0 |
150 changes: 150 additions & 0 deletions
150
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,150 @@ | ||
// 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 <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 | ||
{ | ||
geometry_msgs::msg::Point position; | ||
double velocity; | ||
}; | ||
|
||
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(); | ||
bool hasCollision( | ||
const double current_v, const Path & ego_path, const std::vector<Polygon2d> & ego_polys); | ||
|
||
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, | ||
std::vector<ObjectData> & objects); | ||
|
||
void addMarker( | ||
const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons, | ||
const double color_r, const double color_g, const double color_b, const double color_a, | ||
const std::string & path_ns, const std::string & poly_ns, 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_; | ||
}; | ||
} // 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,42 @@ | ||
<?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>motion_utils</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.