diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 6bfcffde71d60..4c3b418239a5c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp + src/pointcloud_map_loader/differential_map_loader_module.cpp src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 1683fd472e74a..c8f5bed19bb3d 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -12,6 +12,7 @@ Currently, it supports the following two types: - Publish raw pointcloud map - Publish downsampled pointcloud map - Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service #### Publish raw pointcloud map (ROS 2 topic) @@ -28,6 +29,13 @@ Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. +#### Send differential pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. + ### Parameters | Name | Type | Description | Default value | @@ -35,6 +43,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces @@ -42,6 +51,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp new file mode 100644 index 0000000000000..b42b919edb59d --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -0,0 +1,85 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 "differential_map_loader_module.hpp" + +DifferentialMapLoaderModule::DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +{ + get_differential_pcd_maps_service_ = node->create_service( + "service/get_differential_pcd_map", + std::bind( + &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void DifferentialMapLoaderModule::differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + std::vector should_remove(static_cast(cached_ids.size()), true); + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); + if (id_in_cached_list != cached_ids.end()) { + int index = id_in_cached_list - cached_ids.begin(); + should_remove[index] = false; + } else { + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = + loadPointCloudMapCellWithID(path, map_id); + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); + } + } + + for (int i = 0; i < static_cast(cached_ids.size()); ++i) { + if (should_remove[i]) { + response->ids_to_remove.push_back(cached_ids[i]); + } + } +} + +bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res) +{ + auto area = req->area; + std::vector cached_ids = req->cached_ids; + differentialAreaLoad(area, cached_ids, res); + res->header.frame_id = "map"; + return true; +} + +autoware_map_msgs::msg::PointCloudMapCellWithID +DifferentialMapLoaderModule::loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const +{ + sensor_msgs::msg::PointCloud2 pcd; + if (pcl::io::loadPCDFile(path, pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; + pointcloud_map_cell_with_id.pointcloud = pcd; + pointcloud_map_cell_with_id.cell_id = map_id; + return pointcloud_map_cell_with_id; +} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp new file mode 100644 index 0000000000000..5d6188c0b1a1f --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ + +#include "utils.hpp" + +#include + +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class DifferentialMapLoaderModule +{ + using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +public: + explicit DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; + + bool onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res); + void differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const; +}; + +#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index b42be7308b67b..ecbe481345381 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -52,6 +52,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); + bool enable_differential_load = declare_parameter("enable_differential_load"); if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; @@ -65,10 +66,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load) { + if (enable_partial_load | enable_differential_load) { pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); + } + + if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict_); + } } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index a7c289dd0f9a8..d321902131a81 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -15,6 +15,7 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#include "differential_map_loader_module.hpp" #include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" @@ -43,6 +44,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr pcd_map_loader_; std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; + std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 84c36bd093d04..48ac09b77e803 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -172,6 +172,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool skipSmoothGoalConnection( const std::vector> & statuses) const; + /** + * @brief skip smooth goal connection + */ + void computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output); + // debug rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index a6978c8e27d1a..3697ad5576b1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -288,7 +288,7 @@ std::vector generateDrivableLanes(const lanelet::ConstLanelets & std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); -size_t getOverlappedLaneletId(const std::vector & lanes); +boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index b536689ed7b18..73b19ef7ee65f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -627,6 +627,11 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->prev_output_path = path; + + // compute turn signal + computeTurnSignal(planner_data, *path, output); + + // unlock planner data mutex_pd_.unlock(); // publish drivable bounds @@ -645,26 +650,6 @@ void BehaviorPathPlannerNode::run() const auto path_candidate = getPathCandidate(output, planner_data); path_candidate_publisher_->publish(util::toPath(*path_candidate)); - // for turn signal - { - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { - turn_signal.command = TurnIndicatorsCommand::DISABLE; - hazard_signal.command = output.turn_signal_info.hazard_signal.command; - } else { - turn_signal = - turn_signal_decider_.getTurnSignal(planner_data, *path, output.turn_signal_info); - hazard_signal.command = HazardLightsCommand::DISABLE; - } - turn_signal.stamp = get_clock()->now(); - hazard_signal.stamp = get_clock()->now(); - turn_signal_publisher_->publish(turn_signal); - hazard_signal_publisher_->publish(hazard_signal); - - publish_steering_factor(turn_signal); - } - publishSceneModuleDebugMsg(); if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { @@ -677,6 +662,27 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } +void BehaviorPathPlannerNode::computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output) +{ + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { + turn_signal.command = TurnIndicatorsCommand::DISABLE; + hazard_signal.command = output.turn_signal_info.hazard_signal.command; + } else { + turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + hazard_signal.command = HazardLightsCommand::DISABLE; + } + turn_signal.stamp = get_clock()->now(); + hazard_signal.stamp = get_clock()->now(); + turn_signal_publisher_->publish(turn_signal); + hazard_signal_publisher_->publish(hazard_signal); + + publish_steering_factor(turn_signal); +} + void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index b9289248465c0..056632edf0d31 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -59,17 +59,10 @@ double calcInterpolatedVelocity( const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; return interpolated_vel; } -} // namespace - -namespace drivable_area_utils -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using route_handler::RouteHandler; template size_t findNearestSegmentIndex( - const std::vector & points, const Pose & pose, const double dist_threshold, + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { const auto nearest_idx = @@ -81,277 +74,32 @@ size_t findNearestSegmentIndex( return motion_utils::findNearestSegmentIndex(points, pose.position); } -double quantize(const double val, const double resolution) -{ - return std::round(val / resolution) * resolution; -} - -void updateMinMaxPosition( - const Eigen::Vector2d & point, boost::optional & min_x, boost::optional & min_y, - boost::optional & max_x, boost::optional & max_y) -{ - min_x = min_x ? std::min(min_x.get(), point.x()) : point.x(); - min_y = min_y ? std::min(min_y.get(), point.y()) : point.y(); - max_x = max_x ? std::max(max_x.get(), point.x()) : point.x(); - max_y = max_y ? std::max(max_y.get(), point.y()) : point.y(); -} - -bool sumLengthFromTwoPoints( - const Eigen::Vector2d & base_point, const Eigen::Vector2d & target_point, - const double length_threshold, double & sum_length, boost::optional & min_x, - boost::optional & min_y, boost::optional & max_x, boost::optional & max_y) -{ - const double norm_length = (base_point - target_point).norm(); - sum_length += norm_length; - if (length_threshold < sum_length) { - const double diff_length = norm_length - (sum_length - length_threshold); - const Eigen::Vector2d interpolated_point = - base_point + diff_length * (target_point - base_point).normalized(); - updateMinMaxPosition(interpolated_point, min_x, min_y, max_x, max_y); - - const bool is_end = true; - return is_end; - } - - updateMinMaxPosition(target_point, min_x, min_y, max_x, max_y); - const bool is_end = false; - return is_end; -} - -void fillYawFromXY(std::vector & points) -{ - if (points.size() < 2) { - return; - } - - for (size_t i = 0; i < points.size(); ++i) { - const size_t prev_idx = (i == points.size() - 1) ? i - 1 : i; - const size_t next_idx = (i == points.size() - 1) ? i : i + 1; - - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - points.at(prev_idx).position, points.at(next_idx).position); - points.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } -} - -lanelet::ConstLanelets extractLanesFromPathWithLaneId( - const std::shared_ptr & route_handler, const PathWithLaneId & path) -{ - // extract "unique" lane ids from path_with_lane_id - std::vector path_lane_ids; - for (const auto & path_point : path.points) { - for (const size_t lane_id : path_point.lane_ids) { - if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) { - path_lane_ids.push_back(lane_id); - } - } - } - - // get lanes according to lane ids - lanelet::ConstLanelets path_lanes; - path_lanes.reserve(path_lane_ids.size()); - for (const auto path_lane_id : path_lane_ids) { - const auto & lane = route_handler->getLaneletsFromId(static_cast(path_lane_id)); - path_lanes.push_back(lane); - } - - return path_lanes; -} - -size_t getNearestLaneId(const lanelet::ConstLanelets & path_lanes, const Pose & current_pose) -{ - lanelet::ConstLanelet closest_lanelet; - if (lanelet::utils::query::getClosestLanelet(path_lanes, current_pose, &closest_lanelet)) { - for (size_t i = 0; i < path_lanes.size(); ++i) { - if (path_lanes.at(i).id() == closest_lanelet.id()) { - return i; - } - } - } - return 0; -} - -void updateMinMaxPositionFromForwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & forward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - const auto forward_offset_length = motion_utils::calcSignedArcLength( - points, current_pose.position, nearest_segment_idx, nearest_segment_idx); - double sum_length = std::min(forward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - auto current_lane = path_lanes.at(current_lane_idx); - size_t current_point_idx = nearest_segment_idx; - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != bound.size() - 1) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & next_point = bound[current_point_idx + 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, next_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - - ++current_point_idx; - } else { - const auto previous_lane = current_lane; - const size_t previous_point_idx = get_bound_func(previous_lane).size() - 1; - const auto & previous_bound = get_bound_func(previous_lane); - drivable_area_utils::updateMinMaxPosition( - previous_bound[previous_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - - if (current_lane_idx == path_lanes.size() - 1) { - break; - } - - current_lane_idx += 1; - current_lane = path_lanes.at(current_lane_idx); - current_point_idx = 0; - const auto & current_bound = get_bound_func(current_lane); - - const Eigen::Vector2d & prev_point = previous_bound[previous_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - prev_point, current_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } -} - -void updateMinMaxPositionFromBackwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & backward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - size_t current_point_idx = nearest_segment_idx + 1; - const auto backward_offset_length = motion_utils::calcSignedArcLength( - points, nearest_segment_idx + 1, current_pose.position, nearest_segment_idx); - double sum_length = std::min(backward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - lanelet::ConstLanelet current_lane = path_lanes.at(current_lane_idx); - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != 0) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & prev_point = bound[current_point_idx - 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, prev_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - - --current_point_idx; - } else { - const auto next_lane = current_lane; - const size_t next_point_idx = 0; - const auto & next_bound = get_bound_func(next_lane); - drivable_area_utils::updateMinMaxPosition( - next_bound[next_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - - if (current_lane_idx == 0) { - break; - } - - current_lane_idx -= 1; - current_lane = path_lanes.at(current_lane_idx); - const auto & current_bound = get_bound_func(current_lane); - current_point_idx = current_bound.size() - 1; - - const Eigen::Vector2d & next_point = next_bound[next_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - next_point, current_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } -} -std::array getPathScope( - const PathWithLaneId & path, const std::shared_ptr & route_handler, - const Pose & current_pose, const double forward_lane_length, const double backward_lane_length, - const double lane_margin, const double max_dist, const double max_yaw) -{ - const lanelet::ConstLanelets path_lanes = extractLanesFromPathWithLaneId(route_handler, path); - - const size_t nearest_lane_idx = getNearestLaneId(path_lanes, current_pose); - - // define functions to get right/left bounds as a vector - const std::vector> - get_bound_funcs{ - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.rightBound2d(); - }, - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.leftBound2d(); - }}; - - // calculate min/max x and y - boost::optional min_x; - boost::optional min_y; - boost::optional max_x; - boost::optional max_y; - - for (const auto & get_bound_func : get_bound_funcs) { - // search nearest point index to current pose - const auto & nearest_bound = get_bound_func(path_lanes.at(nearest_lane_idx)); - if (nearest_bound.empty()) { +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & pose) +{ + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + double min_lateral_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, closest_idx, pose.position); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + if (lon_dist < 0.0 || segment_length < lon_dist) { continue; } - const std::vector points = std::invoke([&nearest_bound]() { - std::vector points; - points.reserve(nearest_bound.size()); - for (const auto & point : nearest_bound) { // calculate x and y - Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - points.push_back(p); - } - - fillYawFromXY(points); // calculate yaw - return points; - }); - - const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, current_pose, max_dist, max_yaw); - - // forward lanelet - updateMinMaxPositionFromForwardLanelet( - path_lanes, points, current_pose, forward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - - // backward lanelet - updateMinMaxPositionFromBackwardLanelet( - path_lanes, points, current_pose, backward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - } - - if (!min_x || !min_y || !max_x || !max_y) { - const double x = current_pose.position.x; - const double y = current_pose.position.y; - return {x, y, x, y}; + const double lat_dist = + std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + } } - return {min_x.get(), min_y.get(), max_x.get(), max_y.get()}; + return closest_idx; } -} // namespace drivable_area_utils +} // namespace namespace behavior_path_planner::util { @@ -1034,7 +782,7 @@ bool setGoal( // NOTE: goal does not have valid z, that will be calculated by interpolation here PathPointWithLaneId refined_goal{}; const size_t closest_seg_idx_for_goal = - drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); + findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); refined_goal.point.pose = goal; refined_goal.point.pose.position.z = calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal); @@ -1046,8 +794,8 @@ bool setGoal( constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - const size_t closest_seg_idx_for_pre_goal = drivable_area_utils::findNearestSegmentIndex( - input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); + const size_t closest_seg_idx_for_pre_goal = + findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); pre_refined_goal.point.longitudinal_velocity_mps = @@ -1269,7 +1017,7 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } -size_t getOverlappedLaneletId(const std::vector & lanes) +boost::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { const auto lanelets = transformToLanelets(lanes); @@ -1277,7 +1025,7 @@ size_t getOverlappedLaneletId(const std::vector & lanes) for (const auto & lanelet : lanelets) { for (const auto & target_lanelet : target_lanelets) { - if (boost::geometry::overlaps( + if (boost::geometry::intersects( lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) { return true; } @@ -1289,7 +1037,7 @@ size_t getOverlappedLaneletId(const std::vector & lanes) }; if (lanes.size() <= 2) { - return lanes.size(); + return {}; } size_t overlapped_idx = lanes.size(); @@ -1307,15 +1055,15 @@ size_t getOverlappedLaneletId(const std::vector & lanes) std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes) { - const size_t overlapped_lanelet_id = getOverlappedLaneletId(lanes); - if (overlapped_lanelet_id == lanes.size()) { + const auto overlapped_lanelet_id = getOverlappedLaneletId(lanes); + if (!overlapped_lanelet_id) { return lanes; } const std::vector shorten_lanes{ - lanes.begin(), lanes.begin() + overlapped_lanelet_id}; + lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; const std::vector removed_lanes{ - lanes.begin() + overlapped_lanelet_id, lanes.end()}; + lanes.begin() + *overlapped_lanelet_id, lanes.end()}; const auto transformed_lanes = util::transformToLanelets(removed_lanes); @@ -1344,16 +1092,6 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -size_t findNearestSegmentIndex( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) -{ - const auto nearest_idx = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx ? nearest_idx.get() - : motion_utils::findNearestSegmentIndex(points, pose.position); -} - geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) @@ -1389,9 +1127,6 @@ void generateDrivableArea( const auto transformed_lanes = util::transformToLanelets(lanes); const auto current_pose = planner_data->self_pose; const auto route_handler = planner_data->route_handler; - const auto & param = planner_data->parameters; - const double dist_threshold = std::numeric_limits::max(); - const double yaw_threshold = param.ego_nearest_yaw_threshold; constexpr double overlap_threshold = 0.01; auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { @@ -1427,10 +1162,30 @@ void generateDrivableArea( addRightBoundPoints(lane.right_lane); } + const auto has_same_lane = [&](const auto & lane) { + if (lanes.empty()) return false; + const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; + return std::find_if(transformed_lanes.begin(), transformed_lanes.end(), has_same) != + transformed_lanes.end(); + }; + + const auto has_overlap = [&](const auto & lane) { + for (const auto & transformed_lane : transformed_lanes) { + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + // Insert points after goal if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); for (const auto & lane : lanes_after_goal) { + if (has_same_lane(lane) || has_overlap(lane)) { + continue; + } addLeftBoundPoints(lane); addRightBoundPoints(lane); } @@ -1444,38 +1199,36 @@ void generateDrivableArea( constexpr double front_length = 0.5; const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndex(left_bound, front_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); const size_t front_right_start_idx = - findNearestSegmentIndex(right_bound, front_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose); const auto left_start_pose = calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); const auto right_start_pose = calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndex(left_bound, left_start_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); const size_t right_start_idx = - findNearestSegmentIndex(right_bound, right_start_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndex(left_bound, goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); const size_t goal_right_start_idx = - findNearestSegmentIndex(right_bound, goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose); const auto left_goal_pose = calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); const auto right_goal_pose = calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = - findNearestSegmentIndex(left_bound, left_goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); const size_t right_goal_idx = - findNearestSegmentIndex(right_bound, right_goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); // Store Data path.left_bound.clear(); path.right_bound.clear(); - path.left_bound.reserve(left_goal_idx - left_start_idx); - path.right_bound.reserve(right_goal_idx - right_start_idx); // Insert a start point path.left_bound.push_back(left_start_pose.position); diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 028352362a0ba..388f35e770359 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,12 +14,13 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| ------------------------- | ------ | --------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | +| Name | Type | Description | +| ------------------------- | ------ | ------------------------------------ | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | ### Services @@ -38,11 +39,12 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Publications -| Name | Type | Description | -| ------------------------------- | --------------------------------------- | ---------------------- | -| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | -| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| Name | Type | Description | +| ------------------------------- | --------------------------------------- | ------------------------ | +| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | +| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | +| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| `debug/goal_footprint` | visualization_msgs::msg::MarkerArray | goal footprint for debug | ## Route section @@ -57,6 +59,15 @@ The ROS message of route section contains following three elements for each rout - `preferred_primitive`: Preferred lane to follow towards the goal. - `primitives`: All neighbor lanes in the same direction including the preferred lane. +## Goal Validation + +The mission planner has control mechanism to validate the given goal pose and create a route. If goal pose angle between goal pose lanelet and goal pose' yaw is greater than `goal_angle_threshold` parameter, the goal is rejected. +Another control mechanism is the creation of a footprint of the goal pose according to the dimensions of the vehicle and checking whether this footprint is within the lanelets. If goal footprint exceeds lanelets, then the goal is rejected. + +At the image below, there are sample goal pose validation cases. + +![goal_footprints](./media/goal_footprints.svg) + ## Implementation ### Mission Planner diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 20c2cfbd2a0b7..d3a36f1e5cf6d 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -8,6 +8,7 @@ + diff --git a/planning/mission_planner/media/goal_footprints.svg b/planning/mission_planner/media/goal_footprints.svg new file mode 100644 index 0000000000000..c3347c0b52410 --- /dev/null +++ b/planning/mission_planner/media/goal_footprints.svg @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds lane boundaries +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds opposite lane +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 9e5764dfca638..f972c18b66d0d 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -31,6 +31,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index e02d055e20577..b741bd4479763 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -113,6 +114,13 @@ void DefaultPlanner::initialize(rclcpp::Node * node) map_subscriber_ = node_->create_subscription( "input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); + + const auto durable_qos = rclcpp::QoS(1).transient_local(); + pub_goal_footprint_marker_ = + node_->create_publisher("debug/goal_footprint", durable_qos); + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0); } void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) @@ -186,12 +194,110 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) return route_marker_array; } -bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const +visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( + tier4_autoware_utils::LinearRing2d goal_footprint) const +{ + visualization_msgs::msg::MarkerArray msg; + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(2.5); + + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + marker.points.push_back(marker.points.front()); + + msg.markers.push_back(marker); + + return msg; +} + +bool DefaultPlanner::check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin) +{ + std::vector points_intersection; + + // check if goal footprint is in current lane + boost::geometry::intersection( + goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + + // check if goal footprint is in between many lanelets + for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) { + next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); + lanelet::ConstLanelets lanelets; + lanelets.push_back(combined_prev_lanelet); + lanelets.push_back(next_lane); + lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + + // if next lanelet length longer than vehicle longitudinal offset + if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + boost::geometry::intersection( + goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + } else { // if next lanelet length shorter than vehicle longitudinal offset -> recursive call + if (!check_goal_footprint(next_lane, combined_lanelets, goal_footprint, next_lane_length)) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + continue; + } else { + return true; + } + } + } + return false; +} + +bool DefaultPlanner::is_goal_valid( + const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { + const auto logger = node_->get_logger(); lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; } + + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + tier4_autoware_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal)); + pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); + const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); + + double next_lane_length = 0.0; + // combine calculated route lanelets + lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + + // check if goal footprint exceeds lane when the goal isn't in parking_lot + if ( + !check_goal_footprint( + closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && + !is_in_parking_lot( + lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_), + lanelet::utils::conversion::toLaneletPoint(goal.position))) { + RCLCPP_WARN(logger, "Goal's footprint exceeds lane!"); + return false; + } + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { @@ -199,8 +305,7 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; - + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -230,12 +335,11 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } } - return false; } @@ -255,11 +359,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) LaneletRoute route_msg; RouteSections route_sections; - if (!is_goal_valid(points.back())) { - RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); - return route_msg; - } - + lanelet::ConstLanelets all_route_lanelets; for (std::size_t i = 1; i < points.size(); i++) { const auto start_check_point = points.at(i - 1); const auto goal_check_point = points.at(i); @@ -268,12 +368,20 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) start_check_point, goal_check_point, &path_lanelets)) { return route_msg; } + for (const auto & lane : path_lanelets) { + all_route_lanelets.push_back(lane); + } // create local route sections route_handler_.setRouteLanelets(path_lanelets); const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } + if (!is_goal_valid(points.back(), all_route_lanelets)) { + RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); + return route_msg; + } + if (route_handler_.isRouteLooped(route_sections)) { RCLCPP_WARN(logger, "Loop detected within route!"); return route_msg; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index e532261e73e1f..20f731953667a 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -32,6 +33,11 @@ namespace mission_planner::lanelet2 { +struct DefaultPlannerParameters +{ + double goal_angle_threshold_deg; +}; + class DefaultPlanner : public mission_planner::PlannerPlugin { public: @@ -40,6 +46,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; MarkerArray visualize(const LaneletRoute & route) const override; + MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; + vehicle_info_util::VehicleInfo vehicle_info_; private: using RouteSections = std::vector; @@ -52,11 +60,19 @@ class DefaultPlanner : public mission_planner::PlannerPlugin lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; + DefaultPlannerParameters param_; + rclcpp::Node * node_; rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void map_callback(const HADMapBin::ConstSharedPtr msg); - bool is_goal_valid(const geometry_msgs::msg::Pose & goal) const; + bool check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin = 2.0); + bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets); Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); }; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index d31d9ec3c0623..eaadd019b462c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,15 +14,32 @@ #include "utility_functions.hpp" +#include + #include #include +#include bool exists(const std::unordered_set & set, const lanelet::Id & id) { return set.find(id) != set.end(); } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint) +{ + tier4_autoware_utils::Polygon2d footprint_polygon; + boost::geometry::append(footprint_polygon.outer(), footprint[0]); + boost::geometry::append(footprint_polygon.outer(), footprint[1]); + boost::geometry::append(footprint_polygon.outer(), footprint[2]); + boost::geometry::append(footprint_polygon.outer(), footprint[3]); + boost::geometry::append(footprint_polygon.outer(), footprint[4]); + boost::geometry::append(footprint_polygon.outer(), footprint[5]); + boost::geometry::correct(footprint_polygon); + return footprint_polygon; +} + void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { cl->r = r; @@ -36,3 +53,35 @@ void insert_marker_array( { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } + +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + std::vector bound_ids; + + for (const auto & llt : lanelets) { + if (llt.id() != 0) { + bound_ids.push_back(llt.leftBound().id()); + bound_ids.push_back(llt.rightBound().id()); + } + } + + for (const auto & llt : lanelets) { + if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + } + if (std::count(bound_ids.begin(), bound_ids.end(), llt.rightBound().id()) < 2) { + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return std::move(combined_lanelet); +} diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 8217502225439..2d986ac8e56ed 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,7 +15,11 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + #include +#include +#include #include #include @@ -41,8 +45,11 @@ bool exists(const std::vector & vectors, const T & item) return false; } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint); void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index dfd9a9dec757a..8845bbb52c056 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -28,9 +28,6 @@ import numpy as np import rclpy from rclpy.node import Node -import tf2_ros -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener from tier4_planning_msgs.msg import VelocityLimit parser = argparse.ArgumentParser() @@ -62,9 +59,6 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) -PATH_ORIGIN_FRAME = "map" -SELF_POSE_FRAME = "base_link" - class TrajectoryVisualizer(Node): def __init__(self): @@ -83,6 +77,7 @@ def __init__(self): # update flag self.update_ex_vel_lim = False self.update_lat_acc_fil = False + self.update_steer_rate_fil = False self.update_traj_raw = False self.update_traj_resample = False self.update_traj_final = False @@ -91,9 +86,6 @@ def __init__(self): self.update_traj_ob_avoid = False self.update_traj_ob_stop = False - self.tf_buffer = Buffer(node=self) - self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) - self.self_pose = Pose() self.self_pose_received = False self.localization_vx = 0.0 @@ -102,6 +94,7 @@ def __init__(self): self.trajectory_external_velocity_limited = Trajectory() self.trajectory_lateral_acc_filtered = Trajectory() + self.trajectory_steer_rate_filtered = Trajectory() self.trajectory_raw = Trajectory() self.trajectory_time_resampled = Trajectory() self.trajectory_final = Trajectory() @@ -112,7 +105,7 @@ def __init__(self): self.obstacle_stop_traj = Trajectory() self.plotted = [False] * 9 - self.sub_localization_twist = self.create_subscription( + self.sub_localization_kinematics = self.create_subscription( Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1 ) self.sub_vehicle_twist = self.create_subscription( @@ -135,6 +128,9 @@ def __init__(self): self.sub4 = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_time_resampled" ) + self.sub41 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_steering_rate_limited" + ) self.sub5 = message_filters.Subscriber( self, Trajectory, "/planning/scenario_planning/trajectory" ) @@ -152,7 +148,7 @@ def __init__(self): self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") self.ts1 = message_filters.ApproximateTimeSynchronizer( - [self.sub1, self.sub2, self.sub3, self.sub4], 30, 0.5 + [self.sub1, self.sub2, self.sub3, self.sub4, self.sub41], 30, 0.5 ) self.ts1.registerCallback(self.CallbackMotionVelOptTraj) @@ -177,11 +173,10 @@ def __init__(self): return - def test(self): - self.updatePose("map", "base_link") - def CallbackLocalizationTwist(self, cmd): self.localization_vx = cmd.twist.twist.linear.x + self.self_pose = cmd.pose.pose + self.self_pose_received = True def CallbackVehicleTwist(self, cmd): self.vehicle_vx = cmd.longitudinal_velocity @@ -189,12 +184,13 @@ def CallbackVehicleTwist(self, cmd): def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity - def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4): + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): print("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) self.CallBackTrajTimeResampled(cmd4) + self.CallBackTrajSteerRateFiltered(cmd5) def CallBackTrajExVelLim(self, cmd): self.trajectory_external_velocity_limited = cmd @@ -204,6 +200,10 @@ def CallBackTrajLatAccFiltered(self, cmd): self.trajectory_lateral_acc_filtered = cmd self.update_lat_acc_fil = True + def CallBackTrajSteerRateFiltered(self, cmd): + self.trajectory_steer_rate_filtered = cmd + self.update_steer_rate_fil = True + def CallBackTrajRaw(self, cmd): self.trajectory_raw = cmd self.update_traj_raw = True @@ -252,9 +252,12 @@ def setPlotTrajectoryVelocity(self): (self.im6,) = self.ax1.plot( [], [], label="4-2: opt external_velocity_limited", marker="", ls="--" ) - (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker="", ls="--") - (self.im8,) = self.ax1.plot([], [], label="4-4: opt time_resampled", marker="", ls="--") - (self.im9,) = self.ax1.plot([], [], label="4-5: opt final", marker="", ls="-") + (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker=".", ls="--") + (self.im71,) = self.ax1.plot( + [], [], label="4-4: opt steer_rate_filtered", marker="", ls="-." + ) + (self.im8,) = self.ax1.plot([], [], label="4-5: opt time_resampled", marker="*", ls="--") + (self.im9,) = self.ax1.plot([], [], label="4-6: opt final", marker="", ls="-") (self.im10,) = self.ax1.plot( [], [], label="localization twist vx", color="r", marker="*", ls=":", markersize=10 ) @@ -277,6 +280,7 @@ def setPlotTrajectoryVelocity(self): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -285,7 +289,7 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: print("plot start but self pose is not received") return ( @@ -296,6 +300,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -312,6 +317,7 @@ def plotTrajectoryVelocity(self, data): trajectory_raw = self.trajectory_raw trajectory_external_velocity_limited = self.trajectory_external_velocity_limited trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered + trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered trajectory_time_resampled = self.trajectory_time_resampled trajectory_final = self.trajectory_final @@ -360,6 +366,12 @@ def plotTrajectoryVelocity(self, data): self.im7.set_data(x, y) self.update_lat_acc_fil = False + if self.update_steer_rate_fil: + x = self.CalcArcLength(trajectory_steer_rate_filtered) + y = self.ToVelList(trajectory_steer_rate_filtered) + self.im71.set_data(x, y) + self.update_steer_rate_fil = False + if self.update_traj_resample: x = self.CalcArcLength(trajectory_time_resampled) y = self.ToVelList(trajectory_time_resampled) @@ -391,6 +403,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -577,7 +590,7 @@ def setPlotTrajectory(self): def plotTrajectory(self, data): print("plot called") - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) # copy trajectory_raw = self.trajectory_raw @@ -668,25 +681,6 @@ def calcSquaredDist2d(self, p1, p2): dy = p1.position.y - p2.position.y return dx * dx + dy * dy - def updatePose(self, from_link, to_link): - try: - tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) - self.self_pose.position.x = tf.transform.translation.x - self.self_pose.position.y = tf.transform.translation.y - self.self_pose.position.z = tf.transform.translation.z - self.self_pose.orientation.x = tf.transform.rotation.x - self.self_pose.orientation.y = tf.transform.rotation.y - self.self_pose.orientation.z = tf.transform.rotation.z - self.self_pose.orientation.w = tf.transform.rotation.w - print("updatePose succeeded") - self.self_pose_received = True - return - except tf2_ros.TransformException: - self.get_logger().warn( - "lookup transform failed: from {} to {}".format(from_link, to_link) - ) - return - def closeFigure(self): plt.close(self.fig) diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 12b6eef559fe4..e0656a15b82f3 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -34,4 +34,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE launch test) +ament_auto_package(INSTALL_TO_SHARE config launch test) diff --git a/system/default_ad_api/README.md b/system/default_ad_api/README.md new file mode 100644 index 0000000000000..0b6cdf30778a2 --- /dev/null +++ b/system/default_ad_api/README.md @@ -0,0 +1,11 @@ +# default_ad_api + +This package is a default implementation AD API. + +- [autoware state (backward compatibility)](document/autoware-state.md) +- [fail-safe](document/fail-safe.md) +- [interface](document/interface.md) +- [localization](document/localization.md) +- [motion](document/motion.md) +- [operation mode](document/operation-mode.md) +- [routing](document/routing.md) diff --git a/system/default_ad_api/config/default_ad_api.param.yaml b/system/default_ad_api/config/default_ad_api.param.yaml new file mode 100644 index 0000000000000..a15abe091764c --- /dev/null +++ b/system/default_ad_api/config/default_ad_api.param.yaml @@ -0,0 +1,8 @@ +/default_ad_api/node/autoware_state: + ros__parameters: + update_rate: 10.0 + +/default_ad_api/node/motion: + ros__parameters: + require_accept_start: false + stop_check_duration: 1.0 diff --git a/system/default_ad_api/document/autoware-state.md b/system/default_ad_api/document/autoware-state.md new file mode 100644 index 0000000000000..e26756de1f4ba --- /dev/null +++ b/system/default_ad_api/document/autoware-state.md @@ -0,0 +1,16 @@ +# Autoware state compatibility + +## Overview + +Since `/autoware/state` was so widely used, default_ad_api creates it from the states of AD API for backwards compatibility. +The diagnostic checks that ad_service_state_monitor used to perform have been replaced by component_state_monitor. +The service `/autoware/shutdown` to change autoware state to finalizing is also supported for compatibility. + +![autoware-state-architecture](images/autoware-state-architecture.drawio.svg) + +## Conversion + +This is the correspondence between AD API states and autoware states. +The launch state is the data that default_ad_api node holds internally. + +![autoware-state-table](images/autoware-state-table.drawio.svg) diff --git a/system/default_ad_api/document/fail-safe.md b/system/default_ad_api/document/fail-safe.md new file mode 100644 index 0000000000000..b9967089c5e3a --- /dev/null +++ b/system/default_ad_api/document/fail-safe.md @@ -0,0 +1,5 @@ +# Fail-safe API + +## Overview + +The fail-safe API simply relays the MRM state. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/fail_safe/) for AD API specifications. diff --git a/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg new file mode 100644 index 0000000000000..40aebd20ae43f --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg @@ -0,0 +1,332 @@ + + + + + + + + + + + +
+
+
+ component state monitor +
+
+
+ + component state monitor + + + + + + + + +
+
+
+ localization +
+ initialization state +
+
+
+ + localization... + + + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + + + +
+
+
+ topic check status +
+ for startup +
+
+
+ + topic check status... + + + + + + + + +
+
+
+ topic check status +
+ for autonomous +
+
+
+ + topic check status... + + + + + + + + + + + + +
+
+
+ default_ad_api +
+ (localization, routing, operation mode) +
+
+
+ + default_ad_api... + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + + +
+
+
+ topic state monitor +
+
+
+ + topic state monitor + + + + + + + + +
+
+
+ default_ad_api +
+ (autoware state) +
+
+
+ + default_ad_api... + + + + + + +
+
+
+ autoware +
+ state +
+
+
+ + autoware... + + + + + + + + +
+
+
+ autoware +
+ shutdown +
+
+
+ + autoware... + + + + + + + + +
+
+
+ topics that ad_service_state_monitor was checking before +
+
+
+ + topics that ad_service_state_monitor was checking before + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg new file mode 100644 index 0000000000000..ab21c1b865407 --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -0,0 +1,457 @@ + + + + + + + + +
+
+
+ WaitingForRoute +
+
+
+ + WaitingForRoute + + + + + + +
+
+
+ localization state +
+
+
+ + localization state + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + +
+
+
+ auto mode available +
+
+
+ + auto mode available + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ running +
+
+
+ + running + + + + + + +
+
+
+ uninitialized +
+
+
+ + uninitialized + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ initialized +
+
+
+ + initialized + + + + + + +
+
+
+ unset, arrived +
+
+
+ + unset, arrived + + + + + + +
+
+
+ set +
+
+
+ + set + + + + + + +
+
+
+ arrived +
+
+
+ + arrived + + + + + + +
+
+
+ finalizing +
+
+
+ + finalizing + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ not stop +
+
+
+ + not stop + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ Initializing +
+
+
+ + Initializing + + + + + + +
+
+
+ launch state +
+
+
+ + launch state + + + + + + +
+
+
+ Planning +
+
+
+ + Planning + + + + + + +
+
+
+ WaitingForEngage +
+
+
+ + WaitingForEngage + + + + + + +
+
+
+ Driving +
+
+
+ + Driving + + + + + + +
+
+
+ ArrivedGoal +
+
+
+ + ArrivedGoal + + + + + + +
+
+
+ Finalizing +
+
+
+ + Finalizing + + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/localization.drawio.svg b/system/default_ad_api/document/images/localization.drawio.svg new file mode 100644 index 0000000000000..3c1347e8b4a3c --- /dev/null +++ b/system/default_ad_api/document/images/localization.drawio.svg @@ -0,0 +1,553 @@ + + + + + + + + + + + + + +
+
+
+ Applications (FMS, etc.) +
+
+
+ + Applications (FMS, etc.) + + + + + + + + +
+
+
+ /api/localization/initialize +
+
+
+ + /api/localization/initialize + + + + + + + + +
+
+
+ AD API +
+ (default implementation) +
+
+
+ + AD API... + + + + + + + + + + +
+
+
+ /localization/initialize +
+
+
+ + /localization/initialize + + + + + + +
+
+
+ component_state_monitor +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ initialpose3d +
+
+
+ + initialpose3d + + + + + + + + + + + +
+
+
+ simple_planning_simulator +
+
+
+ + simple_planning_simulator + + + + + + +
+
+
+ NDT +
+
+
+ + NDT + + + + + + + + + +
+
+
+ ndt_align_srv +
+
+
+ + ndt_align_srv + + + + + + +
+
+
+ common +
+
+
+ + common + + + + + + +
+
+
+ psim +
+
+
+ + psim + + + + + + +
+
+
+ lsim/real +
+
+
+ + lsim/real + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + + + + + + + +
+
+
+ + initial_pose_adaptor +
+ (Fix z position to fit map) + +
+
+
+ + initial_pose_adaptor... + + + + + + + + +
+
+
+ EKF +
+
+
+ + EKF + + + + + + + + +
+
+
+ gnss pose +
+
+
+ + gnss pose + + + + + + +
+
+
+ Module when using GNSS +
+ (Fix z position to fit map) +
+
+
+ + Module when using GNSS... + + + + + + + + +
+
+
+ automatic_pose_initializer +
+ (Call the API when state is uninitialized) +
+
+
+ + automatic_pose_initializer... + + + + + + + + +
+
+
+ initialpose (from rviz) +
+
+
+ + initialpose (from rviz) + + + + + + + + + + + + +
+
+
+ fit_map_height +
+
+
+ + fit_map_height + + + + + + +
+
+
+ map_height_fitter +
+
+
+ + map_height_fitter + + + + + + +
+
+
+ Module when using NDT +
+
+
+ + Module when using NDT + + + + + + + + +
+
+
+ /localization/initialization_state +
+
+
+ + /localization/initialization_state + + + + + + + + +
+
+
+ sensing twist +
+
+
+ + sensing twist + + + + + + +
+
+
+ pose_initializer +
+
+
+ + pose_initializer + + + + + + +
+
+
+ Module to check vehicle stops +
+
+
+ + Module to check vehicle stops + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/motion-architecture.drawio.svg b/system/default_ad_api/document/images/motion-architecture.drawio.svg new file mode 100644 index 0000000000000..24271b1c65643 --- /dev/null +++ b/system/default_ad_api/document/images/motion-architecture.drawio.svg @@ -0,0 +1,255 @@ + + + + + + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + + + +
+
+
+ Applications +
+
+
+ + Applications + + + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + +
+
+
+ /localization/kinematic_state +
+
+
+ + /localization/kinematic_state + + + + + + + + +
+
+
+ is_paused +
+
+
+ + is_paused + + + + + + + + +
+
+
+ is_start_requested +
+
+
+ + is_start_requested + + + + + + + + +
+
+
+ set_pause +
+
+
+ + set_pause + + + + + + + + +
+
+
+ localization +
+
+
+ + localization + + + + + + + + +
+
+
+ accept_start +
+
+
+ + accept_start + + + + + + + + +
+
+
+ motion_state +
+
+
+ + motion_state + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/motion.drawio.svg b/system/default_ad_api/document/images/motion-state.drawio.svg similarity index 74% rename from system/default_ad_api/document/motion.drawio.svg rename to system/default_ad_api/document/images/motion-state.drawio.svg index c6e2593ed0eb7..3387b8ed29bb7 100644 --- a/system/default_ad_api/document/motion.drawio.svg +++ b/system/default_ad_api/document/images/motion-state.drawio.svg @@ -3,13 +3,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="342px" + width="521px" height="282px" - viewBox="-0.5 -0.5 342 282" - content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5VpNc5swEP01vmZAAhkf2zRpL5nJ1IeeZZCBqUCMkL/66ytAfIo4tgE7iXPIoGUl0Nu3T9LiGXyM9j85ToIX5hE6A4a3n8EfMwDmAMr/meFQGCwTFAafh15hMmvDMvxHlNFQ1k3okbTlKBijIkzaRpfFMXFFy4Y5Z7u225rR9lMT7BPNsHQx1a1/Qk8EymqiRX3jFwn9QD3aAfPiRoRLZzWTNMAe2zVM8GkGHzljoriK9o+EZtiVuBT9nt+4W70YJ7E4pQNUPVJxKCdHPDlX1WRcBMxnMaZPtfU7Z5vYI9kIhmwFIqLy0pSXqcBcfMsAlgaX4jQN3dL8HNLSrXhk9pw337p8LbbhrvJSFJFj+UR5wQoxyTTCIiL4QbpwQrEIt+3RsYq5X/nVsMgLhcwbKNmfGiVrIEx5V/nC+NBwSFgYi7Qx8mtmkA4qyYGhKK5yfNEh4lnu8qJ4fh2uaiInRVDBssV0o+b7ijepjE43rnXUshjsglCQZYJzdHdSy9qRXMtoPTLKeN4Xrh2XuEUoOftLGndWjm3ZRhXULeGC7I+HVY9X2QG1kTJLLdnVUuQoU9AQIcsYngjAOZoIMYvJEa6r202iGxcTHepEH8rzk+VAI9Nvkm6iMPZHpZNnE8ez+ujkgBVEaBw6QXBDOi0u0NUz2dWW4cu4ZulcQ7fQVNhJfYCOi+o7/oNVFcLJ1sVRtaInfvaVtMLq14qRV57bSEW1Zl9DKtCHWXlsnU3gSmyyNTZl25ixF57b7GOuySZoaIB9yIUHjS9co2zmwfy83XzHf/DCg7Q8eGHbz7n/0tLA0dOg4vzYeTDXcNTzIvY6RPdwGlSQNuA7leI6FI2p2j0zLW3ncVwjZRdoe/GA7EX9N28PWOShGuMI160u11EnMEX2agNdQHtHC1cqWCItecMQXJq78ZPMFF3xatJZBbXJfWXCNPRj2aRknY2QsTx0Mf2mzFHoebku9mVVn/gNSpNu8KzTFgswQpIsjqO+xjS9G9jR9WAvi713KU7mXIoTGkeQTGcyQTJNLUZJVle7R0kyr5kb4B3c70mUzJ4d01TAw+PH0A9T8YA9JY+hX0tGKVmZxnklq47/8JKVXpBZZoiPfoZer0H/GdpDK2RPdHjo2RVNdnaA+r4Iuy5JxASy40o8CL+58MD+j1RNuNFEwmPpHx10Jfqqu6HF4sGeDz+pVatCR1sm2BhZusy4kZfzj1aLtF6E+qIL9ImVvVHyRC+R5sAjHGVTjVdpksOTR6G0+iKf990E5IobJuuuSkxdhdFqQbeXKtmsf15UuNe/0YJP/wE=</diagram></mxfile>" + viewBox="-0.5 -0.5 521 282" + content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5Vpdc6IwFP01vnaAQMRHa223D906a2f3OUIEZoE4IWq7v36DhM+gxRq0rT44cEkgOffck5sLAzCJXh8oWvlPxMXhwNDc1wG4GxjGSDf4f2p4ywwW1DKDRwM3M+mlYR78w8KYN1sHLk5qDRkhIQtWdaND4hg7rGZDlJJtvdmShPWnrpCHJcPcQaFs/RO4zBdWHY7KCz9w4Pni0bYxzC5EKG8sZpL4yCXbiglMB2BCCWHZUfQ6wWGKXY5L1u9+z9ViYBTHrEsHU4x4g8K1mNycIYbTJjH/G9+lf7NHMVr2lkNAyTp2cXoXbQBut37A8HyFnPTqlvuc23wWhfxM54dLEjPhRe56cLvBlAUcznEYeDE3MpJ2ECPh1/Dr3unoBUicXJhEmNE33kR0ME2BqyCWLU63FS/l0PsVBxmaMCLBDK+4dQkePxD4tWMJdAkl7HLeiFNCmU88EqNwWlpv6zhWMEsYomyckpUbnBAlSeDk5vsgzJvtQYiPgqypI4Yhwo139bBoBTJTOsCDwFIcIhZs6sQ/CSXrK6FkqoZp15UPGL1VGqxIELOkcudZaihprdt1Wo8aQX1Uc36QPb90VzGRTh40JMmYoXXCvbNfI/QOGsG9NSEhobu+YGk72MlcSclfXLmysC3T0g45VZKQvXqhN/SiEIeKYNgtemEqkAvDPhgIMYnxAa6Ly1WiH8SkSnQgE105zzvLgUSmXzhZR0HsKaWTa2HbNdvoZBsLAKEaOgHtgnQafUBXj2RXXYY7cc2UuQY/haaCRugb8LCovtP+ZFUFoLd18RStaPGfdSmtMNu1QvHKcxmpKDLQc0gFvNTKY8lsMi7FJktiU5rGqF54LpPHnJNNQJMA+wwLDzyDcClJ5o3hcdl8o/3JCw+U4uCJbL5m/iWFQcv+v+C86jgYSjjKcRG7DaK7KPELSCvw7QXjXfJWpmq1zDS3HcdxiZRNoK3RDbRG5W9Yv2EWmOIeh5KsJtdhwzFZOEs3+gDtbcldSVqG4hl7eqIxys1N/3EasqZ4VeksnFrlvjAhUekK8ZK1FMCiwHV3utgWVR3Fr3uYNJ1ndlssDAVBIpcba6gvUZhcDezwfLDnhfOrFCd9yMUJqhGkYjFWL0i6LvloldbVrlGS9HPGhvEO7tckSnpLxtQX8ODwNvRSFQ/QUvJQ/rZESclK144rWTXan16ykgsy8xRx5Xvo5dJo30O7cAGtnjYPLVlRb3sHIOdFyHHwivUgOw6HCNOLC4/R/pKqCjfsSXhM+aWDrETfNRsajW6s4ek7tcI5DW3pITEyZZlxIndHtrBYpOUi1DddoDtW9pTEiVwi3QEPUZRONV4kqx08Oy/kVo/t5n01DjljwmReVYmpqTBSLegzSpVcVZq/jH+9PP58kDx1lozotK+prAZww5bPqbSeUqL82dW69PNv1UB2rksrBbJ4Z3IWIOW6wvzleTab3qmlZNcXXWopqfeHJD8tP8TMxKD8mhVM/wM=</diagram></mxfile>" > + + + + +
+
+
+ State in AD API +
+
+
+ + State in AD API + + @@ -265,6 +283,57 @@ + + + + +
+
+
+ STARTING +
+
+
+ + STARTING + + + + + + +
+
+
+ MOVING +
+
+
+ + MOVING + + + + + + +
+
+
+ STOPPED +
+
+
+ + STOPPED + + diff --git a/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg new file mode 100644 index 0000000000000..4a879e98ed09d --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg @@ -0,0 +1,323 @@ + + + + + + + + + +
+
+
+ external_cmd_selector +
+
+
+ + external_cmd_selector + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + + + + + +
+
+
+ vehicle_interface +
+
+
+ + vehicle_interface + + + + + + + + + + + + + + +
+
+
+ + operation mode +
+ transition manager + +
+
+
+ + operation mode... + + + + + + +
+
+
+ engage, gate_mode +
+
+
+ + engage, gate_mode + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + +
+
+
+ control command +
+ (joystick etc.) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (remote) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (autonomous) +
+
+
+ + control command... + + + + + + + + +
+
+
+ + component_state_monitor +
+ +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-state.drawio.svg b/system/default_ad_api/document/images/operation-mode-state.drawio.svg new file mode 100644 index 0000000000000..3a008fd8a64f3 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-state.drawio.svg @@ -0,0 +1,418 @@ + + + + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is false) +
+
+
+ + stop... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is false) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is true) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is true) +
+
+
+ + stop... + + + + + + + + + + + + +
+
+
+ auto +
+ (in transition) +
+
+
+ + auto... + + + + + + + + + + + + +
+
+
+ stop +
+ (in transition) +
+
+
+ + stop... + + + + + + + + +
+
+
+ disable autoware control +
+
+
+ + disable autoware control + + + + + + + + +
+
+
+ enable autoware control +
+
+
+ + enable autoware control + + + + + + + + +
+
+
+ change operation mode +
+
+
+ + change operation mode + + + + + + + + +
+
+
+ mode change completed +
+
+
+ + mode change completed + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is false) +
+
+
+ + remote... + + + + + + + + +
+
+
+ local +
+ (autoware control is false) +
+
+
+ + local... + + + + + + + + + + + + +
+
+
+ remote +
+ (in transition) +
+
+
+ + remote... + + + + + + + + + + + + +
+
+
+ local +
+ (in transition) +
+
+
+ + local... + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is true) +
+
+
+ + remote... + + + + + + + + + + +
+
+
+ local +
+ (autoware control is true) +
+
+
+ + local... + + + + + + + +
+
+
+ mode change failed +
+
+
+ + mode change failed + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-table.drawio.svg b/system/default_ad_api/document/images/operation-mode-table.drawio.svg new file mode 100644 index 0000000000000..19a3ea5bd1133 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-table.drawio.svg @@ -0,0 +1,317 @@ + + + + + + + +
+
+
+ driver +
+
+
+ + driver + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ manual +
+
+
+ + manual + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ gate_mode +
+
+
+ + gate_mode + + + + + + + +
+
+
+ auto +
+
+
+ + auto + + + + + + +
+
+
+ external +
+
+
+ + external + + + + + + +
+
+
+ engage +
+
+
+ + engage + + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/routing.drawio.svg b/system/default_ad_api/document/images/routing.drawio.svg new file mode 100644 index 0000000000000..6742bfe4c12b0 --- /dev/null +++ b/system/default_ad_api/document/images/routing.drawio.svg @@ -0,0 +1,398 @@ + + + + + + + +
+
+
+ mission_planner +
+
+
+ + mission_planner + + + + + + + + +
+
+
+ planner plugin +
+
+
+ + planner plugin + + + + + + + + + + +
+
+
+ routing_adaptor +
+
+
+ + routing_adaptor + + + + + + +
+
+
+ set route +
+
+
+ + set route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ pose topic +
+
+
+ + pose topic + + + + + + +
+
+
+ planning modules +
+
+
+ + planning modules + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + + + + + + + +
+
+
+ App +
+
+
+ + App + + + + + + +
+
+
+ RViz +
+
+
+ + RViz + + + + + + + + + + + + +
+
+
+ AD +
+ API +
+
+
+ + AD... + + + + + + + + + + + + + + +
+
+
+ main module +
+
+
+ + main module + + + + + + +
+
+
+ set lanelet route +
+
+
+ + set lanelet route + + + + + + +
+
+
+ + route, route state + +
+
+
+ + route, route state + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ route state +
+
+
+ + route state + + + + + + +
+
+
+ + clear route + +
+
+
+ + clear route + + + + + + +
+
+
+ pose array +
+
+
+ + pose array + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/interface.md b/system/default_ad_api/document/interface.md new file mode 100644 index 0000000000000..f3fc6a389c294 --- /dev/null +++ b/system/default_ad_api/document/interface.md @@ -0,0 +1,5 @@ +# Interface API + +## Overview + +The interface API simply returns a version number. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/interface/) for AD API specifications. diff --git a/system/default_ad_api/document/localization.md b/system/default_ad_api/document/localization.md new file mode 100644 index 0000000000000..866322ed807cf --- /dev/null +++ b/system/default_ad_api/document/localization.md @@ -0,0 +1,7 @@ +# Localization API + +## Overview + +Unify the location initialization method to the service. The topic `/initialpose` from rviz is now only subscribed to by adapter node and converted to API call. This API call is forwarded to the pose initializer node so it can centralize the state of pose initialization. For other nodes that require initialpose, pose initializer node publishes as `/initialpose3d`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/) for AD API specifications. + +![localization-architecture](images/localization.drawio.svg) diff --git a/system/default_ad_api/document/motion.md b/system/default_ad_api/document/motion.md new file mode 100644 index 0000000000000..fd01a8f56ed7f --- /dev/null +++ b/system/default_ad_api/document/motion.md @@ -0,0 +1,13 @@ +# Motion API + +## Overview + +Provides a hook for when the vehicle starts. It is typically used for announcements that call attention to the surroundings. Add a pause function to the vehicle_cmd_gate, and API will control it based on vehicle stopped and start requested. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/motion/) for AD API specifications. + +![motion-architecture](images/motion-architecture.drawio.svg) + +## States + +The implementation has more detailed state transitions to manage pause state synchronization. The correspondence with the AD API state is as follows. + +![motion-state](images/motion-state.drawio.svg) diff --git a/system/default_ad_api/document/operation-mode.md b/system/default_ad_api/document/operation-mode.md new file mode 100644 index 0000000000000..703f6aa47b50d --- /dev/null +++ b/system/default_ad_api/document/operation-mode.md @@ -0,0 +1,24 @@ +# Operation mode API + +## Overview + +Introduce operation mode. It handles autoware engage, gate_mode, external_cmd_selector and control_mode abstractly. When the mode is changed, it will be in-transition state, and if the transition completion condition to that mode is not satisfied, it will be returned to the previous mode. Also, currently, the condition for mode change is only `WaitingForEngage` in `/autoware/state`, and the engage state is shared between modes. After introducing the operation mode, each mode will have a transition available flag. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/operation_mode/) for AD API specifications. + +![operation-mode-architecture](images/operation-mode-architecture.drawio.svg) + +## States + +The operation mode has the following state transitions. Disabling autoware control and changing operation mode when autoware control is disabled can be done immediately. +Otherwise, enabling autoware control and changing operation mode when autoware control is enabled causes the state will be transition state. +If the mode change completion condition is not satisfied within the timeout in the transition state, it will return to the previous mode. + +![operation-mode-state](images/operation-mode-state.drawio.svg) + +## Compatibility + +Ideally, vehicle_cmd_gate and external_cmd_selector should be merged so that the operation mode can be handled directly. +However, currently the operation mode transition manager performs the following conversions to match the implementation. +The transition manager monitors each topic in the previous interface and synchronizes the operation mode when it changes. +When the operation mode is changed with the new interface, the transition manager disables synchronization and changes the operation mode using the previous interface. + +![operation-mode-table](images/operation-mode-table.drawio.svg) diff --git a/system/default_ad_api/document/routing.md b/system/default_ad_api/document/routing.md new file mode 100644 index 0000000000000..899136f2d9e50 --- /dev/null +++ b/system/default_ad_api/document/routing.md @@ -0,0 +1,7 @@ +# Routing API + +## Overview + +Unify the route setting method to the service. This API supports two waypoint formats, poses and lanelet segments. The goal and checkpoint topics from rviz is only subscribed to by adapter node and converted to API call. This API call is forwarded to the mission planner node so it can centralize the state of routing. For other nodes that require route, mission planner node publishes as `/planning/mission_planning/route`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/) for AD API specifications. + +![routing-architecture](images/routing.drawio.svg) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index eab13c67ef42c..d3182a9070c2a 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -13,9 +13,14 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare def create_api_node(node_name, class_name, **kwargs): @@ -24,17 +29,23 @@ def create_api_node(node_name, class_name, **kwargs): name=node_name, package="default_ad_api", plugin="default_ad_api::" + class_name, - **kwargs, + parameters=[ParameterFile(LaunchConfiguration("config"))], ) +def get_default_config(): + path = FindPackageShare("default_ad_api") + path = PathJoinSubstitution([path, "config/default_ad_api.param.yaml"]) + return path + + def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), - create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), + create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), create_api_node("routing", "RoutingNode"), ] @@ -51,4 +62,5 @@ def generate_launch_description(): name="web_server", executable="web_server.py", ) - return launch.LaunchDescription([container, web_server]) + argument = DeclareLaunchArgument("config", default_value=get_default_config()) + return launch.LaunchDescription([argument, container, web_server]) diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 5778edf66559f..1aa49383aa48f 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -46,8 +46,7 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); - // TODO(Takagi, Isamu): remove default value - const auto rate = rclcpp::Rate(declare_parameter("update_rate", 10.0)); + const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); component_states_.resize(module_names.size()); diff --git a/system/default_ad_api/src/motion.cpp b/system/default_ad_api/src/motion.cpp index d99f72f437685..b02fd43e9be44 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/default_ad_api/src/motion.cpp @@ -23,8 +23,8 @@ namespace default_ad_api MotionNode::MotionNode(const rclcpp::NodeOptions & options) : Node("motion", options), vehicle_stop_checker_(this) { - stop_check_duration_ = declare_parameter("stop_check_duration", 1.0); - require_accept_start_ = declare_parameter("require_accept_start", false); + stop_check_duration_ = declare_parameter("stop_check_duration"); + require_accept_start_ = declare_parameter("require_accept_start"); is_calling_set_pause_ = false; const auto adaptor = component_interface_utils::NodeAdaptor(this);