Skip to content

Commit b457910

Browse files
style(pre-commit): autofix
1 parent 30578bf commit b457910

File tree

5 files changed

+19
-15
lines changed

5 files changed

+19
-15
lines changed

planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,4 +14,4 @@
1414
ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight)
1515
ignore_objects_behind_ego: false # if true, ignore objects that are behind the ego vehicle
1616
behind_object_distance_threshold: 5.0 # [m] distance behind the ego vehicle to ignore objects
17-
use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point
17+
use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point

planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -61,24 +61,27 @@ tier4_autoware_utils::Polygon2d make_forward_footprint(
6161
{}};
6262
}
6363

64-
//create footprint using predicted_path of object
65-
tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y)
64+
// create footprint using predicted_path of object
65+
tier4_autoware_utils::Polygon2d translate_polygon(
66+
const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y)
6667
{
6768
tier4_autoware_utils::Polygon2d translated_polygon;
6869
const boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translation(x, y);
6970
boost::geometry::transform(polygon, translated_polygon, translation);
7071
return translated_polygon;
7172
}
7273

73-
tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint)
74+
tier4_autoware_utils::Polygon2d create_footprint(
75+
const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint)
7476
{
7577
const auto angle = tf2::getYaw(pose.orientation);
7678
return translate_polygon(
7779
tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y);
7880
}
7981

8082
tier4_autoware_utils::MultiPolygon2d create_object_footprints(
81-
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects, const PlannerParam & params)
83+
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
84+
const PlannerParam & params)
8285
{
8386
tier4_autoware_utils::MultiPolygon2d footprints;
8487

planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "types.hpp"
1919

2020
#include <tier4_autoware_utils/geometry/geometry.hpp>
21+
2122
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
2223

2324
#include <vector>
@@ -42,9 +43,12 @@ tier4_autoware_utils::Polygon2d make_forward_footprint(
4243
const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params,
4344
const double hysteresis);
4445
tier4_autoware_utils::MultiPolygon2d create_object_footprints(
45-
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects, const PlannerParam & params);
46-
tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y);
47-
tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint);
46+
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
47+
const PlannerParam & params);
48+
tier4_autoware_utils::Polygon2d translate_polygon(
49+
const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y);
50+
tier4_autoware_utils::Polygon2d create_footprint(
51+
const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint);
4852
/// @brief project a footprint to the given pose
4953
/// @param [in] base_footprint footprint to project
5054
/// @param [in] pose projection pose

planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node
4949
getOrDeclareParameter<bool>(node, ns + ".ignore_unavoidable_collisions");
5050
pp.ignore_objects_behind_ego =
5151
getOrDeclareParameter<bool>(node, ns + ".ignore_objects_behind_ego");
52-
pp.use_predicted_path =
53-
getOrDeclareParameter<bool>(node, ns + ".use_predicted_path");
52+
pp.use_predicted_path = getOrDeclareParameter<bool>(node, ns + ".use_predicted_path");
5453
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
5554
pp.ego_lateral_offset =
5655
std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m);

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
9090
tier4_autoware_utils::MultiPolygon2d obstacle_predicted_footprints;
9191
std::vector<Collision> collisions;
9292
double footprints_duration_us;
93-
if (params_.use_predicted_path){
93+
if (params_.use_predicted_path) {
9494
stopwatch.tic("footprints");
9595
obstacle_predicted_footprints = create_object_footprints(dynamic_obstacles, params_);
9696
footprints_duration_us = stopwatch.toc("footprints");
@@ -100,12 +100,10 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
100100
debug_data_.obstacle_footprints = obstacle_predicted_footprints;
101101
} else {
102102
stopwatch.tic("footprints");
103-
obstacle_forward_footprints =
104-
make_forward_footprints(dynamic_obstacles, params_, hysteresis);
103+
obstacle_forward_footprints = make_forward_footprints(dynamic_obstacles, params_, hysteresis);
105104
footprints_duration_us = stopwatch.toc("footprints");
106105
stopwatch.tic("collisions");
107-
collisions =
108-
find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
106+
collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
109107
debug_data_.obstacle_footprints = obstacle_forward_footprints;
110108
}
111109
update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);

0 commit comments

Comments
 (0)