-
Notifications
You must be signed in to change notification settings - Fork 673
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
- Loading branch information
1 parent
24e542c
commit acf6198
Showing
11 changed files
with
616 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
99 changes: 99 additions & 0 deletions
99
...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,99 @@ | ||
| // Copyright 2021 Tier IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ | ||
| #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ | ||
|
|
||
| #include "behavior_path_planner/scene_module/scene_module_interface.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <autoware_auto_perception_msgs/msg/predicted_object.hpp> | ||
| #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp> | ||
| #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp> | ||
| #include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp> | ||
| #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp> | ||
|
|
||
| #include <algorithm> | ||
| #include <memory> | ||
| #include <string> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| namespace behavior_path_planner | ||
| { | ||
| struct DynamicAvoidanceParameters | ||
| { | ||
| bool avoid_car{true}; | ||
| bool avoid_truck{true}; | ||
| bool avoid_bus{true}; | ||
| bool avoid_trailer{true}; | ||
| bool avoid_unknown{false}; | ||
| bool avoid_bicycle{false}; | ||
| bool avoid_motorcycle{false}; | ||
| bool avoid_pedestrian{false}; | ||
|
|
||
| // drivable area expansion | ||
| double drivable_area_right_bound_offset{}; | ||
| double drivable_area_left_bound_offset{}; | ||
| std::vector<std::string> drivable_area_types_to_skip{}; | ||
| }; | ||
|
|
||
| class DynamicAvoidanceModule : public SceneModuleInterface | ||
| { | ||
| public: | ||
| struct DynamicAvoidanceObject | ||
| { | ||
| explicit DynamicAvoidanceObject(const PredictedObject & predicted_object) | ||
| : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), | ||
| shape(predicted_object.shape) | ||
| { | ||
| } | ||
|
|
||
| geometry_msgs::msg::Pose pose; | ||
| autoware_auto_perception_msgs::msg::Shape shape; | ||
| }; | ||
|
|
||
| DynamicAvoidanceModule( | ||
| const std::string & name, rclcpp::Node & node, | ||
| std::shared_ptr<DynamicAvoidanceParameters> parameters); | ||
|
|
||
| bool isExecutionRequested() const override; | ||
| bool isExecutionReady() const override; | ||
| ModuleStatus updateState() override; | ||
| ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } | ||
| BehaviorModuleOutput plan() override; | ||
| CandidateOutput planCandidate() const override; | ||
| BehaviorModuleOutput planWaitingApproval() override; | ||
| void onEntry() override; | ||
| void onExit() override; | ||
| // void updateData() override; | ||
| void acceptVisitor( | ||
| [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override | ||
| { | ||
| } | ||
|
|
||
| private: | ||
| std::vector<DynamicAvoidanceObject> calcTargetObjects() const; | ||
| lanelet::ConstLanelets getAdjacentLanes( | ||
| const double forward_distance, const double backward_distance) const; | ||
| Pose getEgoPose() const; | ||
| tier4_autoware_utils::Polygon2d calcDynamicObstaclesPolygon( | ||
| const PathWithLaneId & path, const DynamicAvoidanceObject & object); | ||
|
|
||
| std::shared_ptr<DynamicAvoidanceParameters> parameters_; | ||
| }; | ||
| } // namespace behavior_path_planner | ||
|
|
||
| #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ |
53 changes: 53 additions & 0 deletions
53
...ior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,53 @@ | ||
| // Copyright 2023 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ | ||
| #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ | ||
|
|
||
| #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" | ||
| #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <vector> | ||
|
|
||
| namespace behavior_path_planner | ||
| { | ||
|
|
||
| class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface | ||
| { | ||
| public: | ||
| DynamicAvoidanceModuleManager( | ||
| rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, | ||
| const std::shared_ptr<DynamicAvoidanceParameters> & parameters); | ||
|
|
||
| std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override | ||
| { | ||
| return std::make_shared<DynamicAvoidanceModule>( | ||
| name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_); | ||
| } | ||
|
|
||
| void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override; | ||
|
|
||
| private: | ||
| std::shared_ptr<DynamicAvoidanceParameters> parameters_; | ||
| std::vector<std::shared_ptr<DynamicAvoidanceModule>> registered_modules_; | ||
| }; | ||
|
|
||
| } // namespace behavior_path_planner | ||
|
|
||
| #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.