Skip to content
This repository has been archived by the owner on Aug 27, 2021. It is now read-only.

Test refactoring #3

Merged
merged 62 commits into from
Mar 31, 2021
Merged
Changes from 1 commit
Commits
Show all changes
62 commits
Select commit Hold shift + click to select a range
259a4de
wip
KeisukeShima Feb 25, 2021
1ed8da2
wip: add memo
KeisukeShima Feb 25, 2021
caa5297
refactoring
KeisukeShima Feb 26, 2021
db5748a
create vehicle class
KeisukeShima Mar 1, 2021
17cc672
create trajectory, point_helper class
KeisukeShima Mar 1, 2021
b729ad2
add inline prefix
KeisukeShima Mar 1, 2021
d6a880e
refactoring
KeisukeShima Mar 1, 2021
3fb5650
refactoring
KeisukeShima Mar 1, 2021
48178ed
create OneStepPolygon class
KeisukeShima Mar 2, 2021
d840ded
refactoring
KeisukeShima Mar 2, 2021
e00f392
update design document
KeisukeShima Mar 2, 2021
9ff1392
insert final newline
KeisukeShima Mar 2, 2021
56ef212
add gtest
KeisukeShima Mar 3, 2021
b4cd78c
wip
KeisukeShima Mar 2, 2021
1de69cf
modify createVehicleFootprint()
KeisukeShima Mar 3, 2021
b4f2b0f
cleanup code
KeisukeShima Mar 3, 2021
aeef6aa
change namespace
KeisukeShima Mar 4, 2021
5568f74
use camelCase for function name
KeisukeShima Mar 4, 2021
0ca8a0e
apply uncrustify
KeisukeShima Mar 4, 2021
7602798
test passed
KeisukeShima Mar 4, 2021
863d31c
use const reference for non-primitive types
KeisukeShima Mar 4, 2021
9579da8
remove setter function
KeisukeShima Mar 4, 2021
2142496
modify one_step_polygon class
KeisukeShima Mar 4, 2021
2acc807
apply colcon test
KeisukeShima Mar 4, 2021
c343967
replace boost type to autoware_util
KeisukeShima Mar 4, 2021
4c2c1e6
rename function
KeisukeShima Mar 4, 2021
14f7786
follow lane_departure_checker's way
KeisukeShima Mar 5, 2021
f1bfb63
remove jsonDumpsPose and related code
KeisukeShima Mar 5, 2021
43ff1a3
replace VehicleInfo to Param
KeisukeShima Mar 5, 2021
b9d2047
change logger variable to const reference
KeisukeShima Mar 5, 2021
04fbdbf
Use using to simplify the code
KeisukeShima Mar 8, 2021
6462d77
cleanup include file
KeisukeShima Mar 8, 2021
475bfae
Refactor trajectory, util
KeisukeShima Mar 8, 2021
6696986
Delete Trajectory class
KeisukeShima Mar 8, 2021
1048d65
Refactor point_helper
KeisukeShima Mar 8, 2021
95c024c
Refactor AdaptiveCruiseControl class
KeisukeShima Mar 8, 2021
0a648fa
Remove unused line
KeisukeShima Mar 9, 2021
d471f07
refactor input/output
KeisukeShima Mar 9, 2021
6887e2f
Add test
KeisukeShima Mar 9, 2021
3852167
Refactoring
KeisukeShima Mar 9, 2021
c283861
Replace pcl::PointXYZ with Point3d
KeisukeShima Mar 10, 2021
8807c15
add include file
KeisukeShima Mar 17, 2021
4aca1e3
fix build error
KeisukeShima Mar 17, 2021
e5bbbaa
Apply clang-tidy format
KeisukeShima Mar 19, 2021
dfe42b1
Remove OpenCV dependency
KeisukeShima Mar 22, 2021
64e794b
create ament_cmake_auto_gtest package
KeisukeShima Mar 22, 2021
c8b7cc6
Add readme
KeisukeShima Mar 22, 2021
eeceeb7
Replace tuple with optional
KeisukeShima Mar 22, 2021
06ec3b6
add pcl_ros
KeisukeShima Mar 22, 2021
f83300a
Modify ObstaclePointCloud class
KeisukeShima Mar 22, 2021
388a88d
add build_depends
KeisukeShima Mar 22, 2021
64a1121
fix path
KeisukeShima Mar 22, 2021
4e7e19d
fix path
KeisukeShima Mar 22, 2021
0b0ff64
append depends
KeisukeShima Mar 22, 2021
f720dd0
Fix rebase error
KeisukeShima Mar 24, 2021
490178f
Ported code from the feature/create-node-pkg branch
KeisukeShima Mar 29, 2021
c55df3c
Fix lint
Mar 29, 2021
dd4fa9f
Add .isort.cfg
Mar 29, 2021
7c13039
Fix usage of ament_cmake_auto_gtest
KeisukeShima Mar 30, 2021
20f124b
Cleanup, modify copyright
KeisukeShima Mar 30, 2021
9d1825a
Fix usage of ament_auto_add_gtest
KeisukeShima Mar 30, 2021
454b543
Rename vehicle_info.param.yaml
KeisukeShima Mar 31, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
wip
KeisukeShima committed Mar 24, 2021
commit b4cd78ce541e7c602b8695a9320664433bea94ec
Original file line number Diff line number Diff line change
@@ -28,6 +28,7 @@
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "autoware_utils/autoware_utils.hpp"
#define EIGEN_MPL2_ONLY
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"
@@ -45,8 +46,8 @@ class ObstacleStopPlannerDebugNode
explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front);
~ObstacleStopPlannerDebugNode() {}
bool pushPolygon(
const std::vector<cv::Point2d> & polygon, const double z, const PolygonType & type);
bool pushPolygon(const std::vector<Eigen::Vector3d> & polygon, const PolygonType & type);
const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type);
bool pushPolygon(const autoware_utils::Polygon3d & polygon, 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);
bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type);
@@ -66,10 +67,10 @@ class ObstacleStopPlannerDebugNode
std::shared_ptr<geometry_msgs::msg::Pose> slow_down_end_pose_ptr_;
std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
std::shared_ptr<geometry_msgs::msg::Point> slow_down_obstacle_point_ptr_;
std::vector<std::vector<Eigen::Vector3d>> vehicle_polygons_;
std::vector<std::vector<Eigen::Vector3d>> slow_down_range_polygons_;
std::vector<std::vector<Eigen::Vector3d>> collision_polygons_;
std::vector<std::vector<Eigen::Vector3d>> slow_down_polygons_;
std::vector<autoware_utils::Polygon3d> vehicle_polygons_;
std::vector<autoware_utils::Polygon3d> slow_down_range_polygons_;
std::vector<autoware_utils::Polygon3d> collision_polygons_;
std::vector<autoware_utils::Polygon3d> slow_down_polygons_;
};

} // namespace motion_planning
Original file line number Diff line number Diff line change
@@ -23,13 +23,14 @@
#include "boost/geometry.hpp"
#include "boost/geometry/geometries/linestring.hpp"
#include "boost/geometry/geometries/point_xy.hpp"
// #include "boost/geometry"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "obstacle_stop_planner/util.hpp"
#include "obstacle_stop_planner/vehicle.hpp"

