Skip to content

Commit 13d45ae

Browse files
satoshi-otasatoshi-ota
authoredDec 7, 2023
refactor(avoidance, avoidance_by_lane_change): separate package (autowarefoundation#5790)
* refactor(avoidance): separate package Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(AbLC): separate package Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(bpp): remove separate module Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(bpp): fix test error Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent ab4a3eb commit 13d45ae

38 files changed

+832
-304
lines changed
 

‎planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(behavior_path_avoidance_by_lane_change_module)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml)
7+
8+
ament_auto_add_library(${PROJECT_NAME} SHARED
9+
src/scene.cpp
10+
src/manager.cpp
11+
src/interface.cpp
12+
)
13+
14+
if(BUILD_TESTING)
15+
ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
16+
test/test_behavior_path_planner_node_interface.cpp
17+
)
18+
19+
target_link_libraries(test_${PROJECT_NAME}
20+
${PROJECT_NAME}
21+
)
22+
23+
target_include_directories(test_${PROJECT_NAME} PRIVATE src)
24+
endif()
25+
26+
ament_auto_package(INSTALL_TO_SHARE config)

‎planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
// Copyright 2022 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+
#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
15+
#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
16+
17+
#include "behavior_path_avoidance_module/data_structs.hpp"
18+
19+
#include <vector>
20+
21+
namespace behavior_path_planner
22+
{
23+
struct AvoidanceByLCParameters : public AvoidanceParameters
24+
{
25+
// execute only when the target object longitudinal distance is larger than this param.
26+
double execute_object_longitudinal_margin{0.0};
27+
28+
// execute only when lane change end point is before the object.
29+
bool execute_only_when_lane_change_finish_before_object{false};
30+
};
31+
} // namespace behavior_path_planner
32+
33+
#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_

‎planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp

+48
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
// Copyright 2023 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 BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
16+
#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
17+
18+
#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
19+
#include "behavior_path_avoidance_by_lane_change_module/scene.hpp"
20+
#include "behavior_path_planner/scene_module/lane_change/interface.hpp"
21+
22+
#include <rclcpp/rclcpp.hpp>
23+
24+
#include <memory>
25+
#include <string>
26+
#include <unordered_map>
27+
28+
namespace behavior_path_planner
29+
{
30+
class AvoidanceByLaneChangeInterface : public LaneChangeInterface
31+
{
32+
public:
33+
AvoidanceByLaneChangeInterface(
34+
const std::string & name, rclcpp::Node & node,
35+
const std::shared_ptr<LaneChangeParameters> & parameters,
36+
const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
37+
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
38+
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
39+
objects_of_interest_marker_interface_ptr_map);
40+
41+
bool isExecutionRequested() const override;
42+
43+
protected:
44+
void updateRTCStatus(const double start_distance, const double finish_distance) override;
45+
};
46+
} // namespace behavior_path_planner
47+
48+
#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_

‎planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
// Copyright 2023 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 BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_
16+
#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_
17+
18+
#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
19+
#include "behavior_path_avoidance_by_lane_change_module/interface.hpp"
20+
#include "behavior_path_planner/scene_module/lane_change/manager.hpp"
21+
22+
#include <rclcpp/rclcpp.hpp>
23+
24+
#include <memory>
25+
#include <string>
26+
#include <unordered_map>
27+
#include <vector>
28+
29+
namespace behavior_path_planner
30+
{
31+
using route_handler::Direction;
32+
33+
class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
34+
{
35+
public:
36+
AvoidanceByLaneChangeModuleManager()
37+
: LaneChangeModuleManager(
38+
"avoidance_by_lc", route_handler::Direction::NONE,
39+
LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE)
40+
{
41+
}
42+
43+
void init(rclcpp::Node * node) override;
44+
45+
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
46+
47+
private:
48+
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters_;
49+
};
50+
} // namespace behavior_path_planner
51+
52+
#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp ‎planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

