This repository has been archived by the owner on Jul 3, 2023. It is now read-only.
forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(behavior_path_planner): add dynamic obstacle avoidance module (a…
…utowarefoundation#3415) * implement dynamic avoidance module Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix spell Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * Update planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * dealt with review Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * disable dynamic avoidance with old architecture, and pass build CI Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
- Loading branch information
1 parent
7bd2b78
commit 24bce1e
Showing
15 changed files
with
706 additions
and
12 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
20 changes: 20 additions & 0 deletions
20
planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
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,20 @@ | ||
| /**: | ||
| ros__parameters: | ||
| dynamic_avoidance: | ||
| # avoidance is performed for the object type with true | ||
| target_object: | ||
| car: true | ||
| truck: true | ||
| bus: true | ||
| trailer: true | ||
| unknown: false | ||
| bicycle: false | ||
| motorcycle: true | ||
| pedestrian: false | ||
|
|
||
| min_obstacle_vel: 1.0 # [m/s] | ||
|
|
||
| drivable_area_generation: | ||
| lat_offset_from_obstacle: 0.8 # [m] | ||
| time_to_avoid: 5.0 # [s] | ||
| max_lat_offset_to_avoid: 1.0 # [m] |
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
22 changes: 22 additions & 0 deletions
22
...ng/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md
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,22 @@ | ||
| # Dynamic avoidance design | ||
|
|
||
| ## Purpose / Role | ||
|
|
||
| This is a module designed for avoiding obstacles which are running. | ||
| Static obstacles such as parked vehicles are dealt with by the avoidance module. | ||
|
|
||
| This module is under development. | ||
| In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area. | ||
| Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory. | ||
|
|
||
| ### Parameters | ||
|
|
||
| | Name | Unit | Type | Description | Default value | | ||
| | :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ | | ||
| | target_object.car | [-] | bool | The flag whether to avoid cars or not | true | | ||
| | target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true | | ||
| | ... | [-] | bool | ... | ... | | ||
| | target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 | | ||
| | drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 | | ||
| | drivable_area_generation.time_to_avoid | [s] | double | Elapsed time for avoiding an obstacle | 5.0 | | ||
| | drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 | |
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
116 changes: 116 additions & 0 deletions
116
...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,116 @@ | ||
| // 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__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 <optional> | ||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| namespace behavior_path_planner | ||
| { | ||
| struct DynamicAvoidanceParameters | ||
| { | ||
| // obstacle types to avoid | ||
| 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}; | ||
| double min_obstacle_vel{0.0}; | ||
|
|
||
| // drivable area generation | ||
| double lat_offset_from_obstacle{0.0}; | ||
| double time_to_avoid{0.0}; | ||
| double max_lat_offset_to_avoid{0.0}; | ||
| }; | ||
|
|
||
| class DynamicAvoidanceModule : public SceneModuleInterface | ||
| { | ||
| public: | ||
| struct DynamicAvoidanceObject | ||
| { | ||
| explicit DynamicAvoidanceObject( | ||
| const PredictedObject & predicted_object, const double arg_path_projected_vel) | ||
| : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), | ||
| path_projected_vel(arg_path_projected_vel), | ||
| shape(predicted_object.shape) | ||
| { | ||
| } | ||
|
|
||
| geometry_msgs::msg::Pose pose; | ||
| double path_projected_vel; | ||
| autoware_auto_perception_msgs::msg::Shape shape; | ||
| }; | ||
|
|
||
| #ifdef USE_OLD_ARCHITECTURE | ||
| DynamicAvoidanceModule( | ||
| const std::string & name, rclcpp::Node & node, | ||
| std::shared_ptr<DynamicAvoidanceParameters> parameters); | ||
| #else | ||
| DynamicAvoidanceModule( | ||
| const std::string & name, rclcpp::Node & node, | ||
| std::shared_ptr<DynamicAvoidanceParameters> parameters, | ||
| const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map); | ||
|
|
||
| void updateModuleParams(const std::shared_ptr<DynamicAvoidanceParameters> & parameters) | ||
| { | ||
| parameters_ = parameters; | ||
| } | ||
| #endif | ||
|
|
||
| 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 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; | ||
| std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon( | ||
| const PathWithLaneId & path, const DynamicAvoidanceObject & object) const; | ||
|
|
||
| std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> target_objects_; | ||
| 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_ptr_map_); | ||
| } | ||
|
|
||
| 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.