diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt
index 08a97c9010008..261e8beb0022f 100644
--- a/planning/autoware_static_centerline_generator/CMakeLists.txt
+++ b/planning/autoware_static_centerline_generator/CMakeLists.txt
@@ -60,5 +60,7 @@ endif()
install(PROGRAMS
scripts/app.py
+ scripts/centerline_updater_helper.py
+ scripts/show_lanelet2_map_diff.py
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml
index 24a5536949479..060590803428a 100644
--- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml
+++ b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml
@@ -1,5 +1,9 @@
/**:
ros__parameters:
- marker_color: ["FF0000", "00FF00", "0000FF"]
+ marker_color: ["FF0000", "FF5A00", "FFFF00"]
marker_color_dist_thresh : [0.1, 0.2, 0.3]
output_trajectory_interval: 1.0
+
+ validation:
+ dist_threshold_to_road_border: 0.0
+ max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease.
diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml
index 3d786ab995e5b..38379a52e4c92 100644
--- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml
+++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml
@@ -3,17 +3,19 @@
-
+
-
-
+
-
+
+
+
+
@@ -28,7 +30,7 @@
default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"
/>
-
+
@@ -55,12 +57,8 @@
-
-
-
-
-
+
@@ -75,12 +73,19 @@
+
+
+
+
+
+
+
-
+
diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz
index 62b4c9b75ec87..fc916f188e758 100644
--- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz
+++ b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz
@@ -179,11 +179,11 @@ Visualization Manager:
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /static_centerline_generator/output_centerline
+ Value: /static_centerline_generator/output/centerline
Value: true
View Footprint:
- Alpha: 1
- Color: 255; 0; 0
+ Alpha: 0.5
+ Color: 0; 255; 0
Offset from BaseLink: 0
Rear Overhang: 1.0299999713897705
Value: true
@@ -268,9 +268,33 @@ Visualization Manager:
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /static_centerline_generator/debug/unsafe_footprints
+ Value: /static_centerline_generator/output/validation_results
Value: true
- Enabled: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: Debug Markers
+ Namespaces:
+ curvature: false
+ Topic:
+ Depth: 5
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /static_centerline_generator/debug/markers
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /static_centerline_generator/debug/ego_footprint_bounds
+ Value: true
+ Enabled: true
Name: debug
Enabled: true
Global Options:
diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py
index 3672165caed85..2cd0c2f66d474 100755
--- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py
+++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py
@@ -20,18 +20,20 @@
from PyQt5 import QtCore
from PyQt5.QtWidgets import QApplication
-from PyQt5.QtWidgets import QGridLayout
+from PyQt5.QtWidgets import QGroupBox
from PyQt5.QtWidgets import QMainWindow
from PyQt5.QtWidgets import QPushButton
from PyQt5.QtWidgets import QSizePolicy
from PyQt5.QtWidgets import QSlider
+from PyQt5.QtWidgets import QVBoxLayout
from PyQt5.QtWidgets import QWidget
from autoware_planning_msgs.msg import Trajectory
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSProfile
-from std_msgs.msg import Bool
+from std_msgs.msg import Empty
+from std_msgs.msg import Float32
from std_msgs.msg import Int32
@@ -46,25 +48,8 @@ def setupUI(self):
self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
central_widget = QWidget(self)
- central_widget.setObjectName("central_widget")
-
- self.grid_layout = QGridLayout(central_widget)
+ self.grid_layout = QVBoxLayout(central_widget)
self.grid_layout.setContentsMargins(10, 10, 10, 10)
- self.grid_layout.setObjectName("grid_layout")
-
- # button to update the trajectory's start and end index
- self.update_button = QPushButton("update slider")
- self.update_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
- self.update_button.clicked.connect(self.onUpdateButton)
-
- # button to reset the trajectory's start and end index
- self.reset_button = QPushButton("reset")
- self.reset_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
- self.reset_button.clicked.connect(self.onResetButton)
-
- # button to save map
- self.save_map_button = QPushButton("save map")
- self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
# slide of the trajectory's start and end index
self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal)
@@ -72,69 +57,60 @@ def setupUI(self):
self.min_traj_start_index = 0
self.max_traj_end_index = None
- # set layout
- self.grid_layout.addWidget(self.update_button, 1, 0, 1, -1)
- self.grid_layout.addWidget(self.reset_button, 2, 0, 1, -1)
- self.grid_layout.addWidget(self.save_map_button, 3, 0, 1, -1)
- self.grid_layout.addWidget(self.traj_start_index_slider, 4, 0, 1, -1)
- self.grid_layout.addWidget(self.traj_end_index_slider, 5, 0, 1, -1)
- self.setCentralWidget(central_widget)
-
- def initWithEndIndex(self, max_traj_end_index):
- self.max_traj_end_index = max_traj_end_index
-
- # initialize slider
- self.displayed_min_traj_start_index = self.min_traj_start_index
- self.displayed_max_traj_end_index = self.max_traj_end_index
- self.initializeSlider()
-
- def initializeSlider(self, move_value_to_end=True):
- self.traj_start_index_slider.setMinimum(0)
- self.traj_end_index_slider.setMinimum(0)
- self.traj_start_index_slider.setMaximum(
- self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
- )
- self.traj_end_index_slider.setMaximum(
- self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
+ # Layout: Range of Centerline
+ centerline_vertical_box = QVBoxLayout(self)
+ centerline_vertical_box.addWidget(self.traj_start_index_slider)
+ centerline_vertical_box.addWidget(self.traj_end_index_slider)
+ centerline_group = QGroupBox("Centerline")
+ centerline_group.setLayout(centerline_vertical_box)
+ self.grid_layout.addWidget(centerline_group)
+
+ # 2. Road Boundary
+ road_boundary_group = QGroupBox("Road Boundary")
+ road_boundary_vertical_box = QVBoxLayout(self)
+ road_boundary_group.setLayout(road_boundary_vertical_box)
+ self.grid_layout.addWidget(road_boundary_group)
+
+ # 2.1. Slider
+ self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal)
+ road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider)
+ self.road_boundary_lateral_margin_ratio = 10
+ self.road_boundary_lateral_margin_slider.setMinimum(0)
+ self.road_boundary_lateral_margin_slider.setMaximum(
+ 5 * self.road_boundary_lateral_margin_ratio
)
- if move_value_to_end:
- self.traj_start_index_slider.setValue(0)
- self.traj_end_index_slider.setValue(self.traj_end_index_slider.maximum())
+ road_boundary_vertical_box.addWidget(QPushButton("reset"))
- def onResetButton(self, event):
- current_traj_start_index = self.displayed_min_traj_start_index
- current_traj_end_index = self.displayed_max_traj_end_index
+ # 3. General
+ general_group = QGroupBox("General")
+ general_vertical_box = QVBoxLayout(self)
+ general_group.setLayout(general_vertical_box)
+ self.grid_layout.addWidget(general_group)
- self.displayed_min_traj_start_index = self.min_traj_start_index
- self.displayed_max_traj_end_index = self.max_traj_end_index
+ # 3.1. Validate Centerline
+ self.validate_button = QPushButton("validate")
+ self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+ general_vertical_box.addWidget(self.validate_button)
- self.initializeSlider(False)
- self.traj_start_index_slider.setValue(current_traj_start_index)
- if (
- current_traj_start_index == self.min_traj_start_index
- and current_traj_end_index == self.max_traj_end_index
- ):
- self.traj_end_index_slider.setValue(self.displayed_max_traj_end_index)
- else:
- self.traj_end_index_slider.setValue(
- current_traj_start_index + self.traj_end_index_slider.value()
- )
-
- def onUpdateButton(self, event):
- current_traj_start_index = self.getCurrentStartIndex()
- current_traj_end_index = self.getCurrentEndIndex()
+ # 3.2. Save Map
+ self.save_map_button = QPushButton("save map")
+ self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+ general_vertical_box.addWidget(self.save_map_button)
- self.displayed_min_traj_start_index = current_traj_start_index
- self.displayed_max_traj_end_index = current_traj_end_index
+ self.setCentralWidget(central_widget)
- self.initializeSlider()
+ def initWithEndIndex(self, max_traj_end_index):
+ self.max_traj_end_index = max_traj_end_index
- def getCurrentStartIndex(self):
- return self.displayed_min_traj_start_index + self.traj_start_index_slider.value()
+ # initialize sliders
+ self.traj_start_index_slider.setMinimum(self.min_traj_start_index)
+ self.traj_start_index_slider.setMaximum(self.max_traj_end_index)
+ self.traj_start_index_slider.setValue(self.min_traj_start_index)
- def getCurrentEndIndex(self):
- return self.displayed_min_traj_start_index + self.traj_end_index_slider.value()
+ self.traj_end_index_slider.setMinimum(self.min_traj_start_index)
+ self.traj_end_index_slider.setMaximum(self.max_traj_end_index)
+ self.traj_end_index_slider.setValue(self.max_traj_end_index)
class CenterlineUpdaterHelper(Node):
@@ -144,18 +120,30 @@ def __init__(self):
self.widget = CenterlineUpdaterWidget()
self.widget.show()
self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed)
+ self.widget.validate_button.clicked.connect(self.onValidateButtonPushed)
self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged)
self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged)
+ self.widget.road_boundary_lateral_margin_slider.valueChanged.connect(
+ self.onRoadBoundaryLateralMargin
+ )
# ROS
- self.pub_save_map = self.create_publisher(Bool, "~/save_map", 1)
- self.pub_traj_start_index = self.create_publisher(Int32, "~/traj_start_index", 1)
- self.pub_traj_end_index = self.create_publisher(Int32, "~/traj_end_index", 1)
+ self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1)
+ self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1)
+ self.pub_traj_start_index = self.create_publisher(
+ Int32, "/static_centerline_generator/traj_start_index", 1
+ )
+ self.pub_traj_end_index = self.create_publisher(
+ Int32, "/static_centerline_generator/traj_end_index", 1
+ )
+ self.pub_road_boundary_lateral_margin = self.create_publisher(
+ Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1
+ )
transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
self.sub_whole_centerline = self.create_subscription(
Trajectory,
- "/static_centerline_generator/output_whole_centerline",
+ "/static_centerline_generator/output/whole_centerline",
self.onWholeCenterline,
transient_local_qos,
)
@@ -168,20 +156,31 @@ def onWholeCenterline(self, whole_centerline):
self.widget.initWithEndIndex(len(whole_centerline.points) - 1)
def onSaveMapButtonPushed(self, event):
- msg = Bool()
- msg.data = True
+ msg = Empty()
self.pub_save_map.publish(msg)
+ def onValidateButtonPushed(self, event):
+ msg = Empty()
+ self.pub_validate.publish(msg)
+
def onStartIndexChanged(self, event):
msg = Int32()
- msg.data = self.widget.getCurrentStartIndex()
+ msg.data = self.widget.traj_start_index_slider.value()
self.pub_traj_start_index.publish(msg)
def onEndIndexChanged(self, event):
msg = Int32()
- msg.data = self.widget.getCurrentEndIndex()
+ msg.data = self.widget.traj_end_index_slider.value()
self.pub_traj_end_index.publish(msg)
+ def onRoadBoundaryLateralMargin(self, event):
+ msg = Float32()
+ msg.data = (
+ self.widget.road_boundary_lateral_margin_slider.value()
+ / self.widget.road_boundary_lateral_margin_ratio
+ )
+ self.pub_road_boundary_lateral_margin.publish(msg)
+
def main(args=None):
app = QApplication(sys.argv)
diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh
index e7f86062a9d20..c170e24e475d9 100755
--- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh
+++ b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh
@@ -1,3 +1,5 @@
#!/bin/bash
-ros2 launch autoware_static_centerline_generator static_centerline_generator.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 autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus
+ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus
+
+# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus
diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp
index 9f52f271cd5e5..5afc7e58ba52b 100644
--- a/planning/autoware_static_centerline_generator/src/main.cpp
+++ b/planning/autoware_static_centerline_generator/src/main.cpp
@@ -25,11 +25,19 @@ int main(int argc, char * argv[])
node_options);
// get ros parameter
- const bool run_background = node->declare_parameter("run_background");
+ const auto mode = node->declare_parameter("mode");
// process
- if (!run_background) {
- node->run();
+ if (mode == "AUTO") {
+ node->generate_centerline();
+ node->validate();
+ node->save_map();
+ } else if (mode == "GUI") {
+ node->generate_centerline();
+ } else if (mode == "VMB") {
+ // Do nothing
+ } else {
+ throw std::invalid_argument("The `mode` is invalid.");
}
// NOTE: spin node to keep showing debug path/trajectory in rviz with transient local
diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp
index c5ecc48ee209d..24ccd7d660edf 100644
--- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp
+++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp
@@ -20,6 +20,7 @@
#include "autoware/universe_utils/ros/parameter.hpp"
#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp"
#include "centerline_source/bag_ego_trajectory_based_centerline.hpp"
+#include "interpolation/spline_interpolation_points_2d.hpp"
#include "lanelet2_extension/utility/message_conversion.hpp"
#include "lanelet2_extension/utility/query.hpp"
#include "lanelet2_extension/utility/utilities.hpp"
@@ -34,7 +35,7 @@
#include
#include
-#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/empty.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/int32.hpp"
@@ -55,6 +56,10 @@
#include
#include
+#define RESET_TEXT "\x1B[0m"
+#define RED_TEXT "\x1B[31m"
+#define BOLD_TEXT "\x1B[1m"
+
namespace autoware::static_centerline_generator
{
namespace
@@ -115,7 +120,7 @@ geometry_msgs::msg::Pose get_text_pose(
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;
+ const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5;
return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0);
}
@@ -171,6 +176,32 @@ std::vector resample_trajectory_points(
autoware::motion_utils::resampleTrajectory(input_traj, resample_interval);
return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj);
}
+
+bool arePointsClose(
+ const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double epsilon)
+{
+ return std::abs(p1.x - p2.x) < epsilon && std::abs(p1.y - p2.y) < epsilon;
+}
+
+bool areSameDirection(
+ const double yaw, const geometry_msgs::msg::Point & start_point,
+ const geometry_msgs::msg::Point & end_point)
+{
+ return autoware::universe_utils::normalizeRadian(
+ yaw - std::atan2(end_point.y - start_point.y, end_point.x - start_point.x)) < M_PI_2;
+}
+
+std::vector convertToGeometryPoints(const LineString2d & lanelet_points)
+{
+ std::vector points;
+ for (const auto & lanelet_point : lanelet_points) {
+ geometry_msgs::msg::Point point;
+ point.x = lanelet_point.x();
+ point.y = lanelet_point.y();
+ points.push_back(point);
+ }
+ return points;
+}
} // namespace
StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode(
@@ -181,46 +212,53 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode(
pub_map_bin_ =
create_publisher("lanelet2_map_topic", utils::create_transient_local_qos());
pub_whole_centerline_ =
- create_publisher("output_whole_centerline", utils::create_transient_local_qos());
+ create_publisher("~/output/whole_centerline", utils::create_transient_local_qos());
pub_centerline_ =
- create_publisher("output_centerline", utils::create_transient_local_qos());
+ create_publisher("~/output/centerline", utils::create_transient_local_qos());
// debug publishers
- pub_debug_unsafe_footprints_ =
- create_publisher("debug/unsafe_footprints", utils::create_transient_local_qos());
+ pub_validation_results_ =
+ create_publisher("~/validation_results", utils::create_transient_local_qos());
+ pub_debug_markers_ =
+ create_publisher("~/debug/markers", utils::create_transient_local_qos());
+
+ pub_debug_ego_footprint_bounds_ = create_publisher(
+ "~/debug/ego_footprint_bounds", utils::create_transient_local_qos());
// subscribers
+ sub_footprint_margin_for_road_bound_ = create_subscription(
+ "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1},
+ [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; });
sub_traj_start_index_ = create_subscription(
- "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1},
+ "/static_centerline_generator/traj_start_index", rclcpp::QoS{1},
[this](const std_msgs::msg::Int32 & msg) {
- update_centerline_range(msg.data, traj_range_indices_.second);
+ if (centerline_handler_.update_start_index(msg.data)) {
+ visualize_selected_centerline();
+ }
});
sub_traj_end_index_ = create_subscription(
- "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1},
+ "/static_centerline_generator/traj_end_index", rclcpp::QoS{1},
[this](const std_msgs::msg::Int32 & msg) {
- update_centerline_range(traj_range_indices_.first, msg.data);
+ if (centerline_handler_.update_end_index(msg.data)) {
+ visualize_selected_centerline();
+ }
});
- sub_save_map_ = create_subscription(
- "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) {
- const auto lanelet2_output_file_path =
- autoware::universe_utils::getOrDeclareParameter(
- *this, "lanelet2_output_file_path");
- if (!centerline_with_route_ || msg.data) {
- const auto & c = *centerline_with_route_;
- const auto selected_centerline = std::vector(
- c.centerline.begin() + traj_range_indices_.first,
- c.centerline.begin() + traj_range_indices_.second + 1);
- const auto selected_route_lane_ids = get_route_lane_ids_from_points(selected_centerline);
- save_map(
- lanelet2_output_file_path,
- CenterlineWithRoute{selected_centerline, selected_route_lane_ids});
+ sub_save_map_ = create_subscription(
+ "/static_centerline_generator/save_map", rclcpp::QoS{1},
+ [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) {
+ if (!centerline_handler_.is_valid()) {
+ return;
}
+ save_map();
});
sub_traj_resample_interval_ = create_subscription(
- "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1},
+ "/static_centerline_generator/traj_resample_interval", rclcpp::QoS{1},
[this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) {
// TODO(murooka)
});
+ sub_validate_ = create_subscription(
+ "/static_centerline_generator/validate", rclcpp::QoS{1},
+ [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); });
// services
callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -259,43 +297,43 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode(
}();
}
-void StaticCenterlineGeneratorNode::update_centerline_range(
- const int traj_start_index, const int traj_end_index)
+void StaticCenterlineGeneratorNode::visualize_selected_centerline()
{
- if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) {
- return;
- }
-
- traj_range_indices_ = std::make_pair(traj_start_index, traj_end_index);
-
- const auto & centerline = centerline_with_route_->centerline;
- const auto selected_centerline = std::vector(
- centerline.begin() + traj_range_indices_.first,
- centerline.begin() + traj_range_indices_.second + 1);
-
+ // publish selected centerline
+ const auto selected_centerline = centerline_handler_.get_selected_centerline();
pub_centerline_->publish(
autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now())));
+
+ // delete markers for validation
+ pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now()));
+ pub_debug_markers_->publish(utils::create_delete_all_marker_array(
+ {"unsafe_footprints", "unsafe_footprints_distance"}, now()));
+ pub_debug_ego_footprint_bounds_->publish(
+ utils::create_delete_all_marker_array({"road_bounds"}, now()));
}
-void StaticCenterlineGeneratorNode::run()
+void StaticCenterlineGeneratorNode::generate_centerline()
{
// declare planning setting parameters
const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path");
- const auto lanelet2_output_file_path =
- declare_parameter("lanelet2_output_file_path");
// process
load_map(lanelet2_input_file_path);
- const auto centerline_with_route = generate_centerline_with_route();
- traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1);
+ const auto whole_centerline_with_route = generate_whole_centerline_with_route();
+ centerline_handler_ = CenterlineHandler(whole_centerline_with_route);
- evaluate(centerline_with_route.route_lane_ids, centerline_with_route.centerline);
- save_map(lanelet2_output_file_path, centerline_with_route);
+ visualize_selected_centerline();
+}
- centerline_with_route_ = centerline_with_route;
+void StaticCenterlineGeneratorNode::validate()
+{
+ const auto selected_centerline = centerline_handler_.get_selected_centerline();
+ const auto road_bounds = update_road_boundary(selected_centerline);
+
+ evaluate();
}
-CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_route()
+CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route()
{
if (!route_handler_ptr_) {
RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory.");
@@ -307,14 +345,15 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout
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 route_lane_ids = plan_route_by_lane_ids(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 route_lane_ids = get_route_lane_ids_from_points(bag_centerline);
+ const auto route_lane_ids =
+ plan_route(bag_centerline.front().pose, bag_centerline.back().pose);
return CenterlineWithRoute{bag_centerline, route_lane_ids};
}
throw std::logic_error(
@@ -329,30 +368,9 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout
pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory(
centerline_with_route.centerline, create_header(this->now())));
- pub_centerline_->publish(autoware::motion_utils::convertToTrajectory(
- centerline_with_route.centerline, create_header(this->now())));
-
return centerline_with_route;
}
-std::vector StaticCenterlineGeneratorNode::get_route_lane_ids_from_points(
- const std::vector & points)
-{
- const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.front().pose);
- const auto end_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.back().pose);
- if (start_lanelets.empty() || end_lanelets.empty()) {
- RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's points are not found.");
- return std::vector{};
- }
-
- const lanelet::Id start_lanelet_id = start_lanelets.front().id();
- const lanelet::Id end_lanelet_id = end_lanelets.front().id();
- if (start_lanelet_id == end_lanelet_id) {
- return std::vector{start_lanelet_id};
- }
- return plan_route(start_lanelet_id, end_lanelet_id);
-}
-
void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path)
{
// copy the input LL2 map to the temporary file for debugging
@@ -426,24 +444,35 @@ void StaticCenterlineGeneratorNode::on_load_map(
response->message = "InvalidMapFormat";
}
-std::vector StaticCenterlineGeneratorNode::plan_route(
+std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids(
const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id)
{
- if (!map_bin_ptr_ || !route_handler_ptr_) {
+ if (!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.");
+ 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 plan_route(start_center_pose, end_center_pose);
+}
+
+std::vector StaticCenterlineGeneratorNode::plan_route(
+ const geometry_msgs::msg::Pose & start_center_pose,
+ const geometry_msgs::msg::Pose & end_center_pose)
+{
+ if (!map_bin_ptr_) {
+ RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids.");
+ return std::vector{};
+ }
// plan route by the mission_planner package
const auto route = [&]() {
+ // calculate check points
+ RCLCPP_INFO(get_logger(), "Calculated check points.");
+ const auto check_points =
+ std::vector{start_center_pose, end_center_pose};
+
// create mission_planner plugin
auto plugin_loader = pluginlib::ClassLoader(
"autoware_mission_planner", "autoware::mission_planner::PlannerPlugin");
@@ -459,10 +488,16 @@ std::vector StaticCenterlineGeneratorNode::plan_route(
return route;
}();
- RCLCPP_INFO(get_logger(), "Planned route.");
// get lanelets
const auto route_lane_ids = get_lane_ids_from_route(route);
+
+ std::string route_lane_ids_str = "";
+ for (const lanelet::Id route_lane_id : route_lane_ids) {
+ route_lane_ids_str += std::to_string(route_lane_id) + ",";
+ }
+ RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")");
+
return route_lane_ids;
}
@@ -479,7 +514,7 @@ void StaticCenterlineGeneratorNode::on_plan_route(
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_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id);
const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids);
// extract lane ids
@@ -533,8 +568,11 @@ void StaticCenterlineGeneratorNode::on_plan_path(
return;
}
+ centerline_handler_ =
+ CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids});
+
// publish unsafe_footprints
- evaluate(route_lane_ids, optimized_traj_points);
+ evaluate();
// create output data
auto target_traj_point = optimized_traj_points.cbegin();
@@ -572,16 +610,128 @@ void StaticCenterlineGeneratorNode::on_plan_path(
response->message = "";
}
-void StaticCenterlineGeneratorNode::evaluate(
- const std::vector & route_lane_ids,
- const std::vector & optimized_traj_points)
+RoadBounds StaticCenterlineGeneratorNode::update_road_boundary(
+ const std::vector & centerline)
+{
+ const double max_ego_lon_offset = vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m;
+ const double min_ego_lon_offset = -vehicle_info_.rear_overhang_m;
+ const double max_ego_lat_offset =
+ vehicle_info_.wheel_tread_m / 2.0 + vehicle_info_.left_overhang_m;
+ const double ego_lat_offset = max_ego_lat_offset + footprint_margin_for_road_bound_;
+
+ std::vector ego_left_bound;
+ std::vector ego_right_bound;
+ for (size_t i = 0; i < centerline.size(); ++i) {
+ const auto & centerline_point = centerline.at(i).pose;
+ if (i == 0) {
+ // Add the first bound point
+ ego_left_bound.push_back(autoware::universe_utils::calcOffsetPose(
+ centerline_point, min_ego_lon_offset, ego_lat_offset, 0.0)
+ .position);
+ ego_right_bound.push_back(autoware::universe_utils::calcOffsetPose(
+ centerline_point, min_ego_lon_offset, -ego_lat_offset, 0.0)
+ .position);
+ }
+
+ if (i == centerline.size() - 1) {
+ // Add the last bound point
+ const auto ego_left_bound_last_point =
+ autoware::universe_utils::calcOffsetPose(
+ centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0)
+ .position;
+ if (!arePointsClose(ego_left_bound.back(), ego_left_bound_last_point, 1e-6)) {
+ ego_left_bound.push_back(ego_left_bound_last_point);
+ }
+ const auto ego_right_bound_last_point =
+ autoware::universe_utils::calcOffsetPose(
+ centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0)
+ .position;
+ if (!arePointsClose(ego_right_bound.back(), ego_right_bound_last_point, 1e-6)) {
+ ego_right_bound.push_back(ego_right_bound_last_point);
+ }
+ } else {
+ // Calculate new bound point depending on the orientation
+ const auto & next_centerline_point = centerline.at(i + 1).pose;
+ const double diff_yaw = autoware::universe_utils::normalizeRadian(
+ tf2::getYaw(next_centerline_point.orientation) - tf2::getYaw(centerline_point.orientation));
+ const auto [ego_left_bound_new_point, ego_right_bound_new_point] = [&]() {
+ if (0 < diff_yaw) {
+ return std::make_pair(
+ autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, ego_lat_offset, 0.0)
+ .position,
+ autoware::universe_utils::calcOffsetPose(
+ centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0)
+ .position);
+ }
+ return std::make_pair(
+ autoware::universe_utils::calcOffsetPose(
+ centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0)
+ .position,
+ autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, -ego_lat_offset, 0.0)
+ .position);
+ }();
+
+ // Check if the bound will be longitudinally monotonic.
+ if (areSameDirection(
+ tf2::getYaw(centerline_point.orientation), ego_left_bound.back(),
+ ego_left_bound_new_point)) {
+ ego_left_bound.push_back(ego_left_bound_new_point);
+ }
+ if (areSameDirection(
+ tf2::getYaw(centerline_point.orientation), ego_right_bound.back(),
+ ego_right_bound_new_point)) {
+ ego_right_bound.push_back(ego_right_bound_new_point);
+ }
+ }
+ }
+
+ // Publish marker
+ MarkerArray ego_footprint_bounds_marker_array;
+ {
+ auto left_bound_marker = autoware::universe_utils::createDefaultMarker(
+ "map", now(), "road_bounds", 0, Marker::LINE_STRIP,
+ autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0),
+ autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8));
+ left_bound_marker.lifetime = rclcpp::Duration(0, 0);
+ for (const auto & ego_left_bound_point : ego_left_bound) {
+ left_bound_marker.points.push_back(ego_left_bound_point);
+ }
+ ego_footprint_bounds_marker_array.markers.push_back(left_bound_marker);
+ }
+ {
+ auto right_bound_marker = autoware::universe_utils::createDefaultMarker(
+ "map", now(), "road_bounds", 1, Marker::LINE_STRIP,
+ autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0),
+ autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8));
+ right_bound_marker.lifetime = rclcpp::Duration(0, 0);
+ for (const auto & ego_right_bound_point : ego_right_bound) {
+ right_bound_marker.points.push_back(ego_right_bound_point);
+ }
+ ego_footprint_bounds_marker_array.markers.push_back(right_bound_marker);
+ }
+ pub_debug_ego_footprint_bounds_->publish(ego_footprint_bounds_marker_array);
+
+ return RoadBounds{ego_left_bound, ego_right_bound};
+}
+
+void StaticCenterlineGeneratorNode::evaluate()
{
+ std::cerr << std::endl
+ << "############################################## Validation Results "
+ "##############################################"
+ << std::endl;
+
+ const auto & centerline = centerline_handler_.get_selected_centerline();
+ const auto & route_lane_ids = centerline_handler_.get_route_lane_ids();
const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids);
- const auto dist_thresh_vec = autoware::universe_utils::getOrDeclareParameter>(
- *this, "marker_color_dist_thresh");
- const auto marker_color_vec =
- autoware::universe_utils::getOrDeclareParameter>(
- *this, "marker_color");
+
+ const double dist_thresh_to_road_border =
+ getRosParameter("validation.dist_threshold_to_road_border");
+ const double max_steer_angle_margin =
+ getRosParameter("validation.max_steer_angle_margin");
+
+ const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh");
+ const auto marker_color_vec = getRosParameter>("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);
@@ -593,27 +743,34 @@ void StaticCenterlineGeneratorNode::evaluate(
};
// create right/left bound
- LineString2d right_bound;
- LineString2d left_bound;
+ LineString2d lanelet_right_bound;
+ LineString2d lanelet_left_bound;
for (const auto & lanelet : route_lanelets) {
for (const auto & point : lanelet.rightBound()) {
- boost::geometry::append(right_bound, Point2d(point.x(), point.y()));
+ boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y()));
}
for (const auto & point : lanelet.leftBound()) {
- boost::geometry::append(left_bound, Point2d(point.x(), point.y()));
+ boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y()));
}
}
+ // calculate curvature
+ SplineInterpolationPoints2d centerline_spline(centerline);
+ const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures();
+ const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle(
+ vehicle_info_.max_steer_angle_rad - max_steer_angle_margin);
+
// 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);
+ double max_curvature = std::numeric_limits::min();
+ for (size_t i = 0; i < centerline.size(); ++i) {
+ const auto & traj_point = centerline.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 dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound);
+ const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound);
const double min_dist_to_bound = std::min(dist_to_right, dist_to_left);
if (min_dist_to_bound < min_dist) {
@@ -622,46 +779,111 @@ void StaticCenterlineGeneratorNode::evaluate(
// create marker
const auto marker_color_opt = get_marker_color(min_dist_to_bound);
+ const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_);
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);
- autoware::universe_utils::appendMarkerArray(footprint_marker, &marker_array);
+ const auto footprint_marker = utils::create_footprint_marker(
+ footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7,
+ now(), i);
+ marker_array.markers.push_back(footprint_marker);
// 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);
- autoware::universe_utils::appendMarkerArray(text_marker, &marker_array);
+ const auto text_marker = utils::create_text_marker(
+ "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0),
+ marker_color.at(1), marker_color.at(2), 0.999, now(), i);
+ marker_array.markers.push_back(text_marker);
+ }
+
+ const double curvature = curvature_vec.at(i);
+ const auto text_marker =
+ utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i);
+ marker_array.markers.push_back(text_marker);
+
+ if (max_curvature < std::abs(curvature)) {
+ max_curvature = std::abs(curvature);
}
}
- pub_debug_unsafe_footprints_->publish(marker_array);
+ // publish left boundary
+ const auto left_bound = convertToGeometryPoints(lanelet_left_bound);
+ const auto right_bound = convertToGeometryPoints(lanelet_right_bound);
+
+ marker_array.markers.push_back(
+ utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now()));
+ marker_array.markers.push_back(
+ utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now()));
+ pub_debug_markers_->publish(marker_array);
+
+ // show the validation results
+ // 1. distance from footprints to road boundaries
+ const bool are_footprints_inside_lanelets = [&]() {
+ std::cerr << "1. Footprints inside Lanelets:" << std::endl;
+ if (dist_thresh_to_road_border < min_dist) {
+ std::cerr << " The generated centerline is inside the lanelet. (threshold:"
+ << dist_thresh_to_road_border << " < actual:" << min_dist << ")" << std::endl
+ << " Passed." << std::endl;
+ return true;
+ }
+ std::cerr << RED_TEXT
+ << " The generated centerline is outside the lanelet. (actual:" << min_dist
+ << " <= threshold:" << dist_thresh_to_road_border << ")" << std::endl
+ << " Failed." << RESET_TEXT << std::endl;
+ return false;
+ }();
+ // 2. centerline's curvature
+ const bool is_curvature_low = [&]() {
+ std::cerr << "2. Curvature:" << std::endl;
+ if (max_curvature < curvature_threshold) {
+ std::cerr << " The generated centerline has no high curvature. (actual:" << max_curvature
+ << " < threshold:" << curvature_threshold << ")"
+ << " Passed." << std::endl;
+ return true;
+ }
+ std::cerr << RED_TEXT << " The generated centerline has a too high curvature. (threshold:"
+ << curvature_threshold << " <= actual:" << max_curvature << ")"
+ << " Failed." << RESET_TEXT << std::endl;
+ return false;
+ }();
+ // 3. result
+ std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl;
+ if (are_footprints_inside_lanelets && is_curvature_low) {
+ std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl;
+ } else {
+ std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl;
+ }
- RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist);
+ std::cerr << "###################################################################################"
+ "#############################"
+ << std::endl
+ << std::endl;
+ RCLCPP_INFO(get_logger(), "Validated the generated centerline.");
}
-void StaticCenterlineGeneratorNode::save_map(
- const std::string & lanelet2_output_file_path, const CenterlineWithRoute & centerline_with_route)
+void StaticCenterlineGeneratorNode::save_map()
{
if (!route_handler_ptr_) {
return;
}
- const auto & c = centerline_with_route;
- const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids);
+ const auto & centerline = centerline_handler_.get_selected_centerline();
+ const auto & route_lane_ids = centerline_handler_.get_route_lane_ids();
+
+ const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path");
+
+ const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids);
// update centerline in map
- utils::update_centerline(original_map_ptr_, route_lanelets, c.centerline);
+ utils::update_centerline(original_map_ptr_, route_lanelets, centerline);
RCLCPP_INFO(get_logger(), "Updated centerline in map.");
// save map with modified center line
- std::filesystem::create_directory("/tmp/static_centerline_generator"); // TODO(murooka)
+ std::filesystem::create_directory("/tmp/autoware_static_centerline_generator");
const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_);
lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector);
- RCLCPP_INFO(get_logger(), "Saved map.");
+ RCLCPP_INFO(
+ get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm");
// copy the output LL2 map to the temporary file for debugging
const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"};
diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp
index 2f25a064dca2a..42033f7956bfd 100644
--- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp
+++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp
@@ -15,6 +15,7 @@
#ifndef STATIC_CENTERLINE_GENERATOR_NODE_HPP_
#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_
+#include "autoware/universe_utils/ros/parameter.hpp"
#include "autoware_static_centerline_generator/srv/load_map.hpp"
#include "autoware_static_centerline_generator/srv/plan_path.hpp"
#include "autoware_static_centerline_generator/srv/plan_route.hpp"
@@ -23,7 +24,7 @@
#include "rclcpp/rclcpp.hpp"
#include "type_alias.hpp"
-#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/empty.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/int32.hpp"
#include "tier4_map_msgs/msg/map_projector_info.hpp"
@@ -45,12 +46,67 @@ struct CenterlineWithRoute
std::vector centerline{};
std::vector route_lane_ids{};
};
+struct CenterlineHandler
+{
+ CenterlineHandler() = default;
+ explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route)
+ : whole_centerline_with_route(centerline_with_route),
+ start_index(0),
+ end_index(centerline_with_route.centerline.size() - 1)
+ {
+ }
+ std::vector get_selected_centerline() const
+ {
+ if (!whole_centerline_with_route) {
+ return std::vector{};
+ }
+ const auto & centerline_begin = whole_centerline_with_route->centerline.begin();
+ return std::vector(
+ centerline_begin + start_index, centerline_begin + end_index + 1);
+ }
+ std::vector get_route_lane_ids() const
+ {
+ if (!whole_centerline_with_route) {
+ return std::vector{};
+ }
+ return whole_centerline_with_route->route_lane_ids;
+ }
+ bool is_valid() const { return whole_centerline_with_route && start_index < end_index; }
+ bool update_start_index(const int arg_start_index)
+ {
+ if (whole_centerline_with_route && arg_start_index < end_index) {
+ start_index = arg_start_index;
+ return true;
+ }
+ return false;
+ }
+ bool update_end_index(const int arg_end_index)
+ {
+ if (whole_centerline_with_route && start_index < arg_end_index) {
+ end_index = arg_end_index;
+ return true;
+ }
+ return false;
+ }
+
+ std::optional whole_centerline_with_route{std::nullopt};
+ int start_index{};
+ int end_index{};
+};
+
+struct RoadBounds
+{
+ std::vector left_bound;
+ std::vector right_bound;
+};
class StaticCenterlineGeneratorNode : public rclcpp::Node
{
public:
explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options);
- void run();
+ void generate_centerline();
+ void validate();
+ void save_map();
private:
// load map
@@ -59,33 +115,41 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node
const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
// plan route
- std::vector plan_route(
+ std::vector plan_route_by_lane_ids(
const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id);
+ std::vector plan_route(
+ const geometry_msgs::msg::Pose & start_center_pose,
+ const geometry_msgs::msg::Pose & end_center_pose);
+
void on_plan_route(
const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
// plan centerline
- CenterlineWithRoute generate_centerline_with_route();
+ CenterlineWithRoute generate_whole_centerline_with_route();
std::vector get_route_lane_ids_from_points(
const std::vector & points);
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);
- void save_map(
- const std::string & lanelet2_output_file_path,
- const CenterlineWithRoute & centerline_with_route);
+ void visualize_selected_centerline();
+ RoadBounds update_road_boundary(const std::vector & centerline);
+ void evaluate();
+
+ // parameter
+ template
+ T getRosParameter(const std::string & param_name)
+ {
+ return autoware::universe_utils::getOrDeclareParameter(*this, param_name);
+ }
lanelet::LaneletMapPtr original_map_ptr_{nullptr};
LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
std::shared_ptr route_handler_ptr_{nullptr};
std::unique_ptr map_projector_info_{nullptr};
- std::pair traj_range_indices_{0, 0};
- std::optional centerline_with_route_{std::nullopt};
+ CenterlineHandler centerline_handler_;
+
+ float footprint_margin_for_road_bound_{0.0};
enum class CenterlineSource {
OptimizationTrajectoryBase = 0,
@@ -96,15 +160,19 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node
// publisher
rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr};
- rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr};
rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr};
rclcpp::Publisher::SharedPtr pub_centerline_{nullptr};
+ rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr};
+ rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr};
+ rclcpp::Publisher::SharedPtr pub_debug_markers_{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_save_map_;
+ rclcpp::Subscription::SharedPtr sub_validate_;
rclcpp::Subscription::SharedPtr sub_traj_resample_interval_;
+ rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_;
// service
rclcpp::Service::SharedPtr srv_load_map_;
diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp
index c5157acc2b525..2b7b99bfe81be 100644
--- a/planning/autoware_static_centerline_generator/src/type_alias.hpp
+++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp
@@ -40,6 +40,7 @@ using autoware_planning_msgs::msg::PathPoint;
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
using tier4_planning_msgs::msg::PathWithLaneId;
+using visualization_msgs::msg::Marker;
using visualization_msgs::msg::MarkerArray;
} // namespace autoware::static_centerline_generator
diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp
index de362e3af5d7f..1a8e0eae2fbd9 100644
--- a/planning/autoware_static_centerline_generator/src/utils.cpp
+++ b/planning/autoware_static_centerline_generator/src/utils.cpp
@@ -23,6 +23,7 @@
#include
#include
+#include
namespace autoware::static_centerline_generator
{
@@ -151,6 +152,7 @@ void update_centerline(
auto & lanelet_ref = lanelets_ref.at(lanelet_idx);
const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y);
+ // TODO(murooka) This does not work with L-crank map.
const bool is_inside = lanelet::geometry::inside(lanelet_ref, point);
if (is_inside) {
const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z);
@@ -181,20 +183,17 @@ void update_centerline(
}
}
-MarkerArray create_footprint_marker(
- const LinearRing2d & footprint_poly, const std::array & marker_color,
- const rclcpp::Time & now, const size_t idx)
+Marker create_footprint_marker(
+ const LinearRing2d & footprint_poly, const double width, const double r, const double g,
+ const double b, const double a, const rclcpp::Time & now, const size_t idx)
{
- const double r = marker_color.at(0);
- const double g = marker_color.at(1);
- const double b = marker_color.at(2);
-
auto marker = autoware::universe_utils::createDefaultMarker(
"map", rclcpp::Clock().now(), "unsafe_footprints", idx,
visualization_msgs::msg::Marker::LINE_STRIP,
- autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0),
- autoware::universe_utils::createMarkerColor(r, g, b, 0.999));
+ autoware::universe_utils::createMarkerScale(width, 0.0, 0.0),
+ autoware::universe_utils::createMarkerColor(r, g, b, a));
marker.header.stamp = now;
+ // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local.
marker.lifetime = rclcpp::Duration(0, 0);
for (const auto & point : footprint_poly) {
@@ -206,38 +205,79 @@ MarkerArray create_footprint_marker(
marker.points.push_back(geom_point);
}
marker.points.push_back(marker.points.front());
-
- visualization_msgs::msg::MarkerArray marker_array;
- marker_array.markers.push_back(marker);
-
- return marker_array;
+ return marker;
}
-MarkerArray create_distance_text_marker(
- const geometry_msgs::msg::Pose & pose, const double dist,
- const std::array & marker_color, const rclcpp::Time & now, const size_t idx)
+Marker create_text_marker(
+ const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r,
+ const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx)
{
- const double r = marker_color.at(0);
- const double g = marker_color.at(1);
- const double b = marker_color.at(2);
-
auto marker = autoware::universe_utils::createDefaultMarker(
- "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx,
- visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+ "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5),
- autoware::universe_utils::createMarkerColor(r, g, b, 0.999));
+ autoware::universe_utils::createMarkerColor(r, g, b, a));
marker.pose = pose;
marker.header.stamp = now;
marker.lifetime = rclcpp::Duration(0, 0);
std::stringstream ss;
- ss << std::setprecision(2) << dist;
+ ss << std::setprecision(2) << value;
marker.text = ss.str();
- visualization_msgs::msg::MarkerArray marker_array;
- marker_array.markers.push_back(marker);
+ return marker;
+}
+
+Marker create_points_marker(
+ const std::string & ns, const std::vector & points, const double width,
+ const double r, const double g, const double b, const double a, const rclcpp::Time & now)
+{
+ auto marker = autoware::universe_utils::createDefaultMarker(
+ "map", now, ns, 1, Marker::LINE_STRIP,
+ autoware::universe_utils::createMarkerScale(width, 0.0, 0.0),
+ autoware::universe_utils::createMarkerColor(r, g, b, a));
+ marker.lifetime = rclcpp::Duration(0, 0);
+ marker.points = points;
+ return marker;
+}
+
+MarkerArray create_delete_all_marker_array(
+ const std::vector & ns_vec, const rclcpp::Time & now)
+{
+ Marker marker;
+ marker.header.stamp = now;
+ marker.action = visualization_msgs::msg::Marker::DELETEALL;
+
+ MarkerArray marker_array;
+ for (const auto & ns : ns_vec) {
+ marker.ns = ns;
+ marker_array.markers.push_back(marker);
+ }
return marker_array;
}
+
+std::pair, std::vector>
+calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets)
+{
+ std::vector left_bound;
+ std::vector right_bound;
+ for (const auto & lanelet : lanelets) {
+ for (const auto & lanelet_left_bound_point : lanelet.leftBound()) {
+ geometry_msgs::msg::Point left_bound_point;
+ left_bound_point.x = lanelet_left_bound_point.x();
+ left_bound_point.y = lanelet_left_bound_point.y();
+ left_bound_point.z = lanelet_left_bound_point.z();
+ left_bound.push_back(left_bound_point);
+ }
+ for (const auto & lanelet_right_bound_point : lanelet.rightBound()) {
+ geometry_msgs::msg::Point right_bound_point;
+ right_bound_point.x = lanelet_right_bound_point.x();
+ right_bound_point.y = lanelet_right_bound_point.y();
+ right_bound_point.z = lanelet_right_bound_point.z();
+ right_bound.push_back(right_bound_point);
+ }
+ }
+ return std::make_pair(left_bound, right_bound);
+}
} // namespace utils
} // namespace autoware::static_centerline_generator
diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp
index 70a7e61e19b18..e7fd51b30e658 100644
--- a/planning/autoware_static_centerline_generator/src/utils.hpp
+++ b/planning/autoware_static_centerline_generator/src/utils.hpp
@@ -22,6 +22,7 @@
#include
#include
+#include
#include
namespace autoware::static_centerline_generator
@@ -45,13 +46,23 @@ void update_centerline(
lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets,
const std::vector & new_centerline);
-MarkerArray create_footprint_marker(
- const LinearRing2d & footprint_poly, const std::array & marker_color,
- const rclcpp::Time & now, const size_t idx);
+Marker create_footprint_marker(
+ const LinearRing2d & footprint_poly, const double width, const double r, const double g,
+ const double b, const double a, const rclcpp::Time & now, const size_t idx);
-MarkerArray create_distance_text_marker(
- const geometry_msgs::msg::Pose & pose, const double dist,
- const std::array & marker_color, const rclcpp::Time & now, const size_t idx);
+Marker create_text_marker(
+ const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r,
+ const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx);
+
+Marker create_points_marker(
+ const std::string & ns, const std::vector & points, const double width,
+ const double r, const double g, const double b, const double a, const rclcpp::Time & now);
+
+MarkerArray create_delete_all_marker_array(
+ const std::vector & ns_vec, const rclcpp::Time & now);
+
+std::pair, std::vector>
+calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets);
} // namespace utils
} // namespace autoware::static_centerline_generator
diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3
new file mode 100644
index 0000000000000..ed3448772b794
Binary files /dev/null and b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 differ
diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3
similarity index 100%
rename from planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3
rename to planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3
diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py
index 07f8f3e30e659..4bb0630d13942 100644
--- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py
+++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py
@@ -38,7 +38,7 @@ def generate_test_description():
output="screen",
parameters=[
{"lanelet2_map_path": lanelet2_map_path},
- {"run_background": False},
+ {"mode": "AUTO"},
{"rviz": False},
{"centerline_source": "optimization_trajectory_base"},
{"lanelet2_input_file_path": lanelet2_map_path},
diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp
index d4b430ecd0579..510788dd09d08 100644
--- a/system/default_ad_api/src/operation_mode.cpp
+++ b/system/default_ad_api/src/operation_mode.cpp
@@ -66,8 +66,13 @@ void OperationModeNode::change_mode(
const ResponseT res, const OperationModeRequest::_mode_type mode)
{
if (!mode_available_[mode]) {
+ RCLCPP_WARN(
+ get_logger(),
+ "The target mode is not available. Please check the cause with "
+ "rqt_diagnostics_graph_monitor");
throw component_interface_utils::ServiceException(
- ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system.");
+ ServiceResponse::ERROR_NOT_AVAILABLE,
+ "The target mode is not available. Please check the diagnostics.");
}
const auto req = std::make_shared();
req->mode = mode;
diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt
index b68e7bedb57ed..0c36964f49237 100644
--- a/system/diagnostic_graph_utils/CMakeLists.txt
+++ b/system/diagnostic_graph_utils/CMakeLists.txt
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
ament_auto_add_library(${PROJECT_NAME}_tools SHARED
src/node/dump.cpp
src/node/converter.cpp
+ src/node/logging.cpp
)
rclcpp_components_register_node(${PROJECT_NAME}_tools
@@ -24,4 +25,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools
EXECUTABLE converter_node
)
-ament_auto_package()
+rclcpp_components_register_node(${PROJECT_NAME}_tools
+ PLUGIN "diagnostic_graph_utils::LoggingNode"
+ EXECUTABLE logging_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/system/diagnostic_graph_utils/launch/logging.launch.xml b/system/diagnostic_graph_utils/launch/logging.launch.xml
new file mode 100644
index 0000000000000..12636574eb8be
--- /dev/null
+++ b/system/diagnostic_graph_utils/launch/logging.launch.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp
new file mode 100644
index 0000000000000..009af51e949cd
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/logging.cpp
@@ -0,0 +1,95 @@
+// Copyright 2024 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 "logging.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace diagnostic_graph_utils
+{
+
+LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", options)
+{
+ root_path_ = declare_parameter("root_path");
+ max_depth_ = declare_parameter("max_depth");
+
+ using std::placeholders::_1;
+ sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1));
+ sub_graph_.subscribe(*this, 1);
+
+ const auto period = rclcpp::Rate(declare_parameter("show_rate")).period();
+ timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); });
+}
+
+void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
+{
+ // Search root node.
+ root_unit_ = nullptr;
+ for (const auto & unit : graph->units()) {
+ if (unit->path() == root_path_) {
+ root_unit_ = unit;
+ return;
+ }
+ }
+ RCLCPP_ERROR_STREAM(get_logger(), "root unit is not found: " << root_path_);
+}
+
+void LoggingNode::on_timer()
+{
+ static const auto message = "The target mode is not available for the following reasons:";
+ if (root_unit_ && root_unit_->level() != DiagUnit::DiagnosticStatus::OK) {
+ dump_text_.str("");
+ dump_text_.clear(std::stringstream::goodbit);
+ dump_unit(root_unit_, 0, " ");
+ RCLCPP_WARN_STREAM(get_logger(), message << std::endl << dump_text_.str());
+ }
+}
+
+void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & indent)
+{
+ const auto text_level = [](DiagUnit::DiagnosticLevel level) {
+ if (level == DiagUnit::DiagnosticStatus::OK) return "OK ";
+ if (level == DiagUnit::DiagnosticStatus::WARN) return "WARN ";
+ if (level == DiagUnit::DiagnosticStatus::ERROR) return "ERROR";
+ if (level == DiagUnit::DiagnosticStatus::STALE) return "STALE";
+ return "-----";
+ };
+
+ if (max_depth_ < depth) {
+ return;
+ }
+ if (unit->level() == DiagUnit::DiagnosticStatus::OK) {
+ return;
+ }
+
+ std::string path = unit->path();
+ if (path.empty()) {
+ path = "[anonymous group]";
+ }
+
+ dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl;
+ for (const auto & child : unit->children()) {
+ dump_unit(child.unit, depth + 1, indent + " ");
+ }
+}
+
+} // namespace diagnostic_graph_utils
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::LoggingNode)
diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp
new file mode 100644
index 0000000000000..b2d305c81ca99
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/logging.hpp
@@ -0,0 +1,48 @@
+// Copyright 2024 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 NODE__LOGGING_HPP_
+#define NODE__LOGGING_HPP_
+
+#include "diagnostic_graph_utils/subscription.hpp"
+
+#include
+
+#include
+#include
+
+namespace diagnostic_graph_utils
+{
+
+class LoggingNode : public rclcpp::Node
+{
+public:
+ explicit LoggingNode(const rclcpp::NodeOptions & options);
+
+private:
+ void on_create(DiagGraph::ConstSharedPtr graph);
+ void on_timer();
+ void dump_unit(DiagUnit * unit, int depth, const std::string & indent);
+ DiagGraphSubscription sub_graph_;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ DiagUnit * root_unit_;
+ int max_depth_;
+ std::string root_path_;
+ std::ostringstream dump_text_;
+};
+
+} // namespace diagnostic_graph_utils
+
+#endif // NODE__LOGGING_HPP_
diff --git a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp
index e951f11e49ab7..923fc53055475 100644
--- a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp
+++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp
@@ -50,6 +50,9 @@ struct VehicleInfo
autoware::universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const;
autoware::universe_utils::LinearRing2d createFootprint(
const double lat_margin, const double lon_margin) const;
+
+ double calcMaxCurvature() const;
+ double calcCurvatureFromSteerAngle(const double steer_angle) const;
};
/// Create vehicle info from base parameters
diff --git a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp
index db223663f1145..2dd5b19f2f523 100644
--- a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp
+++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp
@@ -85,4 +85,30 @@ VehicleInfo createVehicleInfo(
};
}
+double VehicleInfo::calcMaxCurvature() const
+{
+ if (std::abs(wheel_base_m) < 1e-6) {
+ throw std::invalid_argument("wheel_base_m is 0.");
+ }
+ if (std::abs(max_steer_angle_rad) < 1e-6) {
+ return std::numeric_limits::max();
+ }
+
+ const double radius = wheel_base_m / std::tan(max_steer_angle_rad);
+ const double curvature = 1.0 / radius;
+ return curvature;
+}
+double VehicleInfo::calcCurvatureFromSteerAngle(const double steer_angle) const
+{
+ if (std::abs(wheel_base_m) < 1e-6) {
+ throw std::invalid_argument("wheel_base_m is 0.");
+ }
+ if (std::abs(steer_angle) < 1e-6) {
+ return std::numeric_limits::max();
+ }
+
+ const double radius = wheel_base_m / std::tan(steer_angle);
+ const double curvature = 1.0 / radius;
+ return curvature;
+}
} // namespace autoware::vehicle_info_utils