Skip to content
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.

Sign up for GitHub

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

perf(dynamic_obstacle_avoidance): decrease the computation time with time_keeper (#7986) #1427

Merged
merged 1 commit into from
Jul 29, 2024
Merged

perf(dynamic_obstacle_avoidance): decrease the computation time with time_keeper (#7986) #1427

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
24 changes: 10 additions & 14 deletions ...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_

#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
#include "autoware/universe_utils/system/stop_watch.hpp"

#include <autoware/universe_utils/geometry/boost_geometry.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -206,20 +205,20 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
std::optional<MinMaxValue> lat_offset_to_avoid{std::nullopt};
bool is_collision_left{false};
bool should_be_avoided{false};
std::vector<PathPointWithLaneId> ref_path_points_for_obj_poly;
std::vector<geometry_msgs::msg::Pose> ref_points_for_obj_poly;
LatFeasiblePaths ego_lat_feasible_paths;

// add additional information (not update to the latest data)
void update(
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
const bool arg_is_collision_left, const bool arg_should_be_avoided,
const std::vector<PathPointWithLaneId> & arg_ref_path_points_for_obj_poly)
const std::vector<geometry_msgs::msg::Pose> & arg_ref_points_for_obj_poly)
{
lon_offset_to_avoid = arg_lon_offset_to_avoid;
lat_offset_to_avoid = arg_lat_offset_to_avoid;
is_collision_left = arg_is_collision_left;
should_be_avoided = arg_should_be_avoided;
ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly;
ref_points_for_obj_poly = arg_ref_points_for_obj_poly;
}
};

Expand Down Expand Up @@ -317,12 +316,12 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
const bool should_be_avoided,
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly)
const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly)
{
if (object_map_.count(uuid) != 0) {
object_map_.at(uuid).update(
lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided,
ref_path_points_for_obj_poly);
ref_points_for_obj_poly);
}
}

Expand Down Expand Up @@ -409,24 +408,25 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
std::optional<std::pair<size_t, size_t>> calcCollisionSection(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
LatLonOffset getLateralLongitudinalOffset(
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const std::vector<geometry_msgs::msg::Pose> & ego_points,
const geometry_msgs::msg::Pose & obj_pose, const size_t obj_seg_idx,
const autoware_perception_msgs::msg::Shape & obj_shape) const;
double calcValidLengthToAvoid(
const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_perception_msgs::msg::Shape & obj_shape,
const bool is_object_same_direction) const;
MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape,
const TimeWhileCollision & time_while_collision) const;
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidRegulatedObject(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
const bool is_collision_left, const double obj_normal_vel,
const std::optional<DynamicAvoidanceObject> & prev_object) const;
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidUnregulatedObject(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
const std::optional<DynamicAvoidanceObject> & prev_object,
const DynamicAvoidanceObject & object) const;
std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
Expand Down Expand Up @@ -454,10 +454,6 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
std::shared_ptr<DynamicAvoidanceParameters> parameters_;

TargetObjectsManager target_objects_manager_;

mutable autoware::universe_utils::StopWatch<
std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
stop_watch_;
};
} // namespace autoware::behavior_path_planner

Expand Down
Loading
Loading