Skip to content

Commit

Permalink
refactor(obstacle_cruise_planner): use polygon utils (tier4#1406)
Browse files Browse the repository at this point in the history 
* refactor(obstacle_cruise_planner): use polygon utils

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168 @boyali
takayuki5168 authored and boyali committed Sep 28, 2022
1 parent 71c2502 commit 027e151
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 118 deletions.
3 changes: 0 additions & 3 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,6 @@ namespace bg = boost::geometry;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;

Polygon2d convertObstacleToPolygon(
const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape);

boost::optional<size_t> getFirstCollisionIndex(
const std::vector<Polygon2d> & traj_polygons, const Polygon2d & obj_polygon,
std::vector<geometry_msgs::msg::Point> & collision_points);
Expand Down
3 changes: 2 additions & 1 deletion planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "motion_utils/trajectory/tmp_conversion.hpp"
#include "obstacle_cruise_planner/polygon_utils.hpp"
#include "obstacle_cruise_planner/utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"

#include <boost/format.hpp>
Expand Down Expand Up @@ -691,7 +692,7 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(

// calculate collision points
const auto obstacle_polygon =
polygon_utils::convertObstacleToPolygon(object_pose, predicted_object.shape);
tier4_autoware_utils::toPolygon2d(object_pose, predicted_object.shape);
std::vector<geometry_msgs::msg::Point> collision_points;
const auto first_within_idx = polygon_utils::getFirstCollisionIndex(
decimated_traj_polygons, obstacle_polygon, collision_points);
Expand Down
121 changes: 7 additions & 114 deletions planning/obstacle_cruise_planner/src/polygon_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "obstacle_cruise_planner/polygon_utils.hpp"

#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"

namespace
{
namespace bg = boost::geometry;
Expand All @@ -29,35 +31,6 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point &
bg::append(polygon.outer(), point);
}

void appendPointToPolygon(Polygon2d & polygon, const Point2d point)
{
bg::append(polygon.outer(), point);
}

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)
{
Polygon2d inverted_polygon;
for (int i = polygon.outer().size() - 1; 0 <= i; --i) {
inverted_polygon.outer().push_back(polygon.outer().at(i));
}
return inverted_polygon;
}

Polygon2d createOneStepPolygon(
const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose,
const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width)
Expand Down Expand Up @@ -100,99 +73,19 @@ Polygon2d createOneStepPolygon(
tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position);
}

polygon = isClockWise(polygon) ? polygon : inverseClockWise(polygon);
polygon = tier4_autoware_utils::isClockwise(polygon)
? polygon
: tier4_autoware_utils::inverseClockwise(polygon);

Polygon2d hull_polygon;
bg::convex_hull(polygon, hull_polygon);

return hull_polygon;
}

geometry_msgs::msg::Polygon rotatePolygon(
const geometry_msgs::msg::Polygon & polygon, const double & angle)
{
const double cos = std::cos(angle);
const double sin = std::sin(angle);
geometry_msgs::msg::Polygon rotated_polygon;
for (const auto & point : polygon.points) {
auto rotated_point = point;
rotated_point.x = cos * point.x - sin * point.y;
rotated_point.y = sin * point.x + cos * point.y;
rotated_polygon.points.push_back(rotated_point);
}
return rotated_polygon;
}
} // namespace

namespace polygon_utils
{
Polygon2d convertObstacleToPolygon(
const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape)
{
Polygon2d polygon;

if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
const auto point0 = tier4_autoware_utils::calcOffsetPose(
pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0)
.position;
const auto point1 = tier4_autoware_utils::calcOffsetPose(
pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0)
.position;
const auto point2 = tier4_autoware_utils::calcOffsetPose(
pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0)
.position;
const auto point3 = tier4_autoware_utils::calcOffsetPose(
pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0)
.position;

appendPointToPolygon(polygon, point0);
appendPointToPolygon(polygon, point1);
appendPointToPolygon(polygon, point2);
appendPointToPolygon(polygon, point3);

// NOTE: push back the first point in order to close polygon
appendPointToPolygon(polygon, polygon.outer().front());
} else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
const double radius = shape.dimensions.x / 2.0;
constexpr int circle_discrete_num = 6;
for (int i = 0; i < circle_discrete_num; ++i) {
geometry_msgs::msg::Point point;
point.x = std::cos(
(static_cast<double>(i) / static_cast<double>(circle_discrete_num)) * 2.0 * M_PI +
M_PI / static_cast<double>(circle_discrete_num)) *
radius +
pose.position.x;
point.y = std::sin(
(static_cast<double>(i) / static_cast<double>(circle_discrete_num)) * 2.0 * M_PI +
M_PI / static_cast<double>(circle_discrete_num)) *
radius +
pose.position.y;
appendPointToPolygon(polygon, point);
}

// NOTE: push back the first point in order to close polygon
appendPointToPolygon(polygon, polygon.outer().front());
} else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
const double poly_yaw = tf2::getYaw(pose.orientation);
const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw);
for (const auto rel_point : rotated_footprint.points) {
geometry_msgs::msg::Point abs_point;
abs_point.x = pose.position.x + rel_point.x;
abs_point.y = pose.position.y + rel_point.y;

appendPointToPolygon(polygon, abs_point);
}
if (polygon.outer().size() > 0) {
// NOTE: push back the first point in order to close polygon
appendPointToPolygon(polygon, polygon.outer().front());
}
} else {
throw std::logic_error("The shape type is not supported in obstacle_cruise_planner.");
}

return isClockWise(polygon) ? polygon : inverseClockWise(polygon);
}

boost::optional<size_t> getFirstCollisionIndex(
const std::vector<Polygon2d> & traj_polygons, const Polygon2d & obj_polygon,
std::vector<geometry_msgs::msg::Point> & collision_geom_points)
Expand Down Expand Up @@ -232,7 +125,7 @@ boost::optional<size_t> getFirstNonCollisionIndex(

size_t latest_collision_idx = start_idx;
for (const auto & path_point : predicted_path.path) {
const auto obj_polygon = convertObstacleToPolygon(path_point, shape);
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(path_point, shape);
for (size_t i = start_idx; i < traj_polygons.size(); ++i) {
const double dist = bg::distance(traj_polygons.at(i), obj_polygon);
if (dist <= epsilon) {
Expand Down Expand Up @@ -277,7 +170,7 @@ boost::optional<size_t> willCollideWithSurroundObstacle(
}

const auto & traj_polygon = traj_polygons.at(j);
const auto obj_polygon = polygon_utils::convertObstacleToPolygon(path_point, shape);
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(path_point, shape);
const double dist = bg::distance(traj_polygon, obj_polygon);

if (dist < epsilon) {
Expand Down

0 comments on commit 027e151

Please sign in to comment.