-
Notifications
You must be signed in to change notification settings - Fork 660
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(predicted_path_checker): check predicted trajectory to avoid co…
…llisions planning can not handle (#2528) Signed-off-by: Berkay Karaman <[email protected]>
- Loading branch information
Showing
13 changed files
with
2,149 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,37 @@ | ||
cmake_minimum_required(VERSION 3.14) | ||
project(predicted_path_checker) | ||
|
||
find_package(autoware_cmake REQUIRED) | ||
autoware_package() | ||
|
||
find_package(Eigen3 REQUIRED) | ||
|
||
|
||
|
||
ament_auto_add_library(predicted_path_checker SHARED | ||
src/predicted_path_checker_node/predicted_path_checker_node.cpp | ||
src/predicted_path_checker_node/collision_checker.cpp | ||
src/predicted_path_checker_node/utils.cpp | ||
src/predicted_path_checker_node/debug_marker.cpp | ||
|
||
) | ||
|
||
target_include_directories(predicted_path_checker | ||
SYSTEM PUBLIC | ||
${EIGEN3_INCLUDE_DIR} | ||
) | ||
|
||
rclcpp_components_register_node(predicted_path_checker | ||
PLUGIN "predicted_path_checker::PredictedPathCheckerNode" | ||
EXECUTABLE predicted_path_checker_node | ||
) | ||
|
||
if(BUILD_TESTING) | ||
|
||
endif() | ||
|
||
ament_auto_package( | ||
INSTALL_TO_SHARE | ||
launch | ||
config | ||
) |
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,23 @@ | ||
# predicted_path_checker | ||
|
||
## Purpose | ||
|
||
## Inner-workings / Algorithms | ||
|
||
### Flow chart | ||
|
||
### Algorithms | ||
|
||
### Check data | ||
|
||
### Diagnostic update | ||
|
||
## Inputs / Outputs | ||
|
||
### Input | ||
|
||
### Output | ||
|
||
## Parameters | ||
|
||
## Assumptions / Known limits |
21 changes: 21 additions & 0 deletions
21
control/predicted_path_checker/config/predicted_path_checker.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,21 @@ | ||
/**: | ||
ros__parameters: | ||
# Node | ||
update_rate: 10.0 | ||
max_deceleration: 1.5 | ||
delay_time: 0.17 | ||
ego_nearest_dist_threshold: 3.0 # [m] | ||
ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] | ||
stop_margin: 0.5 # [m] | ||
min_trajectory_check_length: 1.5 | ||
trajectory_check_time: 3.0 | ||
resample_interval: 0.5 | ||
distinct_point_distance_threshold: 0.3 | ||
distinct_point_yaw_threshold: 5.0 # [deg] | ||
|
||
collision_checker_params: | ||
width_margin: 0.1 | ||
longitudinal_margin: 0.15 | ||
enable_z_axis_obstacle_filtering: false | ||
z_axis_filtering_buffer: 0.3 | ||
chattering_threshold: 0.2 |
128 changes: 128 additions & 0 deletions
128
control/predicted_path_checker/include/predicted_path_checker/collision_checker.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,128 @@ | ||
// Copyright 2023 LeoDrive A.Ş. All rights reserved. | ||
// | ||
// 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 PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ | ||
#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ | ||
|
||
#include <motion_utils/trajectory/interpolation.hpp> | ||
#include <motion_utils/trajectory/tmp_conversion.hpp> | ||
#include <predicted_path_checker/debug_marker.hpp> | ||
#include <predicted_path_checker/utils.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
#include <tier4_autoware_utils/geometry/geometry.hpp> | ||
#include <tier4_autoware_utils/ros/debug_publisher.hpp> | ||
#include <tier4_autoware_utils/ros/transform_listener.hpp> | ||
#include <vehicle_info_util/vehicle_info.hpp> | ||
|
||
#include <geometry_msgs/msg/pose_stamped.hpp> | ||
#include <nav_msgs/msg/odometry.hpp> | ||
#include <visualization_msgs/msg/marker_array.hpp> | ||
|
||
#include <boost/assert.hpp> | ||
#include <boost/assign/list_of.hpp> | ||
#include <boost/geometry.hpp> | ||
|
||
#include <tf2_ros/buffer.h> | ||
#include <tf2_ros/transform_listener.h> | ||
|
||
#include <memory> | ||
#include <utility> | ||
#include <vector> | ||
|
||
namespace predicted_path_checker | ||
{ | ||
using autoware_auto_planning_msgs::msg::Trajectory; | ||
using autoware_auto_planning_msgs::msg::TrajectoryPoint; | ||
using TrajectoryPoints = std::vector<TrajectoryPoint>; | ||
using autoware_auto_perception_msgs::msg::PredictedObject; | ||
using autoware_auto_perception_msgs::msg::PredictedObjects; | ||
using geometry_msgs::msg::Pose; | ||
using geometry_msgs::msg::TransformStamped; | ||
using tier4_autoware_utils::Point2d; | ||
using tier4_autoware_utils::Polygon2d; | ||
using PointArray = std::vector<geometry_msgs::msg::Point>; | ||
|
||
namespace bg = boost::geometry; | ||
|
||
struct CollisionCheckerParam | ||
{ | ||
double width_margin; | ||
double z_axis_filtering_buffer; | ||
bool enable_z_axis_obstacle_filtering; | ||
double chattering_threshold; | ||
}; | ||
|
||
struct PredictedObjectWithDetectionTime | ||
{ | ||
explicit PredictedObjectWithDetectionTime( | ||
const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) | ||
: detection_time(t), point(p), object(std::move(obj)) | ||
{ | ||
} | ||
|
||
rclcpp::Time detection_time; | ||
geometry_msgs::msg::Point point; | ||
PredictedObject object; | ||
}; | ||
|
||
class CollisionChecker | ||
{ | ||
public: | ||
explicit CollisionChecker( | ||
rclcpp::Node * node, std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr); | ||
|
||
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> | ||
checkTrajectoryForCollision( | ||
TrajectoryPoints & predicted_trajectory_array, | ||
PredictedObjects::ConstSharedPtr dynamic_objects); | ||
|
||
void setParam(const CollisionCheckerParam & param); | ||
|
||
private: | ||
// Functions | ||
|
||
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkObstacleHistory( | ||
const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, | ||
const double z_max); | ||
|
||
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkDynamicObjects( | ||
const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, | ||
const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max); | ||
|
||
void updatePredictedObjectHistory(const rclcpp::Time & now) | ||
{ | ||
for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) { | ||
const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold; | ||
|
||
if (expired) { | ||
itr = predicted_object_history_.erase(itr); | ||
continue; | ||
} | ||
|
||
itr++; | ||
} | ||
} | ||
|
||
// Parameter | ||
CollisionCheckerParam param_; | ||
|
||
// Variables | ||
std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr_; | ||
rclcpp::Node * node_; | ||
vehicle_info_util::VehicleInfo vehicle_info_; | ||
std::vector<PredictedObjectWithDetectionTime> predicted_object_history_{}; | ||
}; | ||
} // namespace predicted_path_checker | ||
|
||
#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ |
92 changes: 92 additions & 0 deletions
92
control/predicted_path_checker/include/predicted_path_checker/debug_marker.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,92 @@ | ||
// Copyright 2023 LeoDrive A.Ş. All rights reserved. | ||
// | ||
// 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ | ||
#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <geometry_msgs/msg/point.hpp> | ||
#include <geometry_msgs/msg/pose.hpp> | ||
#include <visualization_msgs/msg/marker.hpp> | ||
#include <visualization_msgs/msg/marker_array.hpp> | ||
|
||
#include <boost/assert.hpp> | ||
#include <boost/assign/list_of.hpp> | ||
#include <boost/geometry.hpp> | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
#define EIGEN_MPL2_ONLY | ||
|
||
#include <eigen3/Eigen/Core> | ||
#include <eigen3/Eigen/Geometry> | ||
#include <tier4_autoware_utils/geometry/boost_geometry.hpp> | ||
|
||
namespace predicted_path_checker | ||
{ | ||
|
||
enum class PolygonType : int8_t { Vehicle = 0, Collision }; | ||
|
||
enum class PointType : int8_t { Stop = 0 }; | ||
|
||
enum class PoseType : int8_t { Stop = 0, Collision }; | ||
|
||
class PredictedPathCheckerDebugNode | ||
{ | ||
public: | ||
explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front); | ||
|
||
~PredictedPathCheckerDebugNode() {} | ||
|
||
bool pushPolygon( | ||
const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); | ||
|
||
bool pushPolygon(const std::vector<Eigen::Vector3d> & polygon, const PolygonType & type); | ||
|
||
bool pushPolyhedron( | ||
const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, | ||
const PolygonType & type); | ||
|
||
bool pushPolyhedron(const std::vector<Eigen::Vector3d> & polyhedron, const PolygonType & type); | ||
|
||
bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); | ||
|
||
bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); | ||
|
||
visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); | ||
|
||
visualization_msgs::msg::MarkerArray makeVisualizationMarker(); | ||
|
||
void publish(); | ||
|
||
private: | ||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_; | ||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_; | ||
rclcpp::Node * node_; | ||
double base_link2front_; | ||
|
||
std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_; | ||
std::shared_ptr<geometry_msgs::msg::Pose> collision_pose_ptr_; | ||
std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_; | ||
std::vector<std::vector<Eigen::Vector3d>> vehicle_polygons_; | ||
std::vector<std::vector<Eigen::Vector3d>> collision_polygons_; | ||
std::vector<std::vector<Eigen::Vector3d>> vehicle_polyhedrons_; | ||
std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_; | ||
}; | ||
|
||
} // namespace predicted_path_checker | ||
|
||
#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ |
Oops, something went wrong.