-
Notifications
You must be signed in to change notification settings - Fork 696
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907
Merged
satoshi-ota
merged 78 commits into
autowarefoundation:main
from
mitukou1109:obstacle_cruise_planner_pointcloud
Jul 3, 2024
Merged
Changes from 12 commits
Commits
Show all changes
78 commits
Select commit
Hold shift + click to select a range
e264cf3
add pointcloud to obstacle properties
mitukou1109 60224e1
add tf listener & pointcloud subscriber
mitukou1109 028e665
add parameters for pointcloud obstacle
mitukou1109 2f054cd
add type aliases
mitukou1109 c41ded8
convert pointcloud to obstacle
mitukou1109 84a31a9
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 a19f529
add type alias
mitukou1109 29f9284
add polygon conversion for pointcloud obstacle
mitukou1109 ce95e63
initialize twist & pose of pointcloud obstacle
mitukou1109 30ccefe
overload to handle both obstacle & predicted path
mitukou1109 23c1fff
implement ego behavior determination against pointcloud obstacles
mitukou1109 fcbb887
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 8924771
generate obstacle from point
mitukou1109 0d99190
revert getCollisionIndex()
mitukou1109 035ad99
generate obstacle from each point in cloud
mitukou1109 b724add
set pointcloud obstacle velocity to 0
mitukou1109 1a3151c
use tf buffer & listener with pointers
mitukou1109 7460587
update latest pointcloud data
mitukou1109 8df16ff
add topic remap
mitukou1109 fede0ff
remove unnecessary includes
mitukou1109 25731b3
set slow down obstacle velocity to 0
mitukou1109 c57545d
add flag to consider pointcloud obstacle for stopping & slowing down
mitukou1109 3b6a30c
style(pre-commit): autofix
pre-commit-ci[bot] db48811
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 844393a
downsample pointcloud using voxel grid
mitukou1109 4c7a339
change shape type of pointcloud obstacle to polygon
mitukou1109 fb00bed
convert pointcloud to obstacle by clustering
mitukou1109 8869a2b
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 06176a8
add parameters for clustering
mitukou1109 98bcd08
add max_num_points parameter to dummy object
mitukou1109 fb00b59
downsample pointcloud when the number of points is larger than max_nu…
mitukou1109 5f9e4ab
add max_num_points property to dummy bus
mitukou1109 0589073
add parameters for pointcloud based obstacles
mitukou1109 caf6e93
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 7f9738e
store pointcloud in obstacle struct
mitukou1109 8784f59
change obstacle conversion method
mitukou1109 5f70818
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 999605b
migrate previous changes to new package
mitukou1109 a22ef49
store necessary points only
mitukou1109 98ea643
move use_pointcloud to common parameter
mitukou1109 3cb8675
extract necessary points from pointcloud
mitukou1109 a6d1743
add use_pointcloud parameter to planner interface
mitukou1109 4925d0b
fix obstacle conversion
mitukou1109 f4a03c4
Merge branch 'main' into obstacle_cruise_planner_pointcloud
mitukou1109 fea93ed
fix collision point determination
mitukou1109 12044c1
simplify pointcloud transformation
mitukou1109 39d7a30
style(pre-commit): autofix
pre-commit-ci[bot] 4c3519d
fix collision point determination
mitukou1109 d7851ff
pick nearest stop collision point
mitukou1109 cc29510
check collision for every point in cluster
mitukou1109 e7059bd
Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…
mitukou1109 63f6e1e
migrate previous changes to new files
mitukou1109 0c913b2
reduce diff
mitukou1109 f2dc8cf
remove use_pointcloud parameter
mitukou1109 bb2c5d6
add parameters for pointcloud filtering
mitukou1109 a2505af
Merge remote-tracking branch 'origin/awf-latest' into obstacle_cruise…
mitukou1109 299824b
add autoware namespace
mitukou1109 64a3191
Revert "add max_num_points parameter to dummy object"
mitukou1109 d5260f7
Revert "downsample pointcloud when the number of points is larger tha…
mitukou1109 d8c8dbb
Revert "add max_num_points property to dummy bus"
mitukou1109 5f3132f
feat(diagnostic_graph_utils): add logging tool
isamu-takagi 1d946a5
fix all OK
isamu-takagi 462f624
feat(default_ad_api): add log when operation mode change fails
isamu-takagi 695527f
Merge remote-tracking branch 'origin/awf-latest' into obstacle_cruise…
mitukou1109 7919138
get only the necessary one of object or pointcloud data
mitukou1109 be7d92f
addfield for obstacle source type
mitukou1109 45d9da8
enable simultaneous use of PredictedObjects and PointCloud
mitukou1109 811a078
Merge remote-tracking branch 'origin/awf-latest' into obstacle_cruise…
mitukou1109 5d5ec3f
separate convertToObstacles() by source type
mitukou1109 3b336d7
avoid using pointer
mitukou1109 eec09a7
reduce diff
mitukou1109 61ee56a
make nest shallower
mitukou1109 5a44be7
Merge remote-tracking branch 'origin/awf-latest' into tmp/shallow_nest
mitukou1109 9b313dd
define vector concatenate function
mitukou1109 a4a5986
shorten variable names
mitukou1109 7f227b6
fix redundant condition
mitukou1109 c3c9532
Merge branch 'main' into obstacle_cruise_planner_pointcloud
satoshi-ota 8e88283
Merge branch 'main' into obstacle_cruise_planner_pointcloud
satoshi-ota File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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 |
---|---|---|
|
@@ -25,6 +25,10 @@ | |
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <boost/geometry.hpp> | ||
|
||
#include <pcl/common/centroid.h> | ||
|
||
#include <optional> | ||
#include <string> | ||
#include <vector> | ||
|
@@ -66,15 +70,41 @@ struct Obstacle | |
uuid(tier4_autoware_utils::toHexString(object.object_id)), | ||
shape(object.shape), | ||
ego_to_obstacle_distance(ego_to_obstacle_distance), | ||
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) | ||
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), | ||
pointcloud_repr(false) | ||
{ | ||
predicted_paths.clear(); | ||
for (const auto & path : object.kinematics.predicted_paths) { | ||
predicted_paths.push_back(path); | ||
} | ||
} | ||
|
||
Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } | ||
Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object) | ||
: stamp(arg_stamp), uuid(""), pointcloud(object), pointcloud_repr(true) | ||
{ | ||
twist.linear.x = twist.linear.y = twist.angular.z = 0.0; | ||
|
||
Eigen::Vector4d centroid; | ||
pcl::compute3DCentroid(pointcloud, centroid); | ||
pose.position.x = centroid.x(); | ||
pose.position.y = centroid.y(); | ||
pose.position.z = centroid.z(); | ||
} | ||
|
||
Polygon2d toPolygon() const | ||
{ | ||
if (pointcloud_repr) { | ||
MultiPoint2d points; | ||
for (const auto & point : pointcloud) { | ||
bg::append(points, Point2d(point.x, point.y)); | ||
} | ||
Polygon2d polygon; | ||
bg::convex_hull(points, polygon); | ||
return polygon; | ||
} else { | ||
return tier4_autoware_utils::toPolygon2d(pose, shape); | ||
} | ||
} | ||
|
||
rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. | ||
geometry_msgs::msg::Pose pose; // interpolated with the current stamp | ||
|
@@ -87,6 +117,8 @@ struct Obstacle | |
std::vector<PredictedPath> predicted_paths; | ||
double ego_to_obstacle_distance; | ||
double lat_dist_from_obstacle_to_traj; | ||
PointCloud pointcloud; | ||
bool pointcloud_repr; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Could you explain the purpose of this variable? |
||
}; | ||
|
||
struct TargetObstacleInterface | ||
|
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
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
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why did you calculate centroid of pointcloud?