forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(motion_velocity_planner): add new motion velocity planning (auto…
…warefoundation#7064) TODO: fix launch files Signed-off-by: Maxime CLEMENT <[email protected]>
- Loading branch information
1 parent
fe851d9
commit f5d6d9a
Showing
50 changed files
with
4,836 additions
and
1 deletion.
There are no files selected for viewing
52 changes: 52 additions & 0 deletions
52
common/motion_utils/include/motion_utils/factor/velocity_factor_interface.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,52 @@ | ||
|
|
||
| // Copyright 2022-2024 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 MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ | ||
| #define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ | ||
|
|
||
| #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp> | ||
| #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp> | ||
| #include <geometry_msgs/msg/pose.hpp> | ||
|
|
||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| namespace motion_utils | ||
| { | ||
| using autoware_adapi_v1_msgs::msg::VelocityFactor; | ||
| using VelocityFactorBehavior = VelocityFactor::_type_type; | ||
| using VelocityFactorStatus = VelocityFactor::_status_type; | ||
| using geometry_msgs::msg::Pose; | ||
|
|
||
| class VelocityFactorInterface | ||
| { | ||
| public: | ||
| [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } | ||
| void init(const VelocityFactorBehavior & behavior) { type_ = behavior; } | ||
| void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } | ||
|
|
||
| template <class PointType> | ||
| void set( | ||
| const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose, | ||
| const VelocityFactorStatus status, const std::string & detail = ""); | ||
|
|
||
| private: | ||
| VelocityFactorBehavior type_{VelocityFactor::UNKNOWN}; | ||
| VelocityFactor velocity_factor_{}; | ||
| }; | ||
|
|
||
| } // namespace motion_utils | ||
|
|
||
| #endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ |
49 changes: 49 additions & 0 deletions
49
common/motion_utils/src/factor/velocity_factor_interface.cpp
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,49 @@ | ||
| // Copyright 2023-2024 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. | ||
|
|
||
| #include <motion_utils/factor/velocity_factor_interface.hpp> | ||
| #include <motion_utils/trajectory/trajectory.hpp> | ||
|
|
||
| #include <autoware_auto_planning_msgs/msg/path_point.hpp> | ||
| #include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp> | ||
| #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp> | ||
|
|
||
| namespace motion_utils | ||
| { | ||
| template <class PointType> | ||
| void VelocityFactorInterface::set( | ||
| const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose, | ||
| const VelocityFactorStatus status, const std::string & detail) | ||
| { | ||
| const auto & curr_point = curr_pose.position; | ||
| const auto & stop_point = stop_pose.position; | ||
| velocity_factor_.type = type_; | ||
| velocity_factor_.pose = stop_pose; | ||
| velocity_factor_.distance = | ||
| static_cast<float>(motion_utils::calcSignedArcLength(points, curr_point, stop_point)); | ||
| velocity_factor_.status = status; | ||
| velocity_factor_.detail = detail; | ||
| } | ||
|
|
||
| template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPointWithLaneId>( | ||
| const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, | ||
| const Pose &, const VelocityFactorStatus, const std::string &); | ||
| template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPoint>( | ||
| const std::vector<autoware_auto_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &, | ||
| const VelocityFactorStatus, const std::string &); | ||
| template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::TrajectoryPoint>( | ||
| const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> &, const Pose &, | ||
| const Pose &, const VelocityFactorStatus, const std::string &); | ||
|
|
||
| } // namespace motion_utils |
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,89 @@ | ||
| nav: | ||
| - 'Introduction': planning | ||
| - 'Behavior Path Planner': | ||
| - 'About Behavior Path': planning/behavior_path_planner | ||
| - 'Concept': | ||
| - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design | ||
| - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design | ||
| - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design | ||
| - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check | ||
| - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design | ||
| - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design | ||
| - 'Scene Module': | ||
| - 'Avoidance': planning/behavior_path_avoidance_module | ||
| - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module | ||
| - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module | ||
| - 'Goal Planner': planning/behavior_path_goal_planner_module | ||
| - 'Lane Change': planning/behavior_path_lane_change_module | ||
| - 'Side Shift': planning/behavior_path_side_shift_module | ||
| - 'Start Planner': planning/behavior_path_start_planner_module | ||
| - 'Behavior Velocity Planner': | ||
| - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner | ||
| - 'Template for Custom Module': planning/behavior_velocity_template_module | ||
| - 'Available Module': | ||
| - 'Blind Spot': planning/behavior_velocity_blind_spot_module | ||
| - 'Crosswalk': planning/behavior_velocity_crosswalk_module | ||
| - 'Detection Area': planning/behavior_velocity_detection_area_module | ||
| - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module | ||
| - 'Intersection': planning/behavior_velocity_intersection_module | ||
| - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module | ||
| - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module | ||
| - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module | ||
| - 'Out of Lane': planning/behavior_velocity_out_of_lane_module | ||
| - 'Run Out': planning/behavior_velocity_run_out_module | ||
| - 'Speed Bump': planning/behavior_velocity_speed_bump_module | ||
| - 'Stop Line': planning/behavior_velocity_stop_line_module | ||
| - 'Traffic Light': planning/behavior_velocity_traffic_light_module | ||
| - 'Virtual Traffic Light': planning/behavior_velocity_virtual_traffic_light_module | ||
| - 'Walkway': planning/behavior_velocity_walkway_module | ||
| - 'Parking': | ||
| - 'Freespace Planner': | ||
| - 'About Freespace Planner': planning/freespace_planner | ||
| - 'Algorithm': planning/freespace_planning_algorithms | ||
| - 'RRT*': planning/freespace_planning_algorithms/rrtstar | ||
| - 'Mission Planner': planning/mission_planner | ||
| - 'Motion Planning': | ||
| - 'Obstacle Avoidance Planner': | ||
| - 'About Obstacle Avoidance': planning/obstacle_avoidance_planner | ||
| - 'Model Predictive Trajectory (MPT)': planning/obstacle_avoidance_planner/docs/mpt | ||
| - 'How to Debug': planning/obstacle_avoidance_planner/docs/debug | ||
| - 'Obstacle Cruise Planner': | ||
| - 'About Obstacle Cruise': planning/obstacle_cruise_planner | ||
| - 'How to Debug': planning/obstacle_cruise_planner/docs/debug | ||
| - 'Obstacle Stop Planner': planning/obstacle_stop_planner | ||
| - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter | ||
| - 'Path Smoother': | ||
| - 'About Path Smoother': planning/path_smoother | ||
| - 'Elastic Band': planning/path_smoother/docs/eb | ||
| - 'Sampling Based Planner': | ||
| - 'Path Sample': planning/sampling_based_planner/path_sampler | ||
| - 'Common library': planning/sampling_based_planner/sampler_common | ||
| - 'Frenet Planner': planning/sampling_based_planner/frenet_planner | ||
| - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler | ||
| - 'Surround Obstacle Checker': | ||
| - 'About Surround Obstacle Checker': planning/surround_obstacle_checker | ||
| - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja | ||
| - 'Motion Velocity Planner': | ||
| - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ | ||
| - 'Available Modules': | ||
| - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ | ||
| - 'Motion Velocity Smoother': | ||
| - 'About Motion Velocity Smoother': planning/motion_velocity_smoother | ||
| - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja | ||
| - 'Scenario Selector': planning/scenario_selector | ||
| - 'Static Centerline Generator': planning/autoware_static_centerline_generator | ||
| - 'API and Library': | ||
| - 'Costmap Generator': planning/costmap_generator | ||
| - 'External Velocity Limit Selector': planning/external_velocity_limit_selector | ||
| - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface | ||
| - 'Route Handler': planning/route_handler | ||
| - 'RTC Interface': planning/rtc_interface | ||
| - 'Additional Tools': | ||
| - 'RTC Replayer': planning/rtc_replayer | ||
| - 'Planning Debug Tools': | ||
| - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools | ||
| - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md | ||
| - 'Planning Test Utils': planning/planning_test_utils | ||
| - 'Planning Test Manager': planning/autoware_planning_test_manager | ||
| - 'Planning Topic Converter': planning/planning_topic_converter | ||
| - 'Planning Validator': planning/planning_validator |
17 changes: 17 additions & 0 deletions
17
planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt
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,17 @@ | ||
| cmake_minimum_required(VERSION 3.14) | ||
| project(autoware_motion_velocity_out_of_lane_module) | ||
|
|
||
| find_package(autoware_cmake REQUIRED) | ||
| autoware_package() | ||
| pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) | ||
|
|
||
| ament_auto_add_library(${PROJECT_NAME} SHARED | ||
| DIRECTORY src | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() | ||
|
|
||
| ament_auto_package(INSTALL_TO_SHARE config) |
Oops, something went wrong.