#include "autoware_utils/autoware_utils.hpp"
#include "obstacle_stop_planner/util/create_vehicle_footprint.hpp"

namespace motion_planning {

@@ -40,11 +41,11 @@ class OneStepPolygon
const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose,
const double expand_width);
Polygon GetBoostPolygon() const {return boost_polygon_;}
std::vector<cv::Point2d> GetPolygon() const {return polygon_;}
autoware_utils::Polygon2d GetPolygon() const {return polygon_;}
void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared<VehicleInfo>(vehicle_info);}

private:
std::vector<cv::Point2d> polygon_;
autoware_utils::Polygon2d polygon_;
Polygon boost_polygon_;
std::shared_ptr<VehicleInfo> vehicle_info_;

@@ -69,65 +70,90 @@ inline void OneStepPolygon::Create(
const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose,
const double expand_width)
{
std::vector<cv::Point2d> one_step_move_vehicle_corner_points;
autoware_utils::Polygon2d one_step_move_vehicle_corner_points;
const auto footprint = createVehicleFootprint(*vehicle_info_, expand_width);

// start step
{
double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation);
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));

bg::strategy::transform::rotate_transformer<bg::radian, double, 2, 2> rotate(yaw);
autoware_utils::LinearRing2d transformed_footprint;
bg::transform(footprint, transformed_footprint, rotate);

bg::strategy::transform::translate_transformer<double, 2, 2> translate(base_step_pose.position.x, base_step_pose.position.y);
bg::transform(transformed_footprint, transformed_footprint, translate);
one_step_move_vehicle_corner_points.outer() = transformed_footprint;

// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
// std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
// base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
// std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));

// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
// std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
// base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
// std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
// std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
// base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
// std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
// std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
// base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
// std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
}
// next step
{
double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation);
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
one_step_move_vehicle_corner_points.push_back(
cv::Point2d(
next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
bg::strategy::transform::rotate_transformer<bg::radian, double, 2, 2> rotate(yaw);
autoware_utils::LinearRing2d transformed_footprint;
bg::transform(footprint, transformed_footprint, rotate);

bg::strategy::transform::translate_transformer<double, 2, 2> translate(base_step_pose.position.x, base_step_pose.position.y);
bg::transform(transformed_footprint, transformed_footprint, translate);
for (const auto& item : transformed_footprint) {
one_step_move_vehicle_corner_points.outer().emplace_back(item);
}


// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
// std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
// next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
// std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) -
// std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
// next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) +
// std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
// std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width),
// next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
// std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width)));
// one_step_move_vehicle_corner_points.push_back(
// cv::Point2d(
// next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) -
// std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width),
// next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) +
// std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width)));
}
polygon_ = convexHull(one_step_move_vehicle_corner_points);
boost_polygon_ = ConvertToBoostPolygon(polygon_);

bg::convex_hull(one_step_move_vehicle_corner_points, polygon_);
// polygon_ = convexHull(one_step_move_vehicle_corner_points);
// boost_polygon_ = ConvertToBoostPolygon(polygon_);
}
kenji-miyake marked this conversation as resolved.
Show resolved Hide resolved

inline std::vector<cv::Point2d> OneStepPolygon::convexHull(
Original file line number Diff line number Diff line change
@@ -27,7 +27,7 @@

namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point, false>;
using Polygon = bg::model::polygon<Point, false>; // clockwise = false
using Line = bg::model::linestring<Point>;
kenji-miyake marked this conversation as resolved.
Show resolved Hide resolved

namespace
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
// Copyright 2020 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.
// 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 OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
#define OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_

#include <autoware_utils/geometry/geometry.hpp>
#include <vehicle_info_util/vehicle_info.hpp>

inline autoware_utils::LinearRing2d createVehicleFootprint(
const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0)
{
using autoware_utils::LinearRing2d;
using autoware_utils::Point2d;

const auto & i = vehicle_info;

const double x_front = i.front_overhang_m_ + i.wheel_base_m_ + margin;
const double x_center = i.wheel_base_m_ / 2.0;
const double x_rear = -(i.rear_overhang_m_ + margin);
const double y_left = i.wheel_tread_m_ / 2.0 + i.left_overhang_m_ + margin;
const double y_right = -(i.wheel_tread_m_ / 2.0 + i.right_overhang_m_ + margin);

LinearRing2d footprint;
footprint.push_back(Point2d{x_front, y_left});
footprint.push_back(Point2d{x_front, y_right});
footprint.push_back(Point2d{x_center, y_right});
footprint.push_back(Point2d{x_rear, y_right});
footprint.push_back(Point2d{x_rear, y_left});
footprint.push_back(Point2d{x_center, y_left});
footprint.push_back(Point2d{x_front, y_left});

return footprint;
}

#endif // OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
1 change: 1 addition & 0 deletions obstacle_stop_planner_refine/package.xml
Original file line number Diff line number Diff line change
@@ -12,6 +12,7 @@

<depend>rclcpp</depend>
<depend>autoware_debug_msgs</depend>
<depend>autoware_utils</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_utils</depend>
Loading