diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7f7961f438284..6fb3282956529 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -353,6 +353,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 0e6464a81f354..5d379f973e30b 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -2272,4 +2272,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 4731d44ed30bb..b19d8353d8236 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -28,23 +28,45 @@ ament_auto_add_executable(main src/utils.cpp ) +ament_auto_add_executable(main2 + src/main2.cpp + src/bag_centerline_generator_node.cpp + src/successive_trajectory_optimizer_node.cpp + src/utils.cpp +) + target_include_directories(main SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) +target_include_directories(main2 + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + target_link_libraries(main ${OpenCV_LIBRARIES} ) +target_link_libraries(main2 + ${OpenCV_LIBRARIES} +) + if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(main static_centerline_optimizer "rosidl_typesupport_cpp") + + rosidl_target_interfaces(main2 + static_centerline_optimizer "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp") target_link_libraries(main "${cpp_typesupport_target}") + + target_link_libraries(main2 "${cpp_typesupport_target}") endif() ament_auto_package( diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/bag_centerline_generator_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/bag_centerline_generator_node.hpp new file mode 100644 index 0000000000000..67c903614f612 --- /dev/null +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/bag_centerline_generator_node.hpp @@ -0,0 +1,91 @@ +// Copyright 2022 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__BAG_CENTERLINE_GENERATOR_NODE_HPP_ +#define STATIC_CENTERLINE_OPTIMIZER__BAG_CENTERLINE_GENERATOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" +#include "rosbag2_cpp/reader.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" +#include "static_centerline_optimizer/type_alias.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include + +namespace static_centerline_optimizer +{ +using static_centerline_optimizer::srv::LoadMap; +using static_centerline_optimizer::srv::PlanPath; +using static_centerline_optimizer::srv::PlanRoute; + +class BagCenterlineGeneratorNode : public rclcpp::Node +{ +public: + explicit BagCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); + void run(); + +private: + std::vector generateCenterlineFromBag(); + + // load map + void load_map(const std::string & lanelet2_input_file_path); + void on_load_map( + const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); + + // plan route + std::vector plan_route( + const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); + void on_plan_route( + const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); + + // plan path + std::vector plan_path(const std::vector & route_lane_ids); + void on_plan_path( + const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); + + void evaluate( + const std::vector & route_lane_ids, + const std::vector & optimized_traj_points); + void save_map( + const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, + const std::vector & optimized_traj_points); + + HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + std::shared_ptr route_handler_ptr_{nullptr}; + + // 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_optimized_centerline_{nullptr}; + + // service + rclcpp::Service::SharedPtr srv_load_map_; + rclcpp::Service::SharedPtr srv_plan_route_; + rclcpp::Service::SharedPtr srv_plan_path_; + + // callback group for service + rclcpp::CallbackGroup::SharedPtr callback_group_; + + // vehicle info + vehicle_info_util::VehicleInfo vehicle_info_; +}; +} // namespace static_centerline_optimizer +#endif // STATIC_CENTERLINE_OPTIMIZER__BAG_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/launch/bag_centerline_generator.launch.xml b/planning/static_centerline_optimizer/launch/bag_centerline_generator.launch.xml new file mode 100644 index 0000000000000..16afbd05b9892 --- /dev/null +++ b/planning/static_centerline_optimizer/launch/bag_centerline_generator.launch.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 89976be5bdd47..4c1641cedb011 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -24,6 +24,7 @@ interpolation lanelet2_extension map_loader + map_projection_loader mission_planner motion_utils obstacle_avoidance_planner @@ -36,6 +37,8 @@ vehicle_info_util python3-flask-cors + python3-mgrs + python3-xmltodict rosidl_default_runtime ament_cmake_gmock diff --git a/planning/static_centerline_optimizer/src/bag_centerline_generator_node.cpp b/planning/static_centerline_optimizer/src/bag_centerline_generator_node.cpp new file mode 100644 index 0000000000000..fff525da77535 --- /dev/null +++ b/planning/static_centerline_optimizer/src/bag_centerline_generator_node.cpp @@ -0,0 +1,658 @@ +// Copyright 2022 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/bag_centerline_generator_node.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" +#include "lanelet2_extension/utility/utilities.hpp" +#include "map_loader/lanelet2_map_loader_node.hpp" +#include "map_projection_loader/map_projection_loader.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "static_centerline_optimizer/msg/points_with_lane_id.hpp" +#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" +#include "static_centerline_optimizer/type_alias.hpp" +#include "static_centerline_optimizer/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +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; +} + +[[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route( + const RouteHandler & route_handler, const LaneletRoute & route) +{ + lanelet::ConstLanelets lanelets; + for (const auto & segment : route.segments) { + const auto & target_lanelet_id = segment.preferred_primitive.id; + const auto target_lanelet = route_handler.getLaneletsFromId(target_lanelet_id); + lanelets.push_back(target_lanelet); + } + + return lanelets; +} + +std::vector get_lane_ids_from_route(const LaneletRoute & route) +{ + std::vector lane_ids; + for (const auto & segment : route.segments) { + const auto & target_lanelet_id = segment.preferred_primitive.id; + lane_ids.push_back(target_lanelet_id); + } + + 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(); +} + +lanelet::BasicPoint2d convertToLaneletPoint(const geometry_msgs::msg::Point & geom_point) +{ + lanelet::BasicPoint2d point(geom_point.x, geom_point.y); + return point; +} + +LinearRing2d create_vehicle_footprint( + const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const double margin = 0.0) +{ + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m + margin; + const double x_rear = -(i.rear_overhang_m + margin); + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; + const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); + + std::vector geom_points; + geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); + geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); + geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); + geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); + + LinearRing2d footprint; + for (const auto & geom_point : geom_points) { + footprint.push_back(Point2d{geom_point.x, geom_point.y}); + } + footprint.push_back(footprint.back()); + + boost::geometry::correct(footprint); + + return footprint; +} + +geometry_msgs::msg::Pose get_text_pose( + const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m; + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; + + return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0); +} + +std::array convertHexStringToDecimal(const std::string & hex_str_color) +{ + unsigned int hex_int_color; + std::istringstream iss(hex_str_color); + iss >> std::hex >> hex_int_color; + + unsigned int unit = 16 * 16; + unsigned int b = hex_int_color % unit; + unsigned int g = (hex_int_color - b) / unit % unit; + unsigned int r = (hex_int_color - g * unit - b) / unit / unit; + + return std::array{r / 255.0, g / 255.0, b / 255.0}; +} + +std::vector check_lanelet_connection( + const RouteHandler & route_handler, const lanelet::ConstLanelets & route_lanelets) +{ + std::vector unconnected_lane_ids; + + for (size_t i = 0; i < route_lanelets.size() - 1; ++i) { + const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); + + const bool is_connected = + std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { + return next_lanelet.id() == route_lanelets.at(i + 1).id(); + }) != next_lanelets.end(); + if (!is_connected) { + unconnected_lane_ids.push_back(route_lanelets.at(i).id()); + } + } + + return unconnected_lane_ids; +} +} // namespace + +BagCenterlineGeneratorNode::BagCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options) +: Node("static_centerline_optimizer", node_options) +{ + // 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_optimized_centerline_ = + create_publisher("output_centerline", create_transient_local_qos()); + + // debug publishers + pub_debug_unsafe_footprints_ = + create_publisher("debug/unsafe_footprints", create_transient_local_qos()); + + // services + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_load_map_ = create_service( + "/planning/static_centerline_optimizer/load_map", + std::bind( + &BagCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + srv_plan_route_ = create_service( + "/planning/static_centerline_optimizer/plan_route", + std::bind( + &BagCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + srv_plan_path_ = create_service( + "/planning/static_centerline_optimizer/plan_path", + std::bind( + &BagCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + + // vehicle info + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); +} + +void BagCenterlineGeneratorNode::run() +{ + // declare planning setting parameters + const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); + // const auto lanelet2_input_file_path = + // // "/home/takayuki/AutonomousDrivingScenarios/map/odaiba_beta/lanelet2_map.osm"; + // "/home/takayuki/AutonomousDrivingScenarios/map/kashiwanoha/lanelet2_map.osm"; + const auto lanelet2_output_file_path = + declare_parameter("lanelet2_output_file_path"); + /* + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + */ + + // process + load_map(lanelet2_input_file_path); + // const auto optimized_traj_points = plan_path(route_lane_ids); + const auto centerline_traj_points = generateCenterlineFromBag(); + auto centerline_traj = motion_utils::convertToTrajectory(centerline_traj_points); + centerline_traj.header.frame_id = "map"; + pub_optimized_centerline_->publish(centerline_traj); + + const auto start_lanelets = + route_handler_ptr_->getClosestLanelets(centerline_traj_points.front().pose); + const auto end_lanelets = + route_handler_ptr_->getClosestLanelets(centerline_traj_points.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); + return; + } + 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); + save_map(lanelet2_output_file_path, route_lane_ids, centerline_traj_points); +} + +std::vector BagCenterlineGeneratorNode::generateCenterlineFromBag() +{ + const std::string bag_filename{ + "/home/takayuki/bag/kashiwanoha/" + "d4d1adc7-4fdf-4613-86b3-0daa2d945b9e_2023-12-22-13-35-13_p0900_0.db3"}; + // "/home/takayuki/bag/1128/left-route-LC1_with-LC/" + // "211130ae-8a64-4311-95ab-1b8406c4499b_2023-11-28-13-28-23_p0900_4.db3"}; + + 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(get_logger(), "Extracted centerline from the bag."); + + return centerline_traj_points; +} + +void BagCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) +{ + const auto map_projector_info = load_info_from_yaml( + "/home/takayuki/AutonomousDrivingScenarios/map/odaiba_beta/map_projector_info.yaml"); + + // load map by the map_loader package + map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { + // load map + lanelet::LaneletMapPtr map_ptr; + // tier4_map_msgs::msg::MapProjectorInfo map_projector_info; + // map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + if (!map_ptr) { + return nullptr; + } + + // create map bin msg + const auto map_bin_msg = + Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); + + return std::make_shared(map_bin_msg); + }(); + + // check if map_bin_ptr_ is not null pointer + if (!map_bin_ptr_) { + RCLCPP_ERROR(get_logger(), "Loading map failed"); + return; + } + RCLCPP_INFO(get_logger(), "Loaded map."); + + // publish map bin msg + pub_map_bin_->publish(*map_bin_ptr_); + RCLCPP_INFO(get_logger(), "Published map."); + + // create route_handler + route_handler_ptr_ = std::make_shared(); + route_handler_ptr_->setMap(*map_bin_ptr_); +} + +void BagCenterlineGeneratorNode::on_load_map( + const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) +{ + const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; + + // save map file temporarily since load map's input must be a file + std::ofstream map_writer; + map_writer.open(tmp_lanelet2_input_file_path, std::ios::out); + map_writer << request->map; + map_writer.close(); + + // load map from the saved map file + load_map(tmp_lanelet2_input_file_path); + + if (map_bin_ptr_) { + return; + } + + response->message = "InvalidMapFormat"; +} + +std::vector BagCenterlineGeneratorNode::plan_route( + const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) +{ + if (!map_bin_ptr_ || !route_handler_ptr_) { + RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); + return std::vector{}; + } + + // calculate check points (= start and goal pose) + const auto check_points = [&]() { + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return std::vector{start_center_pose, end_center_pose}; + }(); + RCLCPP_INFO(get_logger(), "Calculated check points."); + + // plan route by the mission_planner package + const auto route = [&]() { + // create mission_planner plugin + auto plugin_loader = pluginlib::ClassLoader( + "mission_planner", "mission_planner::PlannerPlugin"); + auto mission_planner = + plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); + + // initialize mission_planner + auto node = rclcpp::Node("po"); + mission_planner->initialize(&node, map_bin_ptr_); + + // plan route + const auto route = mission_planner->plan(check_points); + + return route; + }(); + RCLCPP_INFO(get_logger(), "Planned route."); + + // get lanelets + const auto route_lane_ids = get_lane_ids_from_route(route); + return route_lane_ids; +} + +void BagCenterlineGeneratorNode::on_plan_route( + const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) +{ + if (!map_bin_ptr_ || !route_handler_ptr_) { + response->message = "MapNotFound"; + RCLCPP_ERROR(get_logger(), "Map is not ready."); + return; + } + + const lanelet::Id start_lanelet_id = request->start_lane_id; + const lanelet::Id end_lanelet_id = request->end_lane_id; + + // 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); + + // extract lane ids + std::vector lane_ids; + for (const auto & lanelet : route_lanelets) { + lane_ids.push_back(lanelet.id()); + } + + // check calculation result + if (lane_ids.empty()) { + response->message = "RouteNotFound"; + RCLCPP_ERROR(get_logger(), "Route planning failed."); + return; + } + + // set response + response->lane_ids = lane_ids; +} + +std::vector BagCenterlineGeneratorNode::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()); + + // ego nearest search parameters + 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"); + + // extract path with lane id from lanelets + const auto raw_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); + + 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 = convert_to_path(raw_path_with_lane_id); + pub_raw_path_->publish(raw_path); + RCLCPP_INFO(get_logger(), "Converted to path and published."); + + // optimize trajectory by the obstacle_avoidance_planner package + SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options()); + const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path); + pub_optimized_centerline_->publish(optimized_traj); + const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj); + + RCLCPP_INFO(get_logger(), "Optimized trajectory and published."); + + return optimized_traj_points; +} + +void BagCenterlineGeneratorNode::on_plan_path( + const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) +{ + if (!route_handler_ptr_) { + response->message = "MapNotFound"; + RCLCPP_ERROR(get_logger(), "Route handler is not ready."); + return; + } + + // 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); + + // check if input route lanelets are connected to each other. + const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); + if (!unconnected_lane_ids.empty()) { + response->message = "LaneletsNotConnected"; + response->unconnected_lane_ids = unconnected_lane_ids; + RCLCPP_ERROR(get_logger(), "Lanelets are not connected."); + return; + } + + // plan path + const auto optimized_traj_points = plan_path(route_lane_ids); + + // check calculation result + if (optimized_traj_points.empty()) { + response->message = "PathNotFound"; + RCLCPP_ERROR(get_logger(), "Path planning failed."); + return; + } + + // publish unsafe_footprints + evaluate(route_lane_ids, optimized_traj_points); + + // create output data + auto target_traj_point = optimized_traj_points.cbegin(); + bool is_end_lanelet = false; + for (const auto & lanelet : route_lanelets) { + std::vector current_lanelet_points; + + // check if target point is inside the lanelet + while ( + lanelet::geometry::inside(lanelet, convertToLaneletPoint(target_traj_point->pose.position))) { + // memorize points inside the lanelet + current_lanelet_points.push_back(target_traj_point->pose.position); + target_traj_point++; + + if (target_traj_point == optimized_traj_points.cend()) { + is_end_lanelet = true; + break; + } + } + + if (!current_lanelet_points.empty()) { + // register points with lane_id + static_centerline_optimizer::msg::PointsWithLaneId points_with_lane_id; + points_with_lane_id.lane_id = lanelet.id(); + points_with_lane_id.points = current_lanelet_points; + response->points_with_lane_ids.push_back(points_with_lane_id); + } + + if (is_end_lanelet) { + break; + } + } + + // empty string if error did not occur + response->message = ""; +} + +void BagCenterlineGeneratorNode::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 dist_thresh_vec = + has_parameter("marker_color_dist_thresh") + ? get_parameter("marker_color_dist_thresh").as_double_array() + : declare_parameter>("marker_color_dist_thresh"); + const auto marker_color_vec = has_parameter("marker_color") + ? get_parameter("marker_color").as_string_array() + : declare_parameter>("marker_color"); + const auto get_marker_color = [&](const double dist) -> boost::optional> { + for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { + const double dist_thresh = dist_thresh_vec.at(i); + if (dist < dist_thresh) { + return convertHexStringToDecimal(marker_color_vec.at(i)); + } + } + return boost::none; + }; + + // create right/left bound + LineString2d right_bound; + LineString2d left_bound; + for (const auto & lanelet : route_lanelets) { + for (const auto & point : lanelet.rightBound()) { + boost::geometry::append(right_bound, Point2d(point.x(), point.y())); + } + for (const auto & point : lanelet.leftBound()) { + boost::geometry::append(left_bound, Point2d(point.x(), point.y())); + } + } + + // calculate the distance between footprint and right/left bounds + MarkerArray marker_array; + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < optimized_traj_points.size(); ++i) { + const auto & traj_point = optimized_traj_points.at(i); + + const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); + + const double dist_to_right = boost::geometry::distance(footprint_poly, right_bound); + const double dist_to_left = boost::geometry::distance(footprint_poly, left_bound); + const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); + + if (min_dist_to_bound < min_dist) { + min_dist = min_dist_to_bound; + } + + // create marker + const auto marker_color_opt = get_marker_color(min_dist_to_bound); + if (marker_color_opt) { + const auto & marker_color = marker_color_opt.get(); + + // add footprint marker + const auto footprint_marker = + utils::create_footprint_marker(footprint_poly, marker_color, now(), i); + tier4_autoware_utils::appendMarkerArray(footprint_marker, &marker_array); + + // add text of distance to bounds marker + const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); + const auto text_marker = + utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); + tier4_autoware_utils::appendMarkerArray(text_marker, &marker_array); + } + } + + pub_debug_unsafe_footprints_->publish(marker_array); + + RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist); +} + +void BagCenterlineGeneratorNode::save_map( + const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, + const std::vector & optimized_traj_points) +{ + if (!route_handler_ptr_) { + return; + } + + const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + + // update centerline in map + utils::update_centerline(*route_handler_ptr_, route_lanelets, optimized_traj_points); + RCLCPP_INFO(get_logger(), "Updated centerline in map."); + + // save map with modified center line + lanelet::write(lanelet2_output_file_path, *route_handler_ptr_->getLaneletMapPtr()); + RCLCPP_INFO(get_logger(), "Saved map."); +} +} // namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/src/main2.cpp b/planning/static_centerline_optimizer/src/main2.cpp new file mode 100644 index 0000000000000..ba9dd53f22bbc --- /dev/null +++ b/planning/static_centerline_optimizer/src/main2.cpp @@ -0,0 +1,39 @@ +// Copyright 2022 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/bag_centerline_generator_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + // initialize node + rclcpp::NodeOptions node_options; + auto node = + std::make_shared(node_options); + + // get ros parameter + const bool run_background = node->declare_parameter("run_background"); + + // process + if (!run_background) { + node->run(); + } + + // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}