diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 83c814b70f0a5..ac8e472b2f943 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -354,6 +354,7 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getClosestLanelets(const geometry_msgs::msg::Pose & target_pose) const; private: // MUST diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 8d143fcf0b87d..76763821998bd 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -2286,4 +2286,11 @@ bool RouteHandler::findDrivableLanePath( return drivable_lane_path_found; } +lanelet::ConstLanelets RouteHandler::getClosestLanelets( + const geometry_msgs::msg::Pose & target_pose) const +{ + lanelet::ConstLanelets target_lanelets; + lanelet::utils::query::getCurrentLanelets(road_lanelets_, target_pose, &target_lanelets); + return target_lanelets; +} } // namespace route_handler diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 829c06c1fba12..cc33f2234a50c 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -22,6 +22,8 @@ autoware_package() ament_auto_add_executable(main src/main.cpp src/static_centerline_optimizer_node.cpp + src/centerline_source/optimization_trajectory_based_centerline.cpp + src/centerline_source/bag_ego_trajectory_based_centerline.cpp src/utils.cpp ) diff --git a/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml b/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml index 4fdee8a0ca6fc..24a5536949479 100644 --- a/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml +++ b/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: - centerline_source: "optimization_trajectory_base" # select from "optimization_trajectory_base" and "bag_ego_trajectory_base" marker_color: ["FF0000", "00FF00", "0000FF"] marker_color_dist_thresh : [0.1, 0.2, 0.3] + output_trajectory_interval: 1.0 diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp new file mode 100644 index 0000000000000..db954b6ccbd47 --- /dev/null +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -0,0 +1,29 @@ +// Copyright 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 STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_optimizer/type_alias.hpp" + +#include +#include +#include + +namespace static_centerline_optimizer +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node); +} // namespace static_centerline_optimizer +#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp new file mode 100644 index 0000000000000..6e52ff98d78da --- /dev/null +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp @@ -0,0 +1,45 @@ +// Copyright 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 STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT + +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_optimizer/type_alias.hpp" + +#include +#include +#include + +namespace static_centerline_optimizer +{ +class OptimizationTrajectoryBasedCenterline +{ +public: + OptimizationTrajectoryBasedCenterline() = default; + explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); + std::vector generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids); + +private: + std::vector optimize_trajectory(const Path & raw_path) const; + + rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; + rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; +}; +} // namespace static_centerline_optimizer +// clang-format off +#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +// clang-format on diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index a920139a94472..242a76ae0d94f 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp @@ -16,6 +16,7 @@ #define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" +#include "static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp" #include "static_centerline_optimizer/srv/load_map.hpp" #include "static_centerline_optimizer/srv/plan_path.hpp" #include "static_centerline_optimizer/srv/plan_route.hpp" @@ -25,6 +26,7 @@ #include #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" #include @@ -37,6 +39,12 @@ using static_centerline_optimizer::srv::LoadMap; using static_centerline_optimizer::srv::PlanPath; using static_centerline_optimizer::srv::PlanRoute; +struct CenterlineWithRoute +{ + std::vector centerline{}; + std::vector route_lane_ids{}; +}; + class StaticCenterlineOptimizerNode : public rclcpp::Node { public: @@ -44,11 +52,6 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node void run(); private: - struct CenterlineWithRoute - { - std::vector centerline{}; - std::vector route_lane_ids{}; - }; // load map void load_map(const std::string & lanelet2_input_file_path); void on_load_map( @@ -60,13 +63,12 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node void on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); - // plan path + // plan centerline CenterlineWithRoute generate_centerline_with_route(); - std::vector plan_path(const std::vector & route_lane_ids); - std::vector optimize_trajectory(const Path & raw_path) const; void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); + void update_centerline_range(const int traj_start_index, const int traj_end_index); void evaluate( const std::vector & route_lane_ids, const std::vector & optimized_traj_points); @@ -88,19 +90,19 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node BagEgoTrajectoryBase, }; CenterlineSource centerline_source_; + OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; // publisher rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; - rclcpp::Publisher::SharedPtr pub_whole_optimized_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_optimized_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; // subscriber rclcpp::Subscription::SharedPtr sub_traj_start_index_; rclcpp::Subscription::SharedPtr sub_traj_end_index_; rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; // service rclcpp::Service::SharedPtr srv_load_map_; diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp index 8c8c346557fce..52dcd171e8658 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp @@ -28,6 +28,9 @@ namespace static_centerline_optimizer { namespace utils { +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids); + geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id); diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index 2e163e2eb93eb..10718150d1751 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -5,6 +5,9 @@ + + + @@ -78,6 +81,8 @@ + + diff --git a/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh new file mode 100755 index 0000000000000..8164b521553c8 --- /dev/null +++ b/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix static_centerline_optimizer --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus diff --git a/planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh new file mode 100755 index 0000000000000..3967c7a4ca523 --- /dev/null +++ b/planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/static_centerline_optimizer/scripts/tmp/run.sh b/planning/static_centerline_optimizer/scripts/tmp/run.sh deleted file mode 100755 index 9e475b9d8759b..0000000000000 --- a/planning/static_centerline_optimizer/scripts/tmp/run.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp new file mode 100644 index 0000000000000..150218b498ae7 --- /dev/null +++ b/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -0,0 +1,82 @@ +// Copyright 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 "static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp" + +#include "rclcpp/serialization.hpp" +#include "rosbag2_cpp/reader.hpp" +#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" + +#include + +namespace static_centerline_optimizer +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node) +{ + const auto bag_filename = node.declare_parameter("bag_filename"); + + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + + rclcpp::Serialization bag_serialization; + std::vector centerline_traj_points; + while (bag_reader.has_next()) { + const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); + + if (msg->topic_name != "/localization/kinematic_state") { + continue; + } + + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + const auto ros_msg = std::make_shared(); + + bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); + + if (!centerline_traj_points.empty()) { + constexpr double epsilon = 1e-1; + if ( + std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < + epsilon && + std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < + epsilon) { + continue; + } + } + TrajectoryPoint centerline_traj_point; + centerline_traj_point.pose.position = ros_msg->pose.pose.position; + // std::cerr << centerline_traj_point.pose.position.x << " " + // << centerline_traj_point.pose.position.y << std::endl; + centerline_traj_points.push_back(centerline_traj_point); + } + + RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); + + // calculate rough orientation of centerline trajectory points + for (size_t i = 0; i < centerline_traj_points.size(); ++i) { + if (i == centerline_traj_points.size() - 1) { + if (i != 0) { + centerline_traj_points.at(i).pose.orientation = + centerline_traj_points.at(i - 1).pose.orientation; + } + } else { + const double yaw_angle = tier4_autoware_utils::calcAzimuthAngle( + centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); + centerline_traj_points.at(i).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw_angle); + } + } + + return centerline_traj_points; +} +} // namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp new file mode 100644 index 0000000000000..5bce91be722ca --- /dev/null +++ b/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -0,0 +1,192 @@ +// Copyright 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 "static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp" + +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/conversion.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" +#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_optimizer/utils.hpp" + +namespace static_centerline_optimizer +{ +namespace +{ +rclcpp::NodeOptions create_node_options() +{ + return rclcpp::NodeOptions{}; +} + +rclcpp::QoS create_transient_local_qos() +{ + return rclcpp::QoS{1}.transient_local(); +} +Path convert_to_path(const PathWithLaneId & path_with_lane_id) +{ + Path path; + path.header = path_with_lane_id.header; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; + for (const auto & point : path_with_lane_id.points) { + path.points.push_back(point.point); + } + + return path; +} + +Trajectory convert_to_trajectory(const Path & path) +{ + Trajectory traj; + for (const auto & point : path.points) { + TrajectoryPoint traj_point; + traj_point.pose = point.pose; + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + + traj.points.push_back(traj_point); + } + return traj; +} +} // namespace + +OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) +{ + pub_raw_path_with_lane_id_ = + node.create_publisher("input_centerline", create_transient_local_qos()); + pub_raw_path_ = node.create_publisher("debug/raw_centerline", create_transient_local_qos()); +} + +std::vector +OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids) +{ + const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); + + // optimize centerline inside the lane + const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); + + // get ego nearest search parameters and resample interval in behavior_path_planner + const double ego_nearest_dist_threshold = + node.has_parameter("ego_nearest_dist_threshold") + ? node.get_parameter("ego_nearest_dist_threshold").as_double() + : node.declare_parameter("ego_nearest_dist_threshold"); + const double ego_nearest_yaw_threshold = + node.has_parameter("ego_nearest_yaw_threshold") + ? node.get_parameter("ego_nearest_yaw_threshold").as_double() + : node.declare_parameter("ego_nearest_yaw_threshold"); + const double behavior_path_interval = node.has_parameter("output_path_interval") + ? node.get_parameter("output_path_interval").as_double() + : node.declare_parameter("output_path_interval"); + const double behavior_vel_interval = + node.has_parameter("behavior_output_path_interval") + ? node.get_parameter("behavior_output_path_interval").as_double() + : node.declare_parameter("behavior_output_path_interval"); + + // extract path with lane id from lanelets + const auto raw_path_with_lane_id = [&]() { + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); + pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); + RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); + + // convert path with lane id to path + const auto raw_path = [&]() { + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + }(); + pub_raw_path_->publish(raw_path); + RCLCPP_INFO(node.get_logger(), "Converted to path and published."); + + // smooth trajectory and road collision avoidance + const auto optimized_traj_points = optimize_trajectory(raw_path); + RCLCPP_INFO( + node.get_logger(), + "Smoothed trajectory and made it collision free with the road and published."); + + return optimized_traj_points; +} + +std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( + const Path & raw_path) const +{ + // convert to trajectory points + const auto raw_traj_points = [&]() { + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); + + // create an instance of elastic band and model predictive trajectory. + const auto eb_path_smoother_ptr = + path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + const auto mpt_optimizer_ptr = + obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + + // NOTE: The optimization is executed every valid_optimized_traj_points_num points. + constexpr int valid_optimized_traj_points_num = 10; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + + // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use + // warm start. + constexpr int num_initial_optimization = 2; + + std::vector whole_optimized_traj_points; + for (int virtual_ego_pose_idx = -num_initial_optimization; + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + // calculate virtual ego pose for the optimization + constexpr int virtual_ego_pose_offset_idx = 1; + const auto virtual_ego_pose = + raw_traj_points + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; + + // smooth trajectory by elastic band in the path_smoother package + const auto smoothed_traj_points = + eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); + + // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // package + const obstacle_avoidance_planner::PlannerData planner_data{ + raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, + virtual_ego_pose}; + const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); + + // connect the previously and currently optimized trajectory points + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + break; + } + } + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + return whole_optimized_traj_points; +} +} // namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 0feebe8cbceb6..4f729ab5c8c57 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -21,8 +21,7 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" -#include "path_smoother/elastic_band_smoother.hpp" +#include "static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" @@ -35,6 +34,7 @@ #include #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" #include @@ -57,34 +57,6 @@ namespace static_centerline_optimizer { namespace { -Path convert_to_path(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.left_bound = path_with_lane_id.left_bound; - path.right_bound = path_with_lane_id.right_bound; - for (const auto & point : path_with_lane_id.points) { - path.points.push_back(point.point); - } - - return path; -} - -Trajectory convert_to_trajectory(const Path & path) -{ - Trajectory traj; - for (const auto & point : path.points) { - TrajectoryPoint traj_point; - traj_point.pose = point.pose; - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - - traj.points.push_back(traj_point); - } - return traj; -} - [[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route( const RouteHandler & route_handler, const LaneletRoute & route) { @@ -109,22 +81,6 @@ std::vector get_lane_ids_from_route(const LaneletRoute & route) return lane_ids; } -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids) -{ - lanelet::ConstLanelets lanelets; - for (const lanelet::Id lane_id : lane_ids) { - const auto lanelet = route_handler.getLaneletsFromId(lane_id); - lanelets.push_back(lanelet); - } - return lanelets; -} - -rclcpp::NodeOptions create_node_options() -{ - return rclcpp::NodeOptions{}; -} - rclcpp::QoS create_transient_local_qos() { return rclcpp::QoS{1}.transient_local(); @@ -216,6 +172,15 @@ std_msgs::msg::Header createHeader(const rclcpp::Time & now) header.stamp = now; return header; } + +std::vector resampleTrajectoryPoints( + const std::vector & input_traj_points, const double resample_interval) +{ + // resample and calculate trajectory points' orientation + const auto input_traj = motion_utils::convertToTrajectory(input_traj_points); + auto resampled_input_traj = motion_utils::resampleTrajectory(input_traj, resample_interval); + return motion_utils::convertToTrajectoryPointArray(resampled_input_traj); +} } // namespace StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( @@ -224,13 +189,9 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( { // publishers pub_map_bin_ = create_publisher("lanelet2_map_topic", create_transient_local_qos()); - pub_raw_path_with_lane_id_ = - create_publisher("input_centerline", create_transient_local_qos()); - pub_raw_path_ = create_publisher("debug/raw_centerline", create_transient_local_qos()); - pub_whole_optimized_centerline_ = + pub_whole_centerline_ = create_publisher("output_whole_centerline", create_transient_local_qos()); - pub_optimized_centerline_ = - create_publisher("output_centerline", create_transient_local_qos()); + pub_centerline_ = create_publisher("output_centerline", create_transient_local_qos()); // debug publishers pub_debug_unsafe_footprints_ = @@ -240,32 +201,16 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( sub_traj_start_index_ = create_subscription( "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - if (!centerline_with_route_ || traj_end_index_ + 1 < msg.data) { - return; + if (msg.data <= traj_end_index_ + 1) { + update_centerline_range(msg.data, traj_end_index_); } - traj_start_index_ = msg.data; - - const auto & c = *centerline_with_route_; - const auto selected_centerline = std::vector( - c.centerline.begin() + traj_start_index_, c.centerline.begin() + traj_end_index_ + 1); - - pub_optimized_centerline_->publish( - motion_utils::convertToTrajectory(selected_centerline, createHeader(this->now()))); }); sub_traj_end_index_ = create_subscription( "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - if (!centerline_with_route_ || msg.data + 1 < traj_start_index_) { - return; + if (traj_start_index_ <= msg.data + 1) { + update_centerline_range(traj_start_index_, msg.data); } - traj_end_index_ = msg.data; - - const auto & c = *centerline_with_route_; - const auto selected_centerline = std::vector( - c.centerline.begin() + traj_start_index_, c.centerline.begin() + traj_end_index_ + 1); - - pub_optimized_centerline_->publish( - motion_utils::convertToTrajectory(selected_centerline, createHeader(this->now()))); }); sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { @@ -280,6 +225,11 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids}); } }); + sub_traj_resample_interval_ = create_subscription( + "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { + // TODO(murooka) + }); // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -308,6 +258,7 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( centerline_source_ = [&]() { const auto centerline_source_param = declare_parameter("centerline_source"); if (centerline_source_param == "optimization_trajectory_base") { + optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); return CenterlineSource::OptimizationTrajectoryBase; } else if (centerline_source_param == "bag_ego_trajectory_base") { return CenterlineSource::BagEgoTrajectoryBase; @@ -317,6 +268,24 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( }(); } +void StaticCenterlineOptimizerNode::update_centerline_range( + const int traj_start_index, const int traj_end_index) +{ + if (!centerline_with_route_) { + return; + } + + traj_start_index_ = traj_start_index; + traj_end_index_ = traj_end_index; + + const auto & centerline = centerline_with_route_->centerline; + const auto selected_centerline = std::vector( + centerline.begin() + traj_start_index_, centerline.begin() + traj_end_index_ + 1); + + pub_centerline_->publish( + motion_utils::convertToTrajectory(selected_centerline, createHeader(this->now()))); +} + void StaticCenterlineOptimizerNode::run() { // declare planning setting parameters @@ -334,20 +303,53 @@ void StaticCenterlineOptimizerNode::run() centerline_with_route_ = centerline_with_route; } -StaticCenterlineOptimizerNode::CenterlineWithRoute -StaticCenterlineOptimizerNode::generate_centerline_with_route() +CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_route() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto optimized_traj_points = plan_path(route_lane_ids); - return CenterlineWithRoute{optimized_traj_points, route_lane_ids}; - } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + if (!route_handler_ptr_) { + RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); return CenterlineWithRoute{}; } - throw std::logic_error("The centerline source is not supported in static_centerline_optimizer."); + auto centerline_with_route = [&]() { + if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto optimized_centerline = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); + return CenterlineWithRoute{optimized_centerline, route_lane_ids}; + } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + const auto bag_centerline = generate_centerline_with_bag(*this); + const auto start_lanelets = + route_handler_ptr_->getClosestLanelets(bag_centerline.front().pose); + const auto end_lanelets = route_handler_ptr_->getClosestLanelets(bag_centerline.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); + return CenterlineWithRoute{}; + } + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + + return CenterlineWithRoute{bag_centerline, route_lane_ids}; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_optimizer."); + }(); + + const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); + centerline_with_route.centerline = + resampleTrajectoryPoints(centerline_with_route.centerline, output_trajectory_interval); + + pub_whole_centerline_->publish( + motion_utils::convertToTrajectory(centerline_with_route.centerline, createHeader(this->now()))); + + pub_centerline_->publish( + motion_utils::convertToTrajectory(centerline_with_route.centerline, createHeader(this->now()))); + + return centerline_with_route; } void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_file_path) @@ -443,7 +445,7 @@ std::vector StaticCenterlineOptimizerNode::plan_route( plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); // initialize mission_planner - auto node = rclcpp::Node("po"); + auto node = rclcpp::Node("mission_planner"); mission_planner->initialize(&node, map_bin_ptr_); // plan route @@ -472,7 +474,7 @@ void StaticCenterlineOptimizerNode::on_plan_route( // plan route const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // extract lane ids std::vector lane_ids; @@ -491,133 +493,6 @@ void StaticCenterlineOptimizerNode::on_plan_route( response->lane_ids = lane_ids; } -std::vector StaticCenterlineOptimizerNode::plan_path( - const std::vector & route_lane_ids) -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); - return std::vector{}; - } - - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // optimize centerline inside the lane - const auto start_center_pose = - utils::get_center_pose(*route_handler_ptr_, route_lane_ids.front()); - - // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = - has_parameter("ego_nearest_dist_threshold") - ? get_parameter("ego_nearest_dist_threshold").as_double() - : declare_parameter("ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = - has_parameter("ego_nearest_yaw_threshold") - ? get_parameter("ego_nearest_yaw_threshold").as_double() - : declare_parameter("ego_nearest_yaw_threshold"); - const double behavior_path_interval = has_parameter("output_path_interval") - ? get_parameter("output_path_interval").as_double() - : declare_parameter("output_path_interval"); - const double behavior_vel_interval = - has_parameter("behavior_output_path_interval") - ? get_parameter("behavior_output_path_interval").as_double() - : declare_parameter("behavior_output_path_interval"); - - // extract path with lane id from lanelets - const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); - }(); - pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); - RCLCPP_INFO(get_logger(), "Calculated raw path with lane id and published."); - - // convert path with lane id to path - const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); - pub_raw_path_->publish(raw_path); - RCLCPP_INFO(get_logger(), "Converted to path and published."); - - // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); - pub_whole_optimized_centerline_->publish( - motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); - pub_optimized_centerline_->publish( - motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); - RCLCPP_INFO( - get_logger(), "Smoothed trajectory and made it collision free with the road and published."); - - return optimized_traj_points; -} - -std::vector StaticCenterlineOptimizerNode::optimize_trajectory( - const Path & raw_path) const -{ - // convert to trajectory points - const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); - - // create an instance of elastic band and model predictive trajectory. - const auto eb_path_smoother_ptr = - path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); - const auto mpt_optimizer_ptr = - obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); - - // NOTE: The optimization is executed every valid_optimized_traj_points_num points. - constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; - - // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use - // warm start. - constexpr int num_initial_optimization = 2; - - std::vector whole_optimized_traj_points; - for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { - // calculate virtual ego pose for the optimization - constexpr int virtual_ego_pose_offset_idx = 1; - const auto virtual_ego_pose = - raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; - - // smooth trajectory by elastic band in the path_smoother package - const auto smoothed_traj_points = - eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - - // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner - // package - const obstacle_avoidance_planner::PlannerData planner_data{ - raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, - virtual_ego_pose}; - const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); - - // connect the previously and currently optimized trajectory points - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - break; - } - } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - return whole_optimized_traj_points; -} - void StaticCenterlineOptimizerNode::on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) { @@ -629,7 +504,7 @@ void StaticCenterlineOptimizerNode::on_plan_path( // get lanelets from route lane ids const auto route_lane_ids = request->route; - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // check if input route lanelets are connected to each other. const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); @@ -641,7 +516,9 @@ void StaticCenterlineOptimizerNode::on_plan_path( } // plan path - const auto optimized_traj_points = plan_path(route_lane_ids); + const auto optimized_traj_points = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); // check calculation result if (optimized_traj_points.empty()) { @@ -693,7 +570,7 @@ void StaticCenterlineOptimizerNode::evaluate( const std::vector & route_lane_ids, const std::vector & optimized_traj_points) { - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); const auto dist_thresh_vec = has_parameter("marker_color_dist_thresh") ? get_parameter("marker_color_dist_thresh").as_double_array() @@ -770,7 +647,7 @@ void StaticCenterlineOptimizerNode::save_map( } const auto & c = centerline_with_route; - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); // update centerline in map utils::update_centerline(*route_handler_ptr_, route_lanelets, c.centerline); diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index f8cc0953a6dbd..3cc7ed5ca1fda 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -45,6 +45,17 @@ lanelet::Point3d createPoint3d(const double x, const double y, const double z = namespace utils { +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids) +{ + lanelet::ConstLanelets lanelets; + for (const lanelet::Id lane_id : lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + lanelets.push_back(lanelet); + } + return lanelets; +} + geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id) { diff --git a/planning/static_centerline_optimizer/test/data/bag_ego_trajectory.db3 b/planning/static_centerline_optimizer/test/data/bag_ego_trajectory.db3 new file mode 100644 index 0000000000000..0883307acbae0 Binary files /dev/null and b/planning/static_centerline_optimizer/test/data/bag_ego_trajectory.db3 differ diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 36bdfc732ed79..9f0a5df7f3fcc 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -39,6 +39,7 @@ def generate_test_description(): {"lanelet2_map_path": lanelet2_map_path}, {"run_background": False}, {"rviz": False}, + {"centerline_source": "optimization_trajectory_base"}, {"lanelet2_input_file_path": lanelet2_map_path}, {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, {"start_lanelet_id": 215},