Merge branch 'main' into refactor/rework_dual_return_outlier_filter_p…

…arameter
autowarefoundation · Oct 1, 2024 · 7be1c2e · 7be1c2e
2 parents 7e329dc + 6a24683
commit 7be1c2e
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 14 deletions.
diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -461,7 +461,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
     const auto ego_polys = generatePathFootprint(path, expand_width_);
     std::vector<ObjectData> objects;
     // Crop out Pointcloud using an extra wide ego path
-    if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) {
+    if (
+      use_pointcloud_data_ && points_belonging_to_cluster_hulls &&
+      !points_belonging_to_cluster_hulls->empty()) {
       const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
       getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects);
     }
@@ -712,6 +714,9 @@ void AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin, std::vector<Polygon2d> & polygons)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return;
+  }
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -721,8 +726,11 @@ void AEB::generatePathFootprint(
 std::vector<Polygon2d> AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin)
 {
-  std::vector<Polygon2d> polygons;
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return {};
+  }
+  std::vector<Polygon2d> polygons;
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -735,7 +743,7 @@ void AEB::createObjectDataUsingPredictedObjects(
   std::vector<ObjectData> & object_data_vector)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-  if (predicted_objects_ptr_->objects.empty()) return;
+  if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return;
 
   const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity;
   const auto & objects = predicted_objects_ptr_->objects;
@@ -926,6 +934,9 @@ void AEB::cropPointCloudWithEgoFootprintPath(
   const std::vector<Polygon2d> & ego_polys, PointCloud::Ptr filtered_objects)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (ego_polys.empty()) {
+    return;
+  }
   PointCloud::Ptr full_points_ptr(new PointCloud);
   pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr);
   // Create a Point cloud with the points of the ego footprint

diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp
@@ -35,7 +35,7 @@ uint8_t getSemanticType(const std::string & class_name);
 
 std::array<double, 36> convertPoseCovarianceMatrix(const Box3D & box3d);
 
-std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d);
+std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw);
 
 bool isCarLikeVehicleLabel(const uint8_t label);
 

diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp
@@ -50,7 +50,7 @@ void box3DToDetectedObject(
 
   // pose and shape
   // mmdet3d yaw format to ros yaw format
-  float yaw = -box3d.yaw - autoware::universe_utils::pi / 2;
+  const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2;
   obj.kinematics.pose_with_covariance.pose.position =
     autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z);
   obj.kinematics.pose_with_covariance.pose.orientation =
@@ -67,6 +67,8 @@ void box3DToDetectedObject(
   if (has_twist) {
     float vel_x = box3d.vel_x;
     float vel_y = box3d.vel_y;
+
+    // twist of the object is based on the object coordinate system
     geometry_msgs::msg::Twist twist;
     twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y;
     twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y;
@@ -76,7 +78,7 @@ void box3DToDetectedObject(
     obj.kinematics.has_twist = has_twist;
     if (has_variance) {
       obj.kinematics.has_twist_covariance = has_variance;
-      obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d);
+      obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d, yaw);
     }
   }
 }
@@ -113,12 +115,21 @@ std::array<double, 36> convertPoseCovarianceMatrix(const Box3D & box3d)
   return pose_covariance;
 }
 
-std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d)
+std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw)
 {
   using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+
+  // twist covariance matrix is based on the object coordinate system
   std::array<double, 36> twist_covariance{};
-  twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance;
-  twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance;
+  const float cos_yaw = std::cos(yaw);
+  const float sin_yaw = std::sin(yaw);
+  twist_covariance[POSE_IDX::X_X] =
+    box3d.vel_x_variance * cos_yaw * cos_yaw + box3d.vel_y_variance * sin_yaw * sin_yaw;
+  twist_covariance[POSE_IDX::X_Y] =
+    (box3d.vel_y_variance - box3d.vel_x_variance) * sin_yaw * cos_yaw;
+  twist_covariance[POSE_IDX::Y_X] = twist_covariance[POSE_IDX::X_Y];
+  twist_covariance[POSE_IDX::Y_Y] =
+    box3d.vel_x_variance * sin_yaw * sin_yaw + box3d.vel_y_variance * cos_yaw * cos_yaw;
   return twist_covariance;
 }
 

diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp
@@ -125,13 +125,14 @@ TEST(TestSuite, convertPoseCovarianceMatrix)
 TEST(TestSuite, convertTwistCovarianceMatrix)
 {
   autoware::lidar_centerpoint::Box3D box3d;
-  box3d.vel_x_variance = 0.1;
+  box3d.vel_x_variance = 0.5;
   box3d.vel_y_variance = 0.2;
+  float yaw = 0;
 
   std::array<double, 36> twist_covariance =
-    autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d);
+    autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d, yaw);
 
-  EXPECT_FLOAT_EQ(twist_covariance[0], 0.1);
+  EXPECT_FLOAT_EQ(twist_covariance[0], 0.5);
   EXPECT_FLOAT_EQ(twist_covariance[7], 0.2);
 }
 

diff --git a/...itor/schema/bluetooth_monitor.shcema.json → ...itor/schema/bluetooth_monitor.schema.json b/...itor/schema/bluetooth_monitor.shcema.json → ...itor/schema/bluetooth_monitor.schema.json
@@ -6,7 +6,7 @@
     "bluetooth_monitor": {
       "type": "object",
       "properties": {
-        "address": {
+        "addresses": {
           "type": "array",
           "description": "Bluetooth addresses of the device to monitor",
           "items": {
@@ -30,7 +30,7 @@
           "default": 0.1
         }
       },
-      "required": ["address", "port", "timeout", "rtt_warn"],
+      "required": ["addresses", "port", "timeout", "rtt_warn"],
       "additionalProperties": false
     }
   },