Skip to content

Commit e017725

Browse files
authored
refactor(crosswalk): use getOrDeclareParameter (#4765)
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
1 parent bb702a6 commit e017725

File tree

1 file changed

+43
-35
lines changed
  • planning/behavior_velocity_crosswalk_module/src

1 file changed

+43
-35
lines changed

planning/behavior_velocity_crosswalk_module/src/manager.cpp

+43-35
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include "manager.hpp"
1616

1717
#include <behavior_velocity_planner_common/utilization/util.hpp>
18+
#include <tier4_autoware_utils/ros/parameter.hpp>
1819

1920
#include <limits>
2021
#include <memory>
@@ -26,86 +27,93 @@ namespace behavior_velocity_planner
2627
{
2728

2829
using lanelet::autoware::Crosswalk;
30+
using tier4_autoware_utils::getOrDeclareParameter;
2931

3032
CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
3133
: SceneModuleManagerInterfaceWithRTC(
3234
node, getModuleName(),
33-
node.declare_parameter<bool>(std::string(getModuleName()) + ".common.enable_rtc"))
35+
getOrDeclareParameter<bool>(node, std::string(getModuleName()) + ".common.enable_rtc"))
3436
{
3537
const std::string ns(getModuleName());
3638

3739
// for crosswalk parameters
3840
auto & cp = crosswalk_planner_param_;
39-
cp.show_processing_time = node.declare_parameter<bool>(ns + ".common.show_processing_time");
41+
cp.show_processing_time = getOrDeclareParameter<bool>(node, ns + ".common.show_processing_time");
4042

4143
// param for input data
4244
cp.traffic_light_state_timeout =
43-
node.declare_parameter<double>(ns + ".common.traffic_light_state_timeout");
45+
getOrDeclareParameter<double>(node, ns + ".common.traffic_light_state_timeout");
4446

4547
// param for stop position
4648
cp.stop_distance_from_crosswalk =
47-
node.declare_parameter<double>(ns + ".stop_position.stop_distance_from_crosswalk");
49+
getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_crosswalk");
4850
cp.stop_distance_from_object =
49-
node.declare_parameter<double>(ns + ".stop_position.stop_distance_from_object");
51+
getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object");
5052
cp.far_object_threshold =
51-
node.declare_parameter<double>(ns + ".stop_position.far_object_threshold");
53+
getOrDeclareParameter<double>(node, ns + ".stop_position.far_object_threshold");
5254
cp.stop_position_threshold =
53-
node.declare_parameter<double>(ns + ".stop_position.stop_position_threshold");
55+
getOrDeclareParameter<double>(node, ns + ".stop_position.stop_position_threshold");
5456

5557
// param for ego velocity
5658
cp.min_slow_down_velocity =
57-
node.declare_parameter<double>(ns + ".slow_down.min_slow_down_velocity");
58-
cp.max_slow_down_jerk = node.declare_parameter<double>(ns + ".slow_down.max_slow_down_jerk");
59-
cp.max_slow_down_accel = node.declare_parameter<double>(ns + ".slow_down.max_slow_down_accel");
60-
cp.no_relax_velocity = node.declare_parameter<double>(ns + ".slow_down.no_relax_velocity");
59+
getOrDeclareParameter<double>(node, ns + ".slow_down.min_slow_down_velocity");
60+
cp.max_slow_down_jerk = getOrDeclareParameter<double>(node, ns + ".slow_down.max_slow_down_jerk");
61+
cp.max_slow_down_accel =
62+
getOrDeclareParameter<double>(node, ns + ".slow_down.max_slow_down_accel");
63+
cp.no_relax_velocity = getOrDeclareParameter<double>(node, ns + ".slow_down.no_relax_velocity");
6164

6265
// param for stuck vehicle
6366
cp.stuck_vehicle_velocity =
64-
node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_velocity");
67+
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity");
6568
cp.max_stuck_vehicle_lateral_offset =
66-
node.declare_parameter<double>(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset");
69+
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset");
6770
cp.stuck_vehicle_attention_range =
68-
node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_attention_range");
69-
cp.min_acc_for_stuck_vehicle = node.declare_parameter<double>(ns + ".stuck_vehicle.min_acc");
70-
cp.max_jerk_for_stuck_vehicle = node.declare_parameter<double>(ns + ".stuck_vehicle.max_jerk");
71-
cp.min_jerk_for_stuck_vehicle = node.declare_parameter<double>(ns + ".stuck_vehicle.min_jerk");
71+
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range");
72+
cp.min_acc_for_stuck_vehicle = getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.min_acc");
73+
cp.max_jerk_for_stuck_vehicle =
74+
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_jerk");
75+
cp.min_jerk_for_stuck_vehicle =
76+
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.min_jerk");
7277

7378
// param for pass judge logic
7479
cp.ego_pass_first_margin_x =
75-
node.declare_parameter<std::vector<double>>(ns + ".pass_judge.ego_pass_first_margin_x");
80+
getOrDeclareParameter<std::vector<double>>(node, ns + ".pass_judge.ego_pass_first_margin_x");
7681
cp.ego_pass_first_margin_y =
77-
node.declare_parameter<std::vector<double>>(ns + ".pass_judge.ego_pass_first_margin_y");
82+
getOrDeclareParameter<std::vector<double>>(node, ns + ".pass_judge.ego_pass_first_margin_y");
7883
cp.ego_pass_first_additional_margin =
79-
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_first_additional_margin");
84+
getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_pass_first_additional_margin");
8085
cp.ego_pass_later_margin_x =
81-
node.declare_parameter<std::vector<double>>(ns + ".pass_judge.ego_pass_later_margin_x");
86+
getOrDeclareParameter<std::vector<double>>(node, ns + ".pass_judge.ego_pass_later_margin_x");
8287
cp.ego_pass_later_margin_y =
83-
node.declare_parameter<std::vector<double>>(ns + ".pass_judge.ego_pass_later_margin_y");
88+
getOrDeclareParameter<std::vector<double>>(node, ns + ".pass_judge.ego_pass_later_margin_y");
8489
cp.ego_pass_later_additional_margin =
85-
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_later_additional_margin");
90+
getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_pass_later_additional_margin");
8691
cp.max_offset_to_crosswalk_for_yield =
87-
node.declare_parameter<double>(ns + ".pass_judge.max_offset_to_crosswalk_for_yield");
92+
getOrDeclareParameter<double>(node, ns + ".pass_judge.max_offset_to_crosswalk_for_yield");
8893
cp.stop_object_velocity =
89-
node.declare_parameter<double>(ns + ".pass_judge.stop_object_velocity_threshold");
90-
cp.min_object_velocity = node.declare_parameter<double>(ns + ".pass_judge.min_object_velocity");
94+
getOrDeclareParameter<double>(node, ns + ".pass_judge.stop_object_velocity_threshold");
95+
cp.min_object_velocity =
96+
getOrDeclareParameter<double>(node, ns + ".pass_judge.min_object_velocity");
9197
cp.disable_stop_for_yield_cancel =
92-
node.declare_parameter<bool>(ns + ".pass_judge.disable_stop_for_yield_cancel");
98+
getOrDeclareParameter<bool>(node, ns + ".pass_judge.disable_stop_for_yield_cancel");
9399
cp.disable_yield_for_new_stopped_object =
94-
node.declare_parameter<bool>(ns + ".pass_judge.disable_yield_for_new_stopped_object");
100+
getOrDeclareParameter<bool>(node, ns + ".pass_judge.disable_yield_for_new_stopped_object");
95101
cp.timeout_no_intention_to_walk =
96-
node.declare_parameter<double>(ns + ".pass_judge.timeout_no_intention_to_walk");
102+
getOrDeclareParameter<double>(node, ns + ".pass_judge.timeout_no_intention_to_walk");
97103
cp.timeout_ego_stop_for_yield =
98-
node.declare_parameter<double>(ns + ".pass_judge.timeout_ego_stop_for_yield");
104+
getOrDeclareParameter<double>(node, ns + ".pass_judge.timeout_ego_stop_for_yield");
99105

100106
// param for target area & object
101107
cp.crosswalk_attention_range =
102-
node.declare_parameter<double>(ns + ".object_filtering.crosswalk_attention_range");
103-
cp.look_unknown = node.declare_parameter<bool>(ns + ".object_filtering.target_object.unknown");
104-
cp.look_bicycle = node.declare_parameter<bool>(ns + ".object_filtering.target_object.bicycle");
108+
getOrDeclareParameter<double>(node, ns + ".object_filtering.crosswalk_attention_range");
109+
cp.look_unknown =
110+
getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.unknown");
111+
cp.look_bicycle =
112+
getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.bicycle");
105113
cp.look_motorcycle =
106-
node.declare_parameter<bool>(ns + ".object_filtering.target_object.motorcycle");
114+
getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.motorcycle");
107115
cp.look_pedestrian =
108-
node.declare_parameter<bool>(ns + ".object_filtering.target_object.pedestrian");
116+
getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.pedestrian");
109117
}
110118

111119
void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)

0 commit comments

Comments
 (0)