chore: sync beta branch beta/v0.29.0 with tier4/main

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp

@@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
   // Publisher
   autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
-  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
+    this, "~/debug/processing_time_ms_diag"};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   lane_departure_checker_->setParam(param_, vehicle_info);
 
   // Publisher
+  processing_time_publisher_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   // Nothing
 
   // Diagnostic Updater
@@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer()
   }
 
   processing_time_map["Total"] = stop_watch.toc("Total");
-  processing_time_publisher_.publish(processing_time_map);
+  processing_diag_publisher_.publish(processing_time_map);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_time_map["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(

launch/tier4_perception_launch/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>detected_object_feature_remover</exec_depend>
   <exec_depend>detected_object_validation</exec_depend>
   <exec_depend>detection_by_tracker</exec_depend>
+  <exec_depend>elevation_map_loader</exec_depend>
   <exec_depend>euclidean_cluster</exec_depend>
   <exec_depend>ground_segmentation</exec_depend>
   <exec_depend>image_projection_based_fusion</exec_depend>

localization/pose_instability_detector/src/pose_instability_detector.cpp

@@ -354,6 +354,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     start_twist.header.stamp = start_time;
     result_deque.push_front(start_twist);
   } else {
+    if (result_deque.size() < 2) {
+      return result_deque;
+    }
     // If the first element is earlier than start_time, interpolate the first element
     rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp);
     rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp);
@@ -380,6 +383,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     end_twist.header.stamp = end_time;
     result_deque.push_back(end_twist);
   } else {
+    if (result_deque.size() < 2) {
+      return result_deque;
+    }
     // If the last element is later than end_time, interpolate the last element
     rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp);
     rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp);

localization/pose_instability_detector/test/test.cpp

@@ -163,6 +163,64 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small)  // NOL
   EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN);
 }
 
+TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_data_comes)  // NOLINT
+{
+  // [Condition] There is no twist_msg between the two target odometry_msgs.
+  // Normally this doesn't seem to happen.
+  // As far as I can think, this happens when the odometry msg stops (so the next timer callback
+  // will refer to the same odometry msg, and the timestamp difference will be calculated as 0)
+  // This test case shows that an error occurs when two odometry msgs come in close succession and
+  // there is no other odometry msg.
+  // Referring again, this doesn't normally seem to happen in usual operation.
+
+  builtin_interfaces::msg::Time timestamp{};
+
+  // send the twist message1
+  timestamp.sec = 0;
+  timestamp.nanosec = 0;
+  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
+
+  // send the first odometry message after the first twist message
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8 + 1;
+  helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0);
+
+  // process the above message (by timer_callback)
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // send the second odometry message before the second twist message
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8 + 1e7;
+  helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0);
+
+  // send the twist message2
+  timestamp.sec = 1;
+  timestamp.nanosec = 0;
+  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
+
+  // process the above messages (by timer_callback)
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // provoke timer callback again
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // This test is OK if pose_instability_detector does not crash. The diagnostics status is not
+  // checked.
+  SUCCEED();
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

@@ -106,7 +106,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
     temporary_cluster.height = pointcloud_msg->height;
     temporary_cluster.fields = pointcloud_msg->fields;
     temporary_cluster.point_step = point_step;
-    temporary_cluster.data.resize(max_cluster_size_ * point_step);
+    temporary_cluster.data.resize(cluster.indices.size() * point_step);
     clusters_data_size.push_back(0);
   }
 
@@ -117,13 +117,17 @@ bool VoxelGridBasedEuclideanCluster::cluster(
       voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
     if (map.find(index) != map.end()) {
       auto & cluster_data_size = clusters_data_size.at(map[index]);
-      if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
+      if (cluster_data_size > std::size_t(max_cluster_size_ * point_step)) {
         continue;
       }
       std::memcpy(
         &temporary_clusters.at(map[index]).data[cluster_data_size],
         &pointcloud_msg->data[i * point_step], point_step);
       cluster_data_size += point_step;
+      if (cluster_data_size == temporary_clusters.at(map[index]).data.size()) {
+        temporary_clusters.at(map[index])
+          .data.resize(temporary_clusters.at(map[index]).data.size() * 2);
+      }
     }
   }
 

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -292,12 +292,13 @@ void RANSACGroundFilterComponent::filter(
   no_ground_cloud_msg_ptr->header = input->header;
   no_ground_cloud_msg_ptr->fields = input->fields;
   no_ground_cloud_msg_ptr->point_step = point_step;
+  no_ground_cloud_msg_ptr->data.resize(input->data.size());
   size_t output_size = 0;
   // use not downsampled pointcloud for extract pointcloud that higher than height threshold
   for (size_t global_size = 0; global_size < input->data.size(); global_size += point_step) {
-    float x = *reinterpret_cast<float *>(input->data[global_size + x_offset]);
-    float y = *reinterpret_cast<float *>(input->data[global_size + y_offset]);
-    float z = *reinterpret_cast<float *>(input->data[global_size + z_offset]);
+    float x = *reinterpret_cast<const float *>(&input->data[global_size + x_offset]);
+    float y = *reinterpret_cast<const float *>(&input->data[global_size + y_offset]);
+    float z = *reinterpret_cast<const float *>(&input->data[global_size + z_offset]);
     const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z);
     if (std::abs(transformed_point.z()) > height_threshold_) {
       std::memcpy(

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}
 )

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.

planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml

@@ -3,17 +3,19 @@
   <arg name="vehicle_model"/>
 
   <!-- flag -->
-  <arg name="run_background" default="false"/>
+  <arg name="mode" default="AUTO" description="select from AUTO, GUI, and VMB"/>
   <arg name="rviz" default="true"/>
   <arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
-  <arg name="bag_filename" default="bag.db3"/>
 
-  <!-- mandatory arguments when run_background is true -->
+  <!-- mandatory arguments when mode is AUTO -->
   <arg name="lanelet2_input_file_path" default=""/>
-  <arg name="lanelet2_output_file_path" default="/tmp/static_centerline_generator/lanelet2_map.osm"/>
+  <arg name="lanelet2_output_file_path" default="/tmp/autoware_static_centerline_generator/lanelet2_map.osm"/>
   <arg name="start_lanelet_id" default=""/>
   <arg name="end_lanelet_id" default=""/>
 
+  <!-- mandatory arguments when mode is GUI -->
+  <arg name="bag_filename" default="bag.db3"/>
+
   <!-- topic -->
   <arg name="lanelet2_map_topic" default="/map/vector_map"/>
   <arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
@@ -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"
   />
   <arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
-  <arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/path_optimizer.param.yaml"/>
+  <arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml"/>
   <arg name="mission_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
 
   <!-- Global parameters (for PathFootprint in tier4_planning_rviz_plugin) -->
@@ -55,12 +57,8 @@
   <node pkg="autoware_static_centerline_generator" exec="main" name="static_centerline_generator">
     <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
     <remap from="input_centerline" to="~/input_centerline"/>
-    <remap from="output_whole_centerline" to="~/output_whole_centerline"/>
-    <remap from="output_centerline" to="~/output_centerline"/>
-    <remap from="debug/raw_centerline" to="~/debug/raw_centerline"/>
-    <remap from="debug/unsafe_footprints" to="~/debug/unsafe_footprints"/>
 
-    <param name="run_background" value="$(var run_background)"/>
+    <param name="mode" value="$(var mode)"/>
     <param name="lanelet2_input_file_path" value="$(var lanelet2_input_file_path)"/>
     <param name="lanelet2_output_file_path" value="$(var lanelet2_output_file_path)"/>
     <param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
@@ -75,12 +73,19 @@
     <param from="$(var path_smoother_param)"/>
     <param from="$(var path_optimizer_param)"/>
     <param from="$(var mission_planner_param)"/>
+    <param name="check_footprint_inside_lanes" value="false"/>
+    <!-- override the mission_planner's parameter -->
     <!-- node param -->
     <param from="$(find-pkg-share autoware_static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
     <param name="centerline_source" value="$(var centerline_source)"/>
     <param name="bag_filename" value="$(var bag_filename)"/>
   </node>
 
+  <!-- GUI to select the range of centerline -->
+  <group if="$(eval &quot;'$(var mode)'=='GUI'&quot;)">
+    <node pkg="autoware_static_centerline_generator" exec="centerline_updater_helper.py" name="centerline_updater_helper"/>
+  </group>
+
   <!-- rviz -->
-  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.param" if="$(var rviz)"/>
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
 </launch>

planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz

@@ -122,7 +122,7 @@ Visualization Manager:
       Name: Map
     - Class: rviz_plugins/PathWithLaneId
       Color Border Vel Max: 3
-      Enabled: true
+      Enabled: false
       Name: Raw Centerline
       Topic:
         Depth: 5
@@ -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: