Merge branch 'main' into fix/fix_rare_error_in_pose_instability_detector

.github/workflows/clang-tidy-differential.yaml

@@ -38,7 +38,7 @@ jobs:
       - name: Get modified files
         id: get-modified-files
         run: |
-          echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' || true)" >> $GITHUB_OUTPUT
+          echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | tr '\n' ' ' || true)" >> $GITHUB_OUTPUT
         shell: bash
 
       - name: Run clang-tidy

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(

control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp

@@ -294,7 +294,12 @@ class PubSubNode : public rclcpp::Node
     const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end());
     const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end());
 
+    // This test is designed to verify that the filter is applied correctly. However, if topic
+    // communication is delayed, the allowable range of change in the command values between the
+    // sender and receiver of the topic will vary, making the results dependent on processing time.
+    // We define the allowable error margin to account for this.
     constexpr auto threshold_scale = 1.1;
+
     // Output command must be smaller than maximum limit.
     // TODO(Horibe): check for each velocity range.
     if (std::abs(lon_vel) > 0.01) {
@@ -433,7 +438,7 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
   //           << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias
   //           << ")" << std::endl;
 
-  for (size_t i = 0; i < 100; ++i) {
+  for (size_t i = 0; i < 30; ++i) {
     auto start_time = std::chrono::steady_clock::now();
 
     const bool reset_clock = (i == 0);
@@ -449,7 +454,13 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
     std::chrono::milliseconds elapsed =
       std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
 
-    std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10} - elapsed;
+    // The value determines the period of the topic. The filter logic of vehicle_cmd_gate depends on
+    // real-time, and if the time from publishing to subscribing becomes too long, this test will
+    // fail (the test specification itself should be improved). To prevent processing bottlenecks,
+    // please set this value appropriately. It is set to 30ms because it occasionally fails at 10ms.
+    constexpr int running_ms = 30;
+
+    std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{running_ms} - elapsed;
     if (sleep_duration.count() > 0) {
       std::this_thread::sleep_for(sleep_duration);
     }

perception/lidar_centerpoint/test/test_postprocess_kernel.cpp

@@ -94,12 +94,12 @@ TEST_F(PostprocessKernelTest, SingleDetectionTest)
   constexpr float detection_x = 70.f;
   constexpr float detection_y = -38.4f;
   constexpr float detection_z = 1.0;
-  constexpr float detection_log_w = std::log(7.0);
-  constexpr float detection_log_l = std::log(1.0);
-  constexpr float detection_log_h = std::log(2.0);
+  const float detection_log_w = std::log(7.0);
+  const float detection_log_l = std::log(1.0);
+  const float detection_log_h = std::log(2.0);
   constexpr float detection_yaw = M_PI_4;
-  constexpr float detection_yaw_sin = std::sin(detection_yaw);
-  constexpr float detection_yaw_cos = std::sin(detection_yaw);
+  const float detection_yaw_sin = std::sin(detection_yaw);
+  const float detection_yaw_cos = std::sin(detection_yaw);
   constexpr float detection_vel_x = 5.0;
   constexpr float detection_vel_y = -5.0;
 
@@ -240,9 +240,9 @@ TEST_F(PostprocessKernelTest, CircleNMSTest)
   constexpr float detection_x = 70.f;
   constexpr float detection_y = -38.4f;
   constexpr float detection_z = 1.0;
-  constexpr float detection_log_w = std::log(7.0);
-  constexpr float detection_log_l = std::log(1.0);
-  constexpr float detection_log_h = std::log(2.0);
+  const float detection_log_w = std::log(7.0);
+  const float detection_log_l = std::log(1.0);
+  const float detection_log_h = std::log(2.0);
   constexpr float detection_yaw1_sin = 0.0;
   constexpr float detection_yaw1_cos = 1.0;
   constexpr float detection_yaw2_sin = 1.0;

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -49,8 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
-  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
-    &node, "~/debug/" + ns_ + "/processing_time_ms");
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 
   using autoware::universe_utils::getOrDeclareParameter;
   auto & p = params_;
@@ -187,7 +189,11 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
   processing_times["footprints"] = footprints_duration_us / 1000;
   processing_times["collisions"] = collisions_duration_us / 1000;
   processing_times["Total"] = total_time_us / 1000;
-  processing_time_publisher_->publish(processing_times);
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -53,8 +53,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
-  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
-    &node, "~/debug/" + ns_ + "/processing_time_ms");
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 
   const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
   vehicle_lateral_offset_ = static_cast<double>(vehicle_info.max_lateral_offset_m);
@@ -226,7 +228,11 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
   processing_times["obstacles"] = obstacles_us / 1000;
   processing_times["slowdowns"] = slowdowns_us / 1000;
   processing_times["Total"] = total_us / 1000;
-  processing_time_publisher_->publish(processing_times);
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 }  // namespace autoware::motion_velocity_planner

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -57,8 +57,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
-  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
-    &node, "~/debug/" + ns_ + "/processing_time_ms");
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 }
 void OutOfLaneModule::init_parameters(rclcpp::Node & node)
 {
@@ -306,7 +308,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
   processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000;
   processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000;
   processing_times["Total"] = total_time_us / 1000;
-  processing_time_publisher_->publish(processing_times);
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp

@@ -23,6 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <memory>
 #include <string>
@@ -45,7 +46,8 @@ class PluginModuleInterface
   rclcpp::Logger logger_ = rclcpp::get_logger("");
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
-  std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_time_publisher_;
+  std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_diag_publisher_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
   autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 };
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml

@@ -27,6 +27,7 @@
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>
   <depend>rclcpp</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml

@@ -38,6 +38,7 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -82,6 +82,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
   velocity_factor_publisher_ =
     this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
       "~/output/velocity_factors", 1);
+  processing_time_publisher_ = this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/total_time/processing_time_ms", 1);
 
   // Parameters
   smooth_velocity_before_planning_ = declare_parameter<bool>("smooth_velocity_before_planning");
@@ -105,8 +107,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
     std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1));
 
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
-  published_time_publisher_ =
-    std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
 }
 
 void MotionVelocityPlannerNode::on_load_plugin(
@@ -284,10 +284,14 @@ void MotionVelocityPlannerNode::on_trajectory(
   lk.unlock();
 
   trajectory_pub_->publish(output_trajectory_msg);
-  published_time_publisher_->publish_if_subscribed(
+  published_time_publisher_.publish_if_subscribed(
     trajectory_pub_, output_trajectory_msg.header.stamp);
   processing_times["Total"] = stop_watch.toc("Total");
-  processing_time_publisher_.publish(processing_times);
+  processing_diag_publisher_.publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 void MotionVelocityPlannerNode::insert_stop(

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

@@ -32,6 +32,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <tf2_ros/buffer.h>
@@ -96,7 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
     velocity_factor_publisher_;
-  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
+  autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this};
 
   //  parameters
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;
@@ -140,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
     autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points);
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
-
-  std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
 };
 }  // namespace autoware::motion_velocity_planner