diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index ec53a677cdd81..34a7453b51726 100644 --- 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 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 & 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 AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - std::vector polygons; autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } + std::vector 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 & 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 & 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 index de901b06e83c3..9a17cad91efb8 100644 --- 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 convertPoseCovarianceMatrix(const Box3D & box3d); -std::array convertTwistCovarianceMatrix(const Box3D & box3d); +std::array 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 index fc383a8d1da00..feeab969e88bd 100644 --- 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 convertPoseCovarianceMatrix(const Box3D & box3d) return pose_covariance; } -std::array convertTwistCovarianceMatrix(const Box3D & box3d) +std::array 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 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 index 555d820ec3644..5605d2df6a9d9 100644 --- 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 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/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json similarity index 93% rename from system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json rename to system/bluetooth_monitor/schema/bluetooth_monitor.schema.json index 6951ecd6aed88..0914c7ec9a9b9 100644 --- a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json +++ b/system/bluetooth_monitor/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 } },