Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] authored and beyzanurkaya committed May 24, 2024
1 parent 7ef4183 commit 4597f75
Show file tree
Hide file tree
Showing 5 changed files with 19 additions and 15 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,4 @@
ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight)
ignore_objects_behind_ego: false # if true, ignore objects that are behind the ego vehicle
behind_object_distance_threshold: 5.0 # [m] distance behind the ego vehicle to ignore objects
use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point
use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point
Original file line number Diff line number Diff line change
Expand Up @@ -61,24 +61,27 @@ tier4_autoware_utils::Polygon2d make_forward_footprint(
{}};
}

//create footprint using predicted_path of object
tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y)
// create footprint using predicted_path of object
tier4_autoware_utils::Polygon2d translate_polygon(
const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y)
{
tier4_autoware_utils::Polygon2d translated_polygon;
const boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translation(x, y);
boost::geometry::transform(polygon, translated_polygon, translation);
return translated_polygon;
}

tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint)
tier4_autoware_utils::Polygon2d create_footprint(
const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint)
{
const auto angle = tf2::getYaw(pose.orientation);
return translate_polygon(
tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y);
}

tier4_autoware_utils::MultiPolygon2d create_object_footprints(
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects, const PlannerParam & params)
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
const PlannerParam & params)
{
tier4_autoware_utils::MultiPolygon2d footprints;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "types.hpp"

#include <tier4_autoware_utils/geometry/geometry.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>

#include <vector>
Expand All @@ -42,9 +43,12 @@ tier4_autoware_utils::Polygon2d make_forward_footprint(
const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params,
const double hysteresis);
tier4_autoware_utils::MultiPolygon2d create_object_footprints(
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects, const PlannerParam & params);
tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y);
tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint);
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
const PlannerParam & params);
tier4_autoware_utils::Polygon2d translate_polygon(
const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y);
tier4_autoware_utils::Polygon2d create_footprint(
const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint);
/// @brief project a footprint to the given pose
/// @param [in] base_footprint footprint to project
/// @param [in] pose projection pose
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node
getOrDeclareParameter<bool>(node, ns + ".ignore_unavoidable_collisions");
pp.ignore_objects_behind_ego =
getOrDeclareParameter<bool>(node, ns + ".ignore_objects_behind_ego");
pp.use_predicted_path =
getOrDeclareParameter<bool>(node, ns + ".use_predicted_path");
pp.use_predicted_path = getOrDeclareParameter<bool>(node, ns + ".use_predicted_path");
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
pp.ego_lateral_offset =
std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
tier4_autoware_utils::MultiPolygon2d obstacle_predicted_footprints;
std::vector<Collision> collisions;
double footprints_duration_us;
if (params_.use_predicted_path){
if (params_.use_predicted_path) {
stopwatch.tic("footprints");
obstacle_predicted_footprints = create_object_footprints(dynamic_obstacles, params_);
footprints_duration_us = stopwatch.toc("footprints");
Expand All @@ -100,12 +100,10 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
debug_data_.obstacle_footprints = obstacle_predicted_footprints;
} else {
stopwatch.tic("footprints");
obstacle_forward_footprints =
make_forward_footprints(dynamic_obstacles, params_, hysteresis);
obstacle_forward_footprints = make_forward_footprints(dynamic_obstacles, params_, hysteresis);
footprints_duration_us = stopwatch.toc("footprints");
stopwatch.tic("collisions");
collisions =
find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
debug_data_.obstacle_footprints = obstacle_forward_footprints;
}
update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);
Expand Down

0 comments on commit 4597f75

Please sign in to comment.