Skip to content

Commit

Permalink
test(obstacle_collision_checker): refactor and add tests (#8989)
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem authored Oct 7, 2024
1 parent 4f97001 commit 7ab3061
Show file tree
Hide file tree
Showing 5 changed files with 323 additions and 165 deletions.
55 changes: 21 additions & 34 deletions ...tacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc. All rights reserved.
// Copyright 2020-2024 Tier IV, Inc. 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.
Expand All @@ -15,7 +15,6 @@
#ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_

#include <autoware/universe_utils/geometry/boost_geometry.hpp>
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>

#include <autoware_planning_msgs/msg/trajectory.hpp>
Expand All @@ -24,8 +23,6 @@
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <boost/optional.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

Expand Down Expand Up @@ -54,6 +51,8 @@ struct Input
geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory;
autoware::vehicle_info_utils::VehicleInfo vehicle_info;
Param param;
};

struct Output
Expand All @@ -65,43 +64,31 @@ struct Output
std::vector<LinearRing2d> vehicle_passing_areas;
};

class ObstacleCollisionChecker
{
public:
explicit ObstacleCollisionChecker(rclcpp::Node & node);
Output update(const Input & input);

void setParam(const Param & param) { param_ = param; }
Output check_for_collisions(const Input & input);

private:
Param param_;
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
//! This function assumes the input trajectory is sampled dense enough
autoware_planning_msgs::msg::Trajectory resample_trajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval);

//! This function assumes the input trajectory is sampled dense enough
static autoware_planning_msgs::msg::Trajectory resampleTrajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval);
autoware_planning_msgs::msg::Trajectory cut_trajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double length);

static autoware_planning_msgs::msg::Trajectory cutTrajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double length);
std::vector<LinearRing2d> create_vehicle_footprints(
const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);

static std::vector<LinearRing2d> createVehicleFootprints(
const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
std::vector<LinearRing2d> create_vehicle_passing_areas(
const std::vector<LinearRing2d> & vehicle_footprints);

static std::vector<LinearRing2d> createVehiclePassingAreas(
const std::vector<LinearRing2d> & vehicle_footprints);
LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2);

static LinearRing2d createHullFromFootprints(
const LinearRing2d & area1, const LinearRing2d & area2);
bool will_collide(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const std::vector<LinearRing2d> & vehicle_footprints);

static bool willCollide(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const std::vector<LinearRing2d> & vehicle_footprints);

static bool hasCollision(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const LinearRing2d & vehicle_footprint);
};
bool has_collision(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const LinearRing2d & vehicle_footprint);
} // namespace obstacle_collision_checker

#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
28 changes: 14 additions & 14 deletions ..._collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
#include <autoware/universe_utils/ros/self_pose_listener.hpp>
#include <autoware/universe_utils/ros/transform_listener.hpp>
#include <autoware_vehicle_info_utils/vehicle_info.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>

Expand All @@ -38,7 +39,7 @@ namespace obstacle_collision_checker
{
struct NodeParam
{
double update_rate;
double update_rate{};
};

class ObstacleCollisionCheckerNode : public rclcpp::Node
Expand All @@ -64,46 +65,45 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node
geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_;
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

// Callback
void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
void on_obstacle_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
void on_reference_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
void on_predicted_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg);

// Publisher
std::shared_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> time_publisher_;

// Timer
rclcpp::TimerBase::SharedPtr timer_;
void initTimer(double period_s);
void init_timer(double period_s);

bool isDataReady();
bool isDataTimeout();
void onTimer();
bool is_data_ready();
bool is_data_timeout();
void on_timer();

// Parameter
NodeParam node_param_;
Param param_;

// Dynamic Reconfigure
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
rcl_interfaces::msg::SetParametersResult paramCallback(
rcl_interfaces::msg::SetParametersResult param_callback(
const std::vector<rclcpp::Parameter> & parameters);

// Core
Input input_;
Output output_;
std::unique_ptr<ObstacleCollisionChecker> obstacle_collision_checker_;

// Diagnostic Updater
diagnostic_updater::Updater updater_;

void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat);
void check_lane_departure(diagnostic_updater::DiagnosticStatusWrapper & stat);

// Visualization
visualization_msgs::msg::MarkerArray createMarkerArray() const;
visualization_msgs::msg::MarkerArray create_marker_array() const;
};
} // namespace obstacle_collision_checker

Expand Down
76 changes: 31 additions & 45 deletions ...acle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,24 +20,18 @@
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <pcl_ros/transforms.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>

#include <boost/geometry.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <tf2/utils.h>

#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#else
#include <tf2_eigen/tf2_eigen.hpp>
#endif

#include <iostream>
#include <vector>

