15
15
#include " manager.hpp"
16
16
17
17
#include < behavior_velocity_planner_common/utilization/util.hpp>
18
+ #include < tier4_autoware_utils/ros/parameter.hpp>
18
19
19
20
#include < limits>
20
21
#include < memory>
@@ -26,86 +27,93 @@ namespace behavior_velocity_planner
26
27
{
27
28
28
29
using lanelet::autoware::Crosswalk;
30
+ using tier4_autoware_utils::getOrDeclareParameter;
29
31
30
32
CrosswalkModuleManager::CrosswalkModuleManager (rclcpp::Node & node)
31
33
: SceneModuleManagerInterfaceWithRTC(
32
34
node, getModuleName(),
33
- node.declare_parameter <bool >(std::string(getModuleName()) + " .common.enable_rtc" ))
35
+ getOrDeclareParameter <bool >(node, std::string(getModuleName()) + " .common.enable_rtc" ))
34
36
{
35
37
const std::string ns (getModuleName ());
36
38
37
39
// for crosswalk parameters
38
40
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" );
40
42
41
43
// param for input data
42
44
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" );
44
46
45
47
// param for stop position
46
48
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" );
48
50
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" );
50
52
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" );
52
54
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" );
54
56
55
57
// param for ego velocity
56
58
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" );
61
64
62
65
// param for stuck vehicle
63
66
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" );
65
68
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" );
67
70
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" );
72
77
73
78
// param for pass judge logic
74
79
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" );
76
81
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" );
78
83
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" );
80
85
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" );
82
87
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" );
84
89
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" );
86
91
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" );
88
93
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" );
91
97
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" );
93
99
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" );
95
101
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" );
97
103
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" );
99
105
100
106
// param for target area & object
101
107
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" );
105
113
cp.look_motorcycle =
106
- node. declare_parameter <bool >(ns + " .object_filtering.target_object.motorcycle" );
114
+ getOrDeclareParameter <bool >(node, ns + " .object_filtering.target_object.motorcycle" );
107
115
cp.look_pedestrian =
108
- node. declare_parameter <bool >(ns + " .object_filtering.target_object.pedestrian" );
116
+ getOrDeclareParameter <bool >(node, ns + " .object_filtering.target_object.pedestrian" );
109
117
}
110
118
111
119
void CrosswalkModuleManager::launchNewModules (const PathWithLaneId & path)
0 commit comments