Skip to content

Commit c89de99

Browse files
soblinsoblin
committed
adding util test
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 2a9295d commit c89de99

File tree

8 files changed

+214
-62
lines changed

8 files changed

+214
-62
lines changed

‎common/autoware_test_utils/include/autoware_test_utils/visualization.hpp

+27-18
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,14 @@ inline void plot_lanelet2_object(
120120
{
121121
const auto left = lanelet.leftBound();
122122
const auto right = lanelet.rightBound();
123+
124+
const auto line_config = [&]() -> std::optional<LineConfig> {
125+
if (!config_opt) {
126+
return LineConfig{std::string(LineConfig::default_color)};
127+
}
128+
return config_opt.value().line_config;
129+
}();
130+
123131
if (config_opt) {
124132
const auto & config = config_opt.value();
125133

@@ -139,27 +147,28 @@ inline void plot_lanelet2_object(
139147
}
140148

141149
// plot lanelet-id
142-
if (config.label) {
143-
const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() +
144-
right.front().basicPoint2d() + right.back().basicPoint2d()) /
145-
4.0;
146-
axes.text(
147-
Args(center.x(), center.y(), std::to_string(lanelet.id())),
148-
Kwargs("label"_a = config.label.value()));
149-
}
150+
const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() +
151+
right.front().basicPoint2d() + right.back().basicPoint2d()) /
152+
4.0;
153+
axes.text(Args(center.x(), center.y(), std::to_string(lanelet.id())));
150154
}
151155

152-
const auto line_config = [&]() -> std::optional<LineConfig> {
153-
if (!config_opt) {
154-
return LineConfig{std::string(LineConfig::default_color)};
155-
}
156-
return config_opt.value().line_config;
157-
}();
158-
// plot left
159-
plot_lanelet2_object(lanelet.leftBound(), axes, line_config);
156+
if (config_opt && config_opt.value().label) {
157+
auto left_line_config_for_legend = line_config ? line_config.value() : LineConfig::defaults();
158+
left_line_config_for_legend.label = config_opt.value().label.value();
159+
160+
// plot left
161+
plot_lanelet2_object(lanelet.leftBound(), axes, left_line_config_for_legend);
160162

161-
// plot right
162-
plot_lanelet2_object(lanelet.rightBound(), axes, line_config);
163+
// plot right
164+
plot_lanelet2_object(lanelet.rightBound(), axes, line_config);
165+
} else {
166+
// plot left
167+
plot_lanelet2_object(lanelet.leftBound(), axes, line_config);
168+
169+
// plot right
170+
plot_lanelet2_object(lanelet.rightBound(), axes, line_config);
171+
}
163172

164173
// plot centerline direction
165174
const auto centerline_size = lanelet.centerline().size();

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
cmake_minimum_required(VERSION 3.14)
22
project(autoware_behavior_velocity_blind_spot_module)
33

4-
option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" ON)
4+
option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF)
55

66
find_package(autoware_cmake REQUIRED)
77
autoware_package()
@@ -13,6 +13,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1313
src/scene.cpp
1414
src/decisions.cpp
1515
src/util.cpp
16+
src/parameter.cpp
1617
)
1718

1819
if(BUILD_TESTING)

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_
1616
#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_
1717

18+
#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp"
1819
#include "autoware/behavior_velocity_blind_spot_module/scene.hpp"
1920

2021
#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
@@ -38,7 +39,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC
3839
const char * getModuleName() override { return "blind_spot"; }
3940

4041
private:
41-
BlindSpotModule::PlannerParam planner_param_;
42+
PlannerParam planner_param_;
4243

4344
void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
4445

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_
16+
#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_
17+
18+
#include <rclcpp/node.hpp>
19+
20+
#include <string>
21+
22+
namespace autoware::behavior_velocity_planner
23+
{
24+
struct PlannerParam
25+
{
26+
static PlannerParam init(rclcpp::Node & node, const std::string & ns);
27+
bool use_pass_judge_line{};
28+
double stop_line_margin{};
29+
double backward_detection_length{};
30+
double ignore_width_from_center_line{};
31+
double adjacent_extend_width{};
32+
double opposite_adjacent_extend_width{};
33+
double max_future_movement_time{};
34+
double ttc_min{};
35+
double ttc_max{};
36+
double ttc_ego_minimal_velocity{};
37+
};
38+
} // namespace autoware::behavior_velocity_planner
39+
40+
#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp

+1-14
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_
1616
#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_
1717

18+
#include <autoware/behavior_velocity_blind_spot_module/parameter.hpp>
1819
#include <autoware/behavior_velocity_blind_spot_module/util.hpp>
1920
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
2021
#include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
@@ -70,20 +71,6 @@ class BlindSpotModule : public SceneModuleInterface
7071
};
7172

7273
public:
73-
struct PlannerParam
74-
{
75-
bool use_pass_judge_line;
76-
double stop_line_margin;
77-
double backward_detection_length;
78-
double ignore_width_from_center_line;
79-
double adjacent_extend_width;
80-
double opposite_adjacent_extend_width;
81-
double max_future_movement_time;
82-
double ttc_min;
83-
double ttc_max;
84-
double ttc_ego_minimal_velocity;
85-
};
86-
8774
BlindSpotModule(
8875
const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
8976
const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp

+1-17
Original file line numberDiff line numberDiff line change
@@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
3434
node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc"))
3535
{
3636
const std::string ns(BlindSpotModuleManager::getModuleName());
37-
planner_param_.use_pass_judge_line =
38-
getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
39-
planner_param_.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
40-
planner_param_.backward_detection_length =
41-
getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
42-
planner_param_.ignore_width_from_center_line =
43-
getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
44-
planner_param_.adjacent_extend_width =
45-
getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
46-
planner_param_.opposite_adjacent_extend_width =
47-
getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
48-
planner_param_.max_future_movement_time =
49-
getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
50-
planner_param_.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
51-
planner_param_.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
52-
planner_param_.ttc_ego_minimal_velocity =
53-
getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
37+
planner_param_ = PlannerParam::init(node, ns);
5438
}
5539

5640
void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path)

‎planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp"
16+
17+
#include <autoware/universe_utils/ros/parameter.hpp>
18+
19+
#include <string>
20+
21+
namespace autoware::behavior_velocity_planner
22+
{
23+
24+
PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns)
25+
{
26+
using autoware::universe_utils::getOrDeclareParameter;
27+
PlannerParam param;
28+
param.use_pass_judge_line = getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
29+
param.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
30+
param.backward_detection_length =
31+
getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
32+
param.ignore_width_from_center_line =
33+
getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
34+
param.adjacent_extend_width = getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
35+
param.opposite_adjacent_extend_width =
36+
getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
37+
param.max_future_movement_time =
38+
getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
39+
param.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
40+
param.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
41+
param.ttc_ego_minimal_velocity =
42+
getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
43+
return param;
44+
}
45+
} // namespace autoware::behavior_velocity_planner

0 commit comments

Comments
 (0)