namespace
{
pcl::PointCloud<pcl::PointXYZ> getTransformedPointCloud(
pcl::PointCloud<pcl::PointXYZ> get_transformed_point_cloud(
const sensor_msgs::msg::PointCloud2 & pointcloud_msg,
const geometry_msgs::msg::Transform & transform)
{
Expand All @@ -52,7 +46,7 @@ pcl::PointCloud<pcl::PointXYZ> getTransformedPointCloud(
return transformed_pointcloud;
}

pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
pcl::PointCloud<pcl::PointXYZ> filter_point_cloud_by_trajectory(
const pcl::PointCloud<pcl::PointXYZ> & pointcloud,
const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius)
{
Expand All @@ -70,7 +64,7 @@ pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
return filtered_pointcloud;
}

double calcBrakingDistance(
double calc_braking_distance(
const double abs_velocity, const double max_deceleration, const double delay_time)
{
const double idling_distance = abs_velocity * delay_time;
Expand All @@ -82,12 +76,7 @@ double calcBrakingDistance(

namespace obstacle_collision_checker
{
ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node)
: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo())
{
}

Output ObstacleCollisionChecker::update(const Input & input)
Output check_for_collisions(const Input & input)
{
Output output;
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
Expand All @@ -97,31 +86,32 @@ Output ObstacleCollisionChecker::update(const Input & input)
const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x);
const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;
const auto braking_distance =
calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time);
output.resampled_trajectory = cutTrajectory(
resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance);
calc_braking_distance(abs_velocity, input.param.max_deceleration, input.param.delay_time);
output.resampled_trajectory = cut_trajectory(
resample_trajectory(*input.predicted_trajectory, input.param.resample_interval),
braking_distance);
output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true);

// resample pointcloud
const auto obstacle_pointcloud =
getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform);
const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory(
obstacle_pointcloud, output.resampled_trajectory, param_.search_radius);
get_transformed_point_cloud(*input.obstacle_pointcloud, input.obstacle_transform->transform);
const auto filtered_obstacle_pointcloud = filter_point_cloud_by_trajectory(
obstacle_pointcloud, output.resampled_trajectory, input.param.search_radius);

output.vehicle_footprints =
createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_);
create_vehicle_footprints(output.resampled_trajectory, input.param, input.vehicle_info);
output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true);

output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints);
output.vehicle_passing_areas = create_vehicle_passing_areas(output.vehicle_footprints);
output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true);

output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas);
output.will_collide = will_collide(filtered_obstacle_pointcloud, output.vehicle_passing_areas);
output.processing_time_map["willCollide"] = stop_watch.toc(true);

return output;
}

autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory(
autoware_planning_msgs::msg::Trajectory resample_trajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval)
{
autoware_planning_msgs::msg::Trajectory resampled;
Expand All @@ -131,11 +121,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec
for (size_t i = 1; i < trajectory.points.size() - 1; ++i) {
const auto & point = trajectory.points.at(i);

const auto p1 =
autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d();
const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d();

if (boost::geometry::distance(p1, p2) > interval) {
const auto distance =
autoware::universe_utils::calcDistance2d(resampled.points.back(), point.pose.position);
if (distance > interval) {
resampled.points.push_back(point);
}
}
Expand All @@ -144,7 +132,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec
return resampled;
}

autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(
autoware_planning_msgs::msg::Trajectory cut_trajectory(
const autoware_planning_msgs::msg::Trajectory & trajectory, const double length)
{
autoware_planning_msgs::msg::Trajectory cut;
Expand All @@ -157,8 +145,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(

const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position);
const auto p2 = autoware::universe_utils::fromMsg(point.pose.position);
const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d());

const auto points_distance = boost::geometry::distance(p1, p2);
const auto remain_distance = length - total_length;

// Over length
Expand Down Expand Up @@ -187,7 +175,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(
return cut;
}

std::vector<LinearRing2d> ObstacleCollisionChecker::createVehicleFootprints(
std::vector<LinearRing2d> create_vehicle_footprints(
const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
{
Expand All @@ -205,22 +193,21 @@ std::vector<LinearRing2d> ObstacleCollisionChecker::createVehicleFootprints(
return vehicle_footprints;
}

std::vector<LinearRing2d> ObstacleCollisionChecker::createVehiclePassingAreas(
std::vector<LinearRing2d> create_vehicle_passing_areas(
const std::vector<LinearRing2d> & vehicle_footprints)
{
// Create hull from two adjacent vehicle footprints
std::vector<LinearRing2d> areas;
for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) {
const auto & footprint1 = vehicle_footprints.at(i);
const auto & footprint2 = vehicle_footprints.at(i + 1);
areas.push_back(createHullFromFootprints(footprint1, footprint2));
areas.push_back(create_hull_from_footprints(footprint1, footprint2));
}

return areas;
}

LinearRing2d ObstacleCollisionChecker::createHullFromFootprints(
const LinearRing2d & area1, const LinearRing2d & area2)
LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2)
{
autoware::universe_utils::MultiPoint2d combined;
for (const auto & p : area1) {
Expand All @@ -234,33 +221,32 @@ LinearRing2d ObstacleCollisionChecker::createHullFromFootprints(
return hull;
}

bool ObstacleCollisionChecker::willCollide(
bool will_collide(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const std::vector<LinearRing2d> & vehicle_footprints)
{
for (size_t i = 1; i < vehicle_footprints.size(); i++) {
// skip first footprint because surround obstacle checker handle it
const auto & vehicle_footprint = vehicle_footprints.at(i);
if (hasCollision(obstacle_pointcloud, vehicle_footprint)) {
RCLCPP_WARN(
rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide");
if (has_collision(obstacle_pointcloud, vehicle_footprint)) {
RCLCPP_WARN(rclcpp::get_logger("obstacle_collision_checker"), "willCollide");
return true;
}
}

return false;
}

bool ObstacleCollisionChecker::hasCollision(
bool has_collision(
const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
const LinearRing2d & vehicle_footprint)
{
for (const auto & point : obstacle_pointcloud.points) {
if (boost::geometry::within(
autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) {
RCLCPP_WARN(
rclcpp::get_logger("obstacle_collision_checker"),
"[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y);
rclcpp::get_logger("obstacle_collision_checker"), "Collide to Point x: %f y: %f", point.x,
point.y);
return true;
}
}
Expand Down
Loading

0 comments on commit 7ab3061

Please sign in to comment.