+4-6
Original file line numberDiff line numberDiff line change
@@ -12,18 +12,16 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
16-
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
15+
#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
16+
#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
1717

18+
#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
1819
#include "behavior_path_planner/scene_module/lane_change/normal.hpp"
1920

2021
#include <memory>
2122

2223
namespace behavior_path_planner
2324
{
24-
using autoware_auto_planning_msgs::msg::PathWithLaneId;
25-
using geometry_msgs::msg::Pose;
26-
using geometry_msgs::msg::Twist;
2725
using AvoidanceDebugData = DebugData;
2826

2927
class AvoidanceByLaneChange : public NormalLaneChange
@@ -56,4 +54,4 @@ class AvoidanceByLaneChange : public NormalLaneChange
5654
};
5755
} // namespace behavior_path_planner
5856

59-
#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
57+
#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_

‎planning/behavior_path_avoidance_by_lane_change_module/package.xml

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>behavior_path_avoidance_by_lane_change_module</name>
5+
<version>0.1.0</version>
6+
<description>The behavior_path_avoidance_by_lane_change_module package</description>
7+
8+
<maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
9+
<maintainer email="zulfaqar.azmi@tier4.jp">Zulfaqar Azmi</maintainer>
10+
<maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
11+
12+
<license>Apache License 2.0</license>
13+
14+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
15+
<buildtool_depend>autoware_cmake</buildtool_depend>
16+
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
17+
18+
<depend>behavior_path_avoidance_module</depend>
19+
<depend>behavior_path_planner</depend>
20+
<depend>behavior_path_planner_common</depend>
21+
<depend>motion_utils</depend>
22+
<depend>pluginlib</depend>
23+
<depend>rclcpp</depend>
24+
<depend>rtc_interface</depend>
25+
<depend>tier4_autoware_utils</depend>
26+
<depend>tier4_planning_msgs</depend>
27+
<depend>visualization_msgs</depend>
28+
29+
<test_depend>ament_cmake_ros</test_depend>
30+
<test_depend>ament_lint_auto</test_depend>
31+
<test_depend>autoware_lint_common</test_depend>
32+
33+
<export>
34+
<build_type>ament_cmake</build_type>
35+
</export>
36+
</package>

‎planning/behavior_path_avoidance_by_lane_change_module/plugins.xml

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
<library path="behavior_path_avoidance_by_lane_change_module">
2+
<class type="behavior_path_planner::AvoidanceByLaneChangeModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
3+
</library>

‎planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// Copyright 2023 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 "behavior_path_avoidance_by_lane_change_module/interface.hpp"
16+
17+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
18+
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
19+
#include "behavior_path_planner_common/marker_utils/utils.hpp"
20+
21+
#include <algorithm>
22+
#include <memory>
23+
#include <string>
24+
#include <utility>
25+
#include <vector>
26+
27+
namespace behavior_path_planner
28+
{
29+
AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
30+
const std::string & name, rclcpp::Node & node,
31+
const std::shared_ptr<LaneChangeParameters> & parameters,
32+
const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
33+
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
34+
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
35+
objects_of_interest_marker_interface_ptr_map)
36+
: LaneChangeInterface{
37+
name,
38+
node,
39+
parameters,
40+
rtc_interface_ptr_map,
41+
objects_of_interest_marker_interface_ptr_map,
42+
std::make_unique<AvoidanceByLaneChange>(parameters, avoidance_by_lane_change_parameters)}
43+
{
44+
}
45+
46+
bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
47+
{
48+
return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired();
49+
}
50+
51+
void AvoidanceByLaneChangeInterface::updateRTCStatus(
52+
const double start_distance, const double finish_distance)
53+
{
54+
const auto direction = std::invoke([&]() -> std::string {
55+
const auto dir = module_type_->getDirection();
56+
return (dir == Direction::LEFT) ? "left" : "right";
57+
});
58+
59+
rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
60+
uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
61+
}
62+
} // namespace behavior_path_planner

0 commit comments

Comments
 (0)