Merge branch 'main' into fix/mpc/replace-vectorxd-with-vector3d

common/autoware_goal_distance_calculator/launch/goal_distance_calculator.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_goal_distance_calculator)/config/goal_distance_calculator.param.yaml"/>
   <node pkg="autoware_goal_distance_calculator" exec="autoware_goal_distance_calculator_node" name="autoware_goal_distance_calculator" output="screen">
     <param from="$(var config_file)"/>
   </node>

control/autoware_control_validator/config/control_validator.param.yaml

@@ -12,6 +12,7 @@
       rolling_back_velocity: 0.5
       over_velocity_offset: 2.0
       over_velocity_ratio: 0.2
+      nominal_latency: 0.01
 
     vel_lpf_gain: 0.9 # Time constant 0.33
     hold_velocity_error_until_stop: true

control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp

@@ -25,6 +25,7 @@
 #include <autoware_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
@@ -38,6 +39,7 @@
 
 namespace autoware::control_validator
 {
+using autoware_control_msgs::msg::Control;
 using autoware_control_validator::msg::ControlValidatorStatus;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -51,6 +53,7 @@ struct ValidationParams
   double rolling_back_velocity;
   double over_velocity_ratio;
   double over_velocity_offset;
+  double nominal_latency_threshold;
 };
 
 /**
@@ -67,6 +70,12 @@ class ControlValidator : public rclcpp::Node
    */
   explicit ControlValidator(const rclcpp::NodeOptions & options);
 
+  /**
+   * @brief Callback function for the control component output.
+   * @param msg Control message
+   */
+  void on_control_cmd(const Control::ConstSharedPtr msg);
+
   /**
    * @brief Callback function for the predicted trajectory.
    * @param msg Predicted trajectory message
@@ -134,6 +143,7 @@ class ControlValidator : public rclcpp::Node
     DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const;
 
   // Subscribers and publishers
+  rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
   autoware_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
   autoware_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;

control/autoware_control_validator/msg/ControlValidatorStatus.msg

@@ -4,10 +4,12 @@ builtin_interfaces/Time stamp
 bool is_valid_max_distance_deviation
 bool is_rolling_back
 bool is_over_velocity
+bool is_valid_latency
 
 # values
 float64 max_distance_deviation
 float64 target_vel
 float64 vehicle_vel
+float64 latency
 
 int64 invalid_count

control/autoware_control_validator/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <build_depend>rosidl_default_generators</build_depend>
 
+  <depend>autoware_control_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_signal_processing</depend>

control/autoware_control_validator/src/control_validator.cpp

@@ -34,6 +34,8 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
 {
   using std::placeholders::_1;
 
+  sub_control_cmd_ = create_subscription<Control>(
+    "~/input/control_cmd", 1, std::bind(&ControlValidator::on_control_cmd, this, _1));
   sub_predicted_traj_ = create_subscription<Trajectory>(
     "~/input/predicted_trajectory", 1,
     std::bind(&ControlValidator::on_predicted_trajectory, this, _1));
@@ -70,6 +72,7 @@ void ControlValidator::setup_parameters()
     p.rolling_back_velocity = declare_parameter<double>(t + "rolling_back_velocity");
     p.over_velocity_offset = declare_parameter<double>(t + "over_velocity_offset");
     p.over_velocity_ratio = declare_parameter<double>(t + "over_velocity_ratio");
+    p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
   }
   const auto lpf_gain = declare_parameter<double>("vel_lpf_gain");
   vehicle_vel_.setGain(lpf_gain);
@@ -126,6 +129,10 @@ void ControlValidator::setup_diag()
       stat, !validation_status_.is_over_velocity,
       "The vehicle is over-speeding against the target.");
   });
+  d.add(ns + "latency", [&](auto & stat) {
+    set_status(
+      stat, validation_status_.is_valid_latency, "The latency is larger than expected value.");
+  });
 }
 
 bool ControlValidator::is_data_ready()
@@ -147,6 +154,15 @@ bool ControlValidator::is_data_ready()
   return true;
 }
 
+void ControlValidator::on_control_cmd(const Control::ConstSharedPtr msg)
+{
+  validation_status_.latency = (this->now() - msg->stamp).seconds();
+  validation_status_.is_valid_latency =
+    validation_status_.latency < validation_params_.nominal_latency_threshold;
+  validation_status_.invalid_count =
+    is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
+}
+
 void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg)
 {
   stop_watch.tic();
@@ -254,7 +270,8 @@ void ControlValidator::calc_velocity_deviation_status(
 
 bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
 {
-  return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity;
+  return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity &&
+         s.is_valid_latency;
 }
 
 void ControlValidator::display_status()

launch/tier4_control_launch/launch/control.launch.xml

@@ -201,6 +201,7 @@
       <group if="$(var launch_control_validator)">
         <load_composable_node target="/control/control_check_container">
           <composable_node pkg="autoware_control_validator" plugin="autoware::control_validator::ControlValidator" name="control_validator">
+            <remap from="~/input/control_cmd" to="/control/command/control_cmd"/>
             <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
             <remap from="~/input/reference_trajectory" to="/planning/scenario_planning/trajectory"/>
             <remap from="~/input/predicted_trajectory" to="/control/trajectory_follower/lateral/predicted_trajectory"/>

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp

@@ -72,39 +72,40 @@ bool DistanceBasedStaticMapLoader::is_close_to_map(
   }
   return true;
 }
-
 bool DistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
-  if (!isFinite(point)) {
-    return false;
-  }
+  std::shared_ptr<pcl::search::Search<pcl::PointXYZ>> map_cell_kdtree;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
+    if (!isFinite(point)) {
+      return false;
+    }
 
-  const int map_grid_index = static_cast<int>(
-    std::floor((point.x - origin_x_) / map_grid_size_x_) +
-    map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+    const int map_grid_index = static_cast<int>(
+      std::floor((point.x - origin_x_) / map_grid_size_x_) +
+      map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
 
-  if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
-    return false;
-  }
-  if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
-    if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == nullptr) {
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
       return false;
     }
-    std::vector<int> nn_indices(1);
-    std::vector<float> nn_distances(1);
-    if (!current_voxel_grid_array_.at(map_grid_index)
-           ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+    const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index);
+    if (current_voxel_grid == nullptr) {
       return false;
     }
-    if (nn_distances[0] <= distance_threshold) {
-      return true;
-    }
+    map_cell_kdtree = current_voxel_grid->map_cell_kdtree;
   }
-  return false;
+
+  std::vector<int> nn_indices(1);
+  std::vector<float> nn_distances(1);
+  if (!map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+    return false;
+  }
+
+  return nn_distances[0] <= distance_threshold;
 }
 
 DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent(

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp

@@ -65,8 +65,11 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
   {
-    map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
-    map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+    {
+      std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+      map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+      map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+    }
 
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);

perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp

@@ -45,29 +45,30 @@ bool VoxelBasedApproximateStaticMapLoader::is_close_to_map(
 bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
+  VoxelGridPointXYZ map_cell_voxel_grid;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
 
-  const int map_grid_index = static_cast<int>(
-    std::floor((point.x - origin_x_) / map_grid_size_x_) +
-    map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+    const int map_grid_index = static_cast<int>(
+      std::floor((point.x - origin_x_) / map_grid_size_x_) +
+      map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
 
-  if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
-    return false;
-  }
-  if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
-    const int index = current_voxel_grid_array_.at(map_grid_index)
-                        ->map_cell_voxel_grid.getCentroidIndexAt(
-                          current_voxel_grid_array_.at(map_grid_index)
-                            ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z));
-    if (index == -1) {
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
       return false;
-    } else {
-      return true;
     }
+    const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index);
+    if (current_voxel_grid == nullptr) {
+      return false;
+    }
+    map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid;
   }
-  return false;
+
+  const int index = map_cell_voxel_grid.getCentroidIndexAt(
+    map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z));
+  return (index != -1);
 }
 
 VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent(

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp

@@ -80,25 +80,29 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
 bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
+  std::shared_ptr<pcl::search::Search<pcl::PointXYZ>> map_cell_kdtree;
+  VoxelGridPointXYZ map_cell_voxel_grid;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
 
-  const int map_grid_index = static_cast<int>(
-    std::floor((point.x - origin_x_) / map_grid_size_x_) +
-    map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+    const int map_grid_index = static_cast<int>(
+      std::floor((point.x - origin_x_) / map_grid_size_x_) +
+      map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
 
-  if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
-    return false;
-  }
-  if (
-    current_voxel_grid_array_.at(map_grid_index) != nullptr &&
-    is_close_to_neighbor_voxels(
-      point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid,
-      current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) {
-    return true;
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
+      return false;
+    }
+    if (current_voxel_grid_array_.at(map_grid_index) == nullptr) {
+      return false;
+    }
+    map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid;
+    map_cell_kdtree = current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree;
   }
-  return false;
+  return is_close_to_neighbor_voxels(
+    point, distance_threshold, map_cell_voxel_grid, map_cell_kdtree);
 }
 
 VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent(

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp

@@ -71,8 +71,11 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
   {
-    map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
-    map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+    {
+      std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+      map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+      map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+    }
 
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);