From 3aaa748421b0a4226e629d8a850c198597912122 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 27 Feb 2024 15:01:21 +0900 Subject: [PATCH 01/12] Lipsticks Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index de0152ff850..ac665dca722 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -124,11 +124,10 @@ auto DetectionSensor::apply return detected_object; } -unique_identifier_msgs::msg::UUID generateUUIDMsg(const std::string & input) +auto generateUUIDMsg(const std::string & input) -> unique_identifier_msgs::msg::UUID { static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); const auto uuid = generate_uuid(input); - unique_identifier_msgs::msg::UUID uuid_msg; std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); return uuid_msg; @@ -266,18 +265,16 @@ auto DetectionSensor::updat static std::queue> queue_objects; + static std::queue> queue_ground_truth_objects; - queue_objects.push(std::make_pair(msg, current_simulation_time)); - queue_ground_truth_objects.push(std::make_pair(ground_truth_msg, current_simulation_time)); + queue_objects.emplace(msg, current_simulation_time); - static rclcpp::Publisher::SharedPtr - ground_truth_publisher = std::dynamic_pointer_cast< - rclcpp::Publisher>( - ground_truth_publisher_base_ptr_); + queue_ground_truth_objects.emplace(ground_truth_msg, current_simulation_time); autoware_auto_perception_msgs::msg::DetectedObjects delayed_msg; + autoware_auto_perception_msgs::msg::TrackedObjects delayed_ground_truth_msg; if ( @@ -307,6 +304,11 @@ auto DetectionSensor::updat publisher_ptr_->publish(noised_msg); + static rclcpp::Publisher::SharedPtr + ground_truth_publisher = std::dynamic_pointer_cast< + rclcpp::Publisher>( + ground_truth_publisher_base_ptr_); + ground_truth_publisher->publish(delayed_ground_truth_msg); } } From 8f0c1c05b67b253438ffaa2ff33e6477916441a1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 27 Feb 2024 15:39:30 +0900 Subject: [PATCH 02/12] Move the DetectedObject construction to a new free function Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.cpp | 172 +++++++++--------- 1 file changed, 89 insertions(+), 83 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index ac665dca722..7c993cef8e6 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -133,12 +133,7 @@ auto generateUUIDMsg(const std::string & input) -> unique_identifier_msgs::msg:: return uuid_msg; } -template <> -auto DetectionSensor::update( - const double current_simulation_time, - const std::vector & statuses, - const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) - -> void +auto makeDetectedObject(const traffic_simulator_msgs::EntityStatus & status) { auto makeObjectClassification = [](const auto & label) { autoware_auto_perception_msgs::msg::ObjectClassification object_classification; @@ -147,6 +142,84 @@ auto DetectionSensor::updat return object_classification; }; + autoware_auto_perception_msgs::msg::DetectedObject object; + + switch (status.subtype().value()) { + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: + object.classification.push_back( + makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::CAR)); + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: + object.classification.push_back( + makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK)); + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: + object.classification.push_back( + makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::BUS)); + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER)); + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)); + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + break; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); + break; + default: + object.classification.push_back(makeObjectClassification( + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); + break; + } + + simulation_interface::toMsg(status.bounding_box().dimensions(), object.shape.dimensions); + + geometry_msgs::msg::Pose pose; + simulation_interface::toMsg(status.pose(), pose); + + auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); + + geometry_msgs::msg::Point center_point; + simulation_interface::toMsg(status.bounding_box().center(), center_point); + Eigen::Vector3d center(center_point.x, center_point.y, center_point.z); + center = rotation * center; + pose.position.x = pose.position.x + center.x(); + pose.position.y = pose.position.y + center.y(); + pose.position.z = pose.position.z + center.z(); + object.kinematics.pose_with_covariance.pose = pose; + object.kinematics.pose_with_covariance.covariance = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1}; + simulation_interface::toMsg( + status.action_status().twist(), object.kinematics.twist_with_covariance.twist); + + object.shape.type = object.shape.BOUNDING_BOX; + + return object; +} + +template <> +auto DetectionSensor::update( + const double current_simulation_time, + const std::vector & statuses, + const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) + -> void +{ if ( current_simulation_time - previous_simulation_time_ - configuration_.update_duration() >= -0.002) { @@ -170,69 +243,7 @@ auto DetectionSensor::updat std::find(detected_objects.begin(), detected_objects.end(), status.name()) != detected_objects.end() and status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { - autoware_auto_perception_msgs::msg::DetectedObject object; - switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); - break; - default: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - } - - simulation_interface::toMsg(status.bounding_box().dimensions(), object.shape.dimensions); - geometry_msgs::msg::Pose pose; - simulation_interface::toMsg(status.pose(), pose); - auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); - geometry_msgs::msg::Point center_point; - simulation_interface::toMsg(status.bounding_box().center(), center_point); - Eigen::Vector3d center(center_point.x, center_point.y, center_point.z); - center = rotation * center; - pose.position.x = pose.position.x + center.x(); - pose.position.y = pose.position.y + center.y(); - pose.position.z = pose.position.z + center.z(); - object.kinematics.pose_with_covariance.pose = pose; - object.kinematics.pose_with_covariance.covariance = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1}; - simulation_interface::toMsg( - status.action_status().twist(), object.kinematics.twist_with_covariance.twist); - object.shape.type = object.shape.BOUNDING_BOX; - + const auto object = makeDetectedObject(status); msg.objects.push_back(object); // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp @@ -242,20 +253,15 @@ auto DetectionSensor::updat const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) -> autoware_auto_perception_msgs::msg::TrackedObject { autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - tracked_object.existence_probability = detected_object.existence_probability; - - tracked_object.classification = detected_object.classification; - - tracked_object.kinematics.pose_with_covariance = - detected_object.kinematics.pose_with_covariance; - tracked_object.kinematics.twist_with_covariance = - detected_object.kinematics.twist_with_covariance; - tracked_object.kinematics.orientation_availability = - detected_object.kinematics.orientation_availability; - - tracked_object.shape = detected_object.shape; - tracked_object.object_id = generateUUIDMsg(name); - + // clang-format off + tracked_object.classification = detected_object.classification; + tracked_object.existence_probability = detected_object.existence_probability; + tracked_object.kinematics.orientation_availability = detected_object.kinematics.orientation_availability; + tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = detected_object.kinematics.twist_with_covariance; + tracked_object.object_id = generateUUIDMsg(name); + tracked_object.shape = detected_object.shape; + // clang-format on return tracked_object; }; From f5642a2a29be12d8508950b80f5e24c42b1ebada Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 27 Feb 2024 17:21:54 +0900 Subject: [PATCH 03/12] Split ROS message type object construction into some free functions Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.cpp | 235 +++++++++++------- 1 file changed, 142 insertions(+), 93 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 7c993cef8e6..8770273abae 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -124,95 +124,163 @@ auto DetectionSensor::apply return detected_object; } -auto generateUUIDMsg(const std::string & input) -> unique_identifier_msgs::msg::UUID +template +auto make(From &&...) -> To; + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> unique_identifier_msgs::msg::UUID { static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); - const auto uuid = generate_uuid(input); - unique_identifier_msgs::msg::UUID uuid_msg; - std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); - return uuid_msg; + const auto uuid = generate_uuid(status.name()); + unique_identifier_msgs::msg::UUID message; + std::copy(uuid.begin(), uuid.end(), message.uuid.begin()); + return message; } -auto makeDetectedObject(const traffic_simulator_msgs::EntityStatus & status) +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::ObjectClassification { - auto makeObjectClassification = [](const auto & label) { - autoware_auto_perception_msgs::msg::ObjectClassification object_classification; - object_classification.label = label; - object_classification.probability = 1; - return object_classification; - }; + auto object_classification = autoware_auto_perception_msgs::msg::ObjectClassification(); + + object_classification.label = [&]() { + switch (status.subtype().value()) { + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: + return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: + return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: + return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: + return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: + return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: + return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: + return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + default: + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + } + }(); - autoware_auto_perception_msgs::msg::DetectedObject object; - - switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: - object.classification.push_back( - makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::CAR)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: - object.classification.push_back( - makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: - object.classification.push_back( - makeObjectClassification(autoware_auto_perception_msgs::msg::ObjectClassification::BUS)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); - break; - default: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - } + object_classification.probability = 1; - simulation_interface::toMsg(status.bounding_box().dimensions(), object.shape.dimensions); + return object_classification; +} - geometry_msgs::msg::Pose pose; +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs::msg::Pose +{ + auto pose = geometry_msgs::msg::Pose(); simulation_interface::toMsg(status.pose(), pose); - auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); - - geometry_msgs::msg::Point center_point; + auto center_point = geometry_msgs::msg::Point(); simulation_interface::toMsg(status.bounding_box().center(), center_point); - Eigen::Vector3d center(center_point.x, center_point.y, center_point.z); - center = rotation * center; + + Eigen::Vector3d center = quaternion_operation::getRotationMatrix(pose.orientation) * + Eigen::Vector3d(center_point.x, center_point.y, center_point.z); + pose.position.x = pose.position.x + center.x(); pose.position.y = pose.position.y + center.y(); pose.position.z = pose.position.z + center.z(); - object.kinematics.pose_with_covariance.pose = pose; - object.kinematics.pose_with_covariance.covariance = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1}; - simulation_interface::toMsg( - status.action_status().twist(), object.kinematics.twist_with_covariance.twist); - object.shape.type = object.shape.BOUNDING_BOX; + return pose; +} + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs::msg::Twist +{ + auto twist = geometry_msgs::msg::Twist(); + simulation_interface::toMsg(status.action_status().twist(), twist); + return twist; +} + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::DetectedObjectKinematics +{ + auto kinematics = autoware_auto_perception_msgs::msg::DetectedObjectKinematics(); + + kinematics.pose_with_covariance.pose = make(status); + + // clang-format off + kinematics.pose_with_covariance.covariance = { + /* + Row-major representation of the 6x6 covariance matrix. The orientation + parameters use a fixed-axis representation. In order, the parameters + are: (x, y, z, rotation about X axis, rotation about Y axis, rotation + about Z axis) + */ + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + }; + // clang-format on + + kinematics.twist_with_covariance.twist = make(status); + + kinematics.orientation_availability = [&]() { + switch (status.subtype().value()) { + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: + case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: + return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + default: + return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + } + }(); + + return kinematics; +} - return object; +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::Shape +{ + auto shape = autoware_auto_perception_msgs::msg::Shape(); + simulation_interface::toMsg(status.bounding_box().dimensions(), shape.dimensions); + shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + return shape; } +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::DetectedObject +{ + auto detected_object = autoware_auto_perception_msgs::msg::DetectedObject(); + // clang-format off + detected_object.classification.push_back(make(status)); + detected_object.kinematics = make(status); + detected_object.shape = make(status); + // clang-format on + return detected_object; +} + +template <> +auto make( + const traffic_simulator_msgs::EntityStatus & status, + const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) + -> autoware_auto_perception_msgs::msg::TrackedObject +{ + // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp + auto tracked_object = autoware_auto_perception_msgs::msg::TrackedObject(); + // clang-format off + tracked_object.object_id = make(status); + tracked_object.existence_probability = detected_object.existence_probability; + tracked_object.classification = detected_object.classification; + tracked_object.kinematics.orientation_availability = detected_object.kinematics.orientation_availability; + tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = detected_object.kinematics.twist_with_covariance; + tracked_object.shape = detected_object.shape; + // clang-format on + return tracked_object; +}; + template <> auto DetectionSensor::update( const double current_simulation_time, @@ -243,29 +311,10 @@ auto DetectionSensor::updat std::find(detected_objects.begin(), detected_objects.end(), status.name()) != detected_objects.end() and status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { - const auto object = makeDetectedObject(status); + const auto object = make(status); msg.objects.push_back(object); - - // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp - static auto toTrackedObject = - [&]( - const std::string & name, - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) - -> autoware_auto_perception_msgs::msg::TrackedObject { - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - // clang-format off - tracked_object.classification = detected_object.classification; - tracked_object.existence_probability = detected_object.existence_probability; - tracked_object.kinematics.orientation_availability = detected_object.kinematics.orientation_availability; - tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; - tracked_object.kinematics.twist_with_covariance = detected_object.kinematics.twist_with_covariance; - tracked_object.object_id = generateUUIDMsg(name); - tracked_object.shape = detected_object.shape; - // clang-format on - return tracked_object; - }; - - ground_truth_msg.objects.push_back(toTrackedObject(status.name(), object)); + ground_truth_msg.objects.push_back( + make(status, object)); } } From 6ff4898485a4efb4e80f8290f51bf05038cec89e Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 27 Feb 2024 18:27:57 +0900 Subject: [PATCH 04/12] Add type U for ground truth to the template parameter of DetectionSensor Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.hpp | 8 ++++---- .../detection_sensor/detection_sensor.cpp | 9 +-------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 3917feae62a..c045a514d8f 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -67,12 +67,12 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; }; -template +template class DetectionSensor : public DetectionSensorBase { const typename rclcpp::Publisher::SharedPtr publisher_ptr_; - typename rclcpp::PublisherBase::SharedPtr ground_truth_publisher_base_ptr_; + const typename rclcpp::Publisher::SharedPtr ground_truth_publisher_; std::mt19937 random_engine_; @@ -84,10 +84,10 @@ class DetectionSensor : public DetectionSensorBase const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration, const typename rclcpp::Publisher::SharedPtr & publisher, - const typename rclcpp::PublisherBase::SharedPtr & ground_truth_publisher = nullptr) + const typename rclcpp::Publisher::SharedPtr & ground_truth_publisher = nullptr) : DetectionSensorBase(current_simulation_time, configuration), publisher_ptr_(publisher), - ground_truth_publisher_base_ptr_(ground_truth_publisher), + ground_truth_publisher_(ground_truth_publisher), random_engine_(configuration.random_seed()) { } diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 8770273abae..b997912b8c8 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -145,8 +145,6 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) object_classification.label = [&]() { switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: @@ -359,12 +357,7 @@ auto DetectionSensor::updat publisher_ptr_->publish(noised_msg); - static rclcpp::Publisher::SharedPtr - ground_truth_publisher = std::dynamic_pointer_cast< - rclcpp::Publisher>( - ground_truth_publisher_base_ptr_); - - ground_truth_publisher->publish(delayed_ground_truth_msg); + ground_truth_publisher_->publish(delayed_ground_truth_msg); } } } // namespace simple_sensor_simulator From 640f90ba0914a2c80c833482cd631526e151cf0f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 29 Feb 2024 14:41:09 +0900 Subject: [PATCH 05/12] Move the process of applying noise to a new structure `DefaultNoise` Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.hpp | 2 +- .../detection_sensor/detection_sensor.cpp | 87 ++++++++++++------- 2 files changed, 56 insertions(+), 33 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index c045a514d8f..824ffa02bdd 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -74,7 +74,7 @@ class DetectionSensor : public DetectionSensorBase const typename rclcpp::Publisher::SharedPtr ground_truth_publisher_; - std::mt19937 random_engine_; + std::default_random_engine random_engine_; auto applyPositionNoise(typename T::_objects_type::value_type) -> typename T::_objects_type::value_type; diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index b997912b8c8..fd85fc3c0a9 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -279,6 +280,50 @@ auto make( return tracked_object; }; +struct Noise +{ + std::default_random_engine & random_engine; + + const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration; + + explicit Noise( + std::default_random_engine & random_engine, + const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration) + : random_engine(random_engine), detection_sensor_configuration(detection_sensor_configuration) + { + } +}; + +struct DefaultNoise : public Noise +{ + using Noise::Noise; + + auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) + -> decltype(auto) + { + auto position_noise_distribution = + std::normal_distribution<>(0.0, detection_sensor_configuration.pos_noise_stddev()); + + for (auto && detected_object : detected_objects.objects) { + detected_object.kinematics.pose_with_covariance.pose.position.x += + position_noise_distribution(random_engine); + detected_object.kinematics.pose_with_covariance.pose.position.y += + position_noise_distribution(random_engine); + } + + detected_objects.objects.erase( + std::remove_if( + detected_objects.objects.begin(), detected_objects.objects.end(), + [this](auto &&) { + return std::uniform_real_distribution()(random_engine) < + detection_sensor_configuration.probability_of_lost(); + }), + detected_objects.objects.end()); + + return detected_objects; + } +}; + template <> auto DetectionSensor::update( const double current_simulation_time, @@ -289,7 +334,7 @@ auto DetectionSensor::updat if ( current_simulation_time - previous_simulation_time_ - configuration_.update_duration() >= -0.002) { - const std::vector detected_objects = filterObjectsBySensorRange( + const auto detected_objects = filterObjectsBySensorRange( statuses, configuration_.detect_all_objects_in_range() ? getDetectedObjects(statuses) : lidar_detected_entities, @@ -322,42 +367,20 @@ auto DetectionSensor::updat static std::queue> queue_ground_truth_objects; - queue_objects.emplace(msg, current_simulation_time); - - queue_ground_truth_objects.emplace(ground_truth_msg, current_simulation_time); - - autoware_auto_perception_msgs::msg::DetectedObjects delayed_msg; - - autoware_auto_perception_msgs::msg::TrackedObjects delayed_ground_truth_msg; - - if ( - current_simulation_time - queue_objects.front().second >= - configuration_.object_recognition_delay()) { - delayed_msg = queue_objects.front().first; - delayed_ground_truth_msg = queue_ground_truth_objects.front().first; + if (queue_objects.emplace(msg, current_simulation_time); + current_simulation_time - queue_objects.front().second >= + configuration_.object_recognition_delay()) { + publisher_ptr_->publish( + DefaultNoise(random_engine_, configuration_)(queue_objects.front().first)); queue_objects.pop(); } - if ( - current_simulation_time - queue_ground_truth_objects.front().second >= - configuration_.object_recognition_ground_truth_delay()) { - delayed_ground_truth_msg = queue_ground_truth_objects.front().first; + if (queue_ground_truth_objects.emplace(ground_truth_msg, current_simulation_time); + current_simulation_time - queue_ground_truth_objects.front().second >= + configuration_.object_recognition_ground_truth_delay()) { + ground_truth_publisher_->publish(queue_ground_truth_objects.front().first); queue_ground_truth_objects.pop(); } - - autoware_auto_perception_msgs::msg::DetectedObjects noised_msg; - noised_msg.header = delayed_msg.header; - noised_msg.objects.reserve(delayed_msg.objects.size()); - for (const auto & object : delayed_msg.objects) { - if (auto probability_of_lost = std::uniform_real_distribution(); - probability_of_lost(random_engine_) > configuration_.probability_of_lost()) { - noised_msg.objects.push_back(applyPositionNoise(object)); - } - } - - publisher_ptr_->publish(noised_msg); - - ground_truth_publisher_->publish(delayed_ground_truth_msg); } } } // namespace simple_sensor_simulator From d738fc83813ba2e99f375a02ce7ea9965aa56de7 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 29 Feb 2024 15:25:52 +0900 Subject: [PATCH 06/12] Rename variables to appropriate for the current class content Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.hpp | 14 ++++-- .../detection_sensor/detection_sensor.cpp | 50 +++++++++---------- 2 files changed, 33 insertions(+), 31 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 824ffa02bdd..5f0d867cf2d 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -70,12 +70,18 @@ class DetectionSensorBase template class DetectionSensor : public DetectionSensorBase { - const typename rclcpp::Publisher::SharedPtr publisher_ptr_; + const typename rclcpp::Publisher::SharedPtr detected_objects_publisher; - const typename rclcpp::Publisher::SharedPtr ground_truth_publisher_; + const typename rclcpp::Publisher::SharedPtr ground_truth_objects_publisher; std::default_random_engine random_engine_; + std::queue> + detected_objects_queue; + + std::queue> + ground_truth_objects_queue; + auto applyPositionNoise(typename T::_objects_type::value_type) -> typename T::_objects_type::value_type; @@ -86,8 +92,8 @@ class DetectionSensor : public DetectionSensorBase const typename rclcpp::Publisher::SharedPtr & publisher, const typename rclcpp::Publisher::SharedPtr & ground_truth_publisher = nullptr) : DetectionSensorBase(current_simulation_time, configuration), - publisher_ptr_(publisher), - ground_truth_publisher_(ground_truth_publisher), + detected_objects_publisher(publisher), + ground_truth_objects_publisher(ground_truth_publisher), random_engine_(configuration.random_seed()) { } diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index fd85fc3c0a9..1609392d266 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -334,52 +334,48 @@ auto DetectionSensor::updat if ( current_simulation_time - previous_simulation_time_ - configuration_.update_duration() >= -0.002) { - const auto detected_objects = filterObjectsBySensorRange( + const auto detected_objects_in_range = filterObjectsBySensorRange( statuses, configuration_.detect_all_objects_in_range() ? getDetectedObjects(statuses) : lidar_detected_entities, configuration_.range()); - autoware_auto_perception_msgs::msg::DetectedObjects msg; - msg.header.stamp = current_ros_time; - msg.header.frame_id = "map"; + autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + detected_objects.header.stamp = current_ros_time; + detected_objects.header.frame_id = "map"; - autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_msg; - ground_truth_msg.header = msg.header; + autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; + ground_truth_objects.header = detected_objects.header; previous_simulation_time_ = current_simulation_time; for (const auto & status : statuses) { if ( - std::find(detected_objects.begin(), detected_objects.end(), status.name()) != - detected_objects.end() and + std::find( + detected_objects_in_range.begin(), detected_objects_in_range.end(), status.name()) != + detected_objects_in_range.end() and status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { - const auto object = make(status); - msg.objects.push_back(object); - ground_truth_msg.objects.push_back( - make(status, object)); + const auto detected_object = + make(status); + detected_objects.objects.push_back(detected_object); + ground_truth_objects.objects.push_back( + make(status, detected_object)); } } - static std::queue> - queue_objects; - - static std::queue> - queue_ground_truth_objects; - - if (queue_objects.emplace(msg, current_simulation_time); - current_simulation_time - queue_objects.front().second >= + if (detected_objects_queue.emplace(detected_objects, current_simulation_time); + current_simulation_time - detected_objects_queue.front().second >= configuration_.object_recognition_delay()) { - publisher_ptr_->publish( - DefaultNoise(random_engine_, configuration_)(queue_objects.front().first)); - queue_objects.pop(); + detected_objects_publisher->publish( + DefaultNoise(random_engine_, configuration_)(detected_objects_queue.front().first)); + detected_objects_queue.pop(); } - if (queue_ground_truth_objects.emplace(ground_truth_msg, current_simulation_time); - current_simulation_time - queue_ground_truth_objects.front().second >= + if (ground_truth_objects_queue.emplace(ground_truth_objects, current_simulation_time); + current_simulation_time - ground_truth_objects_queue.front().second >= configuration_.object_recognition_ground_truth_delay()) { - ground_truth_publisher_->publish(queue_ground_truth_objects.front().first); - queue_ground_truth_objects.pop(); + ground_truth_objects_publisher->publish(ground_truth_objects_queue.front().first); + ground_truth_objects_queue.pop(); } } } From 9d9dd534b82545149d6a5e7691f0a33004c0c157 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 29 Feb 2024 17:16:20 +0900 Subject: [PATCH 07/12] Add new struct `CustomNoiseApplicator` Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.cpp | 56 ++++++++++++++----- 1 file changed, 41 insertions(+), 15 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 1609392d266..54798b6a714 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -280,23 +280,26 @@ auto make( return tracked_object; }; -struct Noise +struct DefaultNoiseApplicator { + const double current_simulation_time; + + const rclcpp::Time & current_ros_time; + std::default_random_engine & random_engine; const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration; - explicit Noise( + explicit DefaultNoiseApplicator( + double current_simulation_time, const rclcpp::Time & current_ros_time, std::default_random_engine & random_engine, const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration) - : random_engine(random_engine), detection_sensor_configuration(detection_sensor_configuration) + : current_simulation_time(current_simulation_time), + current_ros_time(current_ros_time), + random_engine(random_engine), + detection_sensor_configuration(detection_sensor_configuration) { } -}; - -struct DefaultNoise : public Noise -{ - using Noise::Noise; auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) -> decltype(auto) @@ -324,6 +327,28 @@ struct DefaultNoise : public Noise } }; +struct CustomNoiseApplicator : public DefaultNoiseApplicator +{ + using DefaultNoiseApplicator::DefaultNoiseApplicator; + + /* + NOTE: for Autoware developers + + If you need to apply experimental noise to the DetectedObjects that the + simulator publishes, comment out the following member functions and + implement them. + + See class DefaultNoiseApplicator for the default noise implementation. + This class inherits from DefaultNoiseApplicator, so you can use its data + members, or you can explicitly call DefaultNoiseApplicator::operator(). + */ + // auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_object) + // -> decltype(auto) + // { + // return detected_objects; + // } +}; + template <> auto DetectionSensor::update( const double current_simulation_time, @@ -334,7 +359,9 @@ auto DetectionSensor::updat if ( current_simulation_time - previous_simulation_time_ - configuration_.update_duration() >= -0.002) { - const auto detected_objects_in_range = filterObjectsBySensorRange( + previous_simulation_time_ = current_simulation_time; + + const auto detected_object_names_in_range = filterObjectsBySensorRange( statuses, configuration_.detect_all_objects_in_range() ? getDetectedObjects(statuses) : lidar_detected_entities, @@ -347,13 +374,11 @@ auto DetectionSensor::updat autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; ground_truth_objects.header = detected_objects.header; - previous_simulation_time_ = current_simulation_time; - for (const auto & status : statuses) { if ( std::find( - detected_objects_in_range.begin(), detected_objects_in_range.end(), status.name()) != - detected_objects_in_range.end() and + detected_object_names_in_range.begin(), detected_object_names_in_range.end(), + status.name()) != detected_object_names_in_range.end() and status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { const auto detected_object = make(status); @@ -366,8 +391,9 @@ auto DetectionSensor::updat if (detected_objects_queue.emplace(detected_objects, current_simulation_time); current_simulation_time - detected_objects_queue.front().second >= configuration_.object_recognition_delay()) { - detected_objects_publisher->publish( - DefaultNoise(random_engine_, configuration_)(detected_objects_queue.front().first)); + auto apply_noise = CustomNoiseApplicator( + current_simulation_time, current_ros_time, random_engine_, configuration_); + detected_objects_publisher->publish(apply_noise(detected_objects_queue.front().first)); detected_objects_queue.pop(); } From 3997537f4ea650aa1ce4f25299745a63c3472038 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 1 Mar 2024 07:42:55 +0900 Subject: [PATCH 08/12] Simplify complex and inefficient function `filterObjectsBySensorRange` Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.hpp | 23 +-- .../detection_sensor/detection_sensor.cpp | 143 +++++------------- 2 files changed, 46 insertions(+), 120 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 5f0d867cf2d..90b9d9e3d6c 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -41,22 +41,12 @@ class DetectionSensorBase { } - auto isWithinRange( - const geometry_msgs::Point & point1, const geometry_msgs::Point & point2, - const double range) const -> bool; + auto isTheEntityStatusToWhichThisSensorIsAttached( + const traffic_simulator_msgs::EntityStatus &) const -> bool; - auto filterObjectsBySensorRange( - const std::vector &, const std::vector &, - const double) const -> std::vector; - - auto getEntityPose(const std::vector &, const std::string &) - const -> geometry_msgs::Pose; - - auto getDetectedObjects(const std::vector &) const - -> std::vector; - - auto getSensorPose(const std::vector &) const - -> geometry_msgs::Pose; + auto findTheEntityStatusToWhichThisSensorIsAttached( + const std::vector &) const + -> std::vector::const_iterator; public: virtual ~DetectionSensorBase() = default; @@ -82,9 +72,6 @@ class DetectionSensor : public DetectionSensorBase std::queue> ground_truth_objects_queue; - auto applyPositionNoise(typename T::_objects_type::value_type) -> - typename T::_objects_type::value_type; - public: explicit DetectionSensor( const double current_simulation_time, diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 54798b6a714..9c6db9fc196 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -32,97 +32,35 @@ namespace simple_sensor_simulator { -auto DetectionSensorBase::isWithinRange( - const geometry_msgs::Point & point1, const geometry_msgs::Point & point2, - const double range) const -> bool +auto distance(const geometry_msgs::Pose & pose1, const geometry_msgs::Pose & pose2) { - auto distanceX = point1.x() - point2.x(); - auto distanceY = point1.y() - point2.y(); - auto distanceZ = point1.z() - point2.z(); - - double distance = std::hypot(distanceX, distanceY, distanceZ); - return distance <= range; -} - -auto DetectionSensorBase::getSensorPose( - const std::vector & statuses) const -> geometry_msgs::Pose -{ - for (const auto & status : statuses) { - if ( - status.type().type() == traffic_simulator_msgs::EntityType::EGO && - status.name() == configuration_.entity()) { - return status.pose(); - } - } - throw SimulationRuntimeError("Detection sensor can be attached only ego entity."); + return std::hypot( + pose1.position().x() - pose2.position().x(), // + pose1.position().y() - pose2.position().y(), // + pose1.position().z() - pose2.position().z()); } -auto DetectionSensorBase::getEntityPose( - const std::vector & entity_statuses, - const std::string & entity_string) const -> geometry_msgs::Pose +auto DetectionSensorBase::isTheEntityStatusToWhichThisSensorIsAttached( + const traffic_simulator_msgs::EntityStatus & status) const -> bool { - for (const auto & entity_status : entity_statuses) { - if (entity_status.name() == entity_string) { - return entity_status.pose(); - } - } - - throw SimulationRuntimeError( - configuration_.detect_all_objects_in_range() - ? "Filtered object is not includes in entity statuses" - : "Detected object by lidar sensor is not included in lidar detected entity"); + return status.name() == configuration_.entity() and + status.type().type() == traffic_simulator_msgs::EntityType::EGO; } -auto DetectionSensorBase::getDetectedObjects( +auto DetectionSensorBase::findTheEntityStatusToWhichThisSensorIsAttached( const std::vector & statuses) const - -> std::vector -{ - std::vector detected_objects; - const auto pose = getSensorPose(statuses); - - for (const auto & status : statuses) { - if ( - status.name() != configuration_.entity() && - isWithinRange(status.pose().position(), pose.position(), 300.0)) { - detected_objects.emplace_back(status.name()); - } - } - - return detected_objects; -} - -auto DetectionSensorBase::filterObjectsBySensorRange( - const std::vector & entity_statuses, - const std::vector & selected_entity_strings, - const double detection_sensor_range) const -> std::vector + -> std::vector::const_iterator { - std::vector detected_objects; - const auto sensor_pose = getSensorPose(entity_statuses); - - for (const auto & selected_entity_status : selected_entity_strings) { - const auto selected_entity_pose = getEntityPose(entity_statuses, selected_entity_status); - if ( - selected_entity_status != configuration_.entity() && - isWithinRange( - selected_entity_pose.position(), sensor_pose.position(), detection_sensor_range)) { - detected_objects.emplace_back(selected_entity_status); - } + if (auto iter = std::find_if( + statuses.begin(), statuses.end(), + [this](const auto & status) { + return isTheEntityStatusToWhichThisSensorIsAttached(status); + }); + iter != statuses.end()) { + return iter; + } else { + throw SimulationRuntimeError("Detection sensor can be attached only ego entity."); } - return detected_objects; -} - -template <> -auto DetectionSensor::applyPositionNoise( - autoware_auto_perception_msgs::msg::DetectedObject detected_object) - -> autoware_auto_perception_msgs::msg::DetectedObject -{ - auto position_noise_distribution = - std::normal_distribution<>(0.0, configuration_.pos_noise_stddev()); - detected_object.kinematics.pose_with_covariance.pose.position.x += - position_noise_distribution(random_engine_); - detected_object.kinematics.pose_with_covariance.pose.position.y += - position_noise_distribution(random_engine_); - return detected_object; } template @@ -146,19 +84,19 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) object_classification.label = [&]() { switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: + case traffic_simulator_msgs::EntitySubtype::CAR: return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: + case traffic_simulator_msgs::EntitySubtype::TRUCK: return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: + case traffic_simulator_msgs::EntitySubtype::BUS: return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: + case traffic_simulator_msgs::EntitySubtype::TRAILER: return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: + case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: + case traffic_simulator_msgs::EntitySubtype::BICYCLE: return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: + case traffic_simulator_msgs::EntitySubtype::PEDESTRIAN: return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; default: return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; @@ -226,8 +164,8 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) kinematics.orientation_availability = [&]() { switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: + case traffic_simulator_msgs::EntitySubtype::BICYCLE: + case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; default: return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; @@ -361,12 +299,6 @@ auto DetectionSensor::updat -0.002) { previous_simulation_time_ = current_simulation_time; - const auto detected_object_names_in_range = filterObjectsBySensorRange( - statuses, - configuration_.detect_all_objects_in_range() ? getDetectedObjects(statuses) - : lidar_detected_entities, - configuration_.range()); - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header.stamp = current_ros_time; detected_objects.header.frame_id = "map"; @@ -374,12 +306,19 @@ auto DetectionSensor::updat autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; ground_truth_objects.header = detected_objects.header; + auto is_in_range = [&, ego_entity_status = findTheEntityStatusToWhichThisSensorIsAttached( + statuses)](const auto & status) { + return not isTheEntityStatusToWhichThisSensorIsAttached(status) and + distance(status.pose(), ego_entity_status->pose()) <= + std::min(configuration_.range(), 300.0) and + (configuration_.detect_all_objects_in_range() or + std::find( + lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) != + lidar_detected_entities.end()); + }; + for (const auto & status : statuses) { - if ( - std::find( - detected_object_names_in_range.begin(), detected_object_names_in_range.end(), - status.name()) != detected_object_names_in_range.end() and - status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { + if (is_in_range(status)) { const auto detected_object = make(status); detected_objects.objects.push_back(detected_object); From 7eb9ae5ba21a35f64cef1404f0d76037d633aeca Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 1 Mar 2024 08:04:03 +0900 Subject: [PATCH 09/12] Update `DefaultNoiseApplicator` to hold reference to Ego entity's status Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.cpp | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 9c6db9fc196..1788b3ba0dc 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -190,11 +190,11 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) -> autoware_auto_perception_msgs::msg::DetectedObject { auto detected_object = autoware_auto_perception_msgs::msg::DetectedObject(); - // clang-format off - detected_object.classification.push_back(make(status)); - detected_object.kinematics = make(status); - detected_object.shape = make(status); - // clang-format on + detected_object.classification.push_back( + make(status)); + detected_object.kinematics = + make(status); + detected_object.shape = make(status); return detected_object; } @@ -224,21 +224,33 @@ struct DefaultNoiseApplicator const rclcpp::Time & current_ros_time; + const traffic_simulator_msgs::EntityStatus & ego_entity_status; + std::default_random_engine & random_engine; const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration; explicit DefaultNoiseApplicator( double current_simulation_time, const rclcpp::Time & current_ros_time, + const traffic_simulator_msgs::EntityStatus & ego_entity_status, std::default_random_engine & random_engine, const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration) : current_simulation_time(current_simulation_time), current_ros_time(current_ros_time), + ego_entity_status(ego_entity_status), random_engine(random_engine), detection_sensor_configuration(detection_sensor_configuration) { } + DefaultNoiseApplicator(const DefaultNoiseApplicator &) = delete; + + DefaultNoiseApplicator(DefaultNoiseApplicator &&) = delete; + + auto operator=(const DefaultNoiseApplicator &) = delete; + + auto operator=(DefaultNoiseApplicator &&) = delete; + auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) -> decltype(auto) { @@ -306,8 +318,9 @@ auto DetectionSensor::updat autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; ground_truth_objects.header = detected_objects.header; - auto is_in_range = [&, ego_entity_status = findTheEntityStatusToWhichThisSensorIsAttached( - statuses)](const auto & status) { + const auto ego_entity_status = findTheEntityStatusToWhichThisSensorIsAttached(statuses); + + auto is_in_range = [&](const auto & status) { return not isTheEntityStatusToWhichThisSensorIsAttached(status) and distance(status.pose(), ego_entity_status->pose()) <= std::min(configuration_.range(), 300.0) and @@ -331,7 +344,8 @@ auto DetectionSensor::updat current_simulation_time - detected_objects_queue.front().second >= configuration_.object_recognition_delay()) { auto apply_noise = CustomNoiseApplicator( - current_simulation_time, current_ros_time, random_engine_, configuration_); + current_simulation_time, current_ros_time, *ego_entity_status, random_engine_, + configuration_); detected_objects_publisher->publish(apply_noise(detected_objects_queue.front().first)); detected_objects_queue.pop(); } From 33e4dd7f0f56ac92755e4c5fa24d13292e0b9495 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 1 Mar 2024 08:18:48 +0900 Subject: [PATCH 10/12] Rename function argument `detected_object` to `detected_objects` Signed-off-by: yamacir-kit --- .../src/sensor_simulation/detection_sensor/detection_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 1788b3ba0dc..e43f422ce21 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -292,7 +292,7 @@ struct CustomNoiseApplicator : public DefaultNoiseApplicator This class inherits from DefaultNoiseApplicator, so you can use its data members, or you can explicitly call DefaultNoiseApplicator::operator(). */ - // auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_object) + // auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) // -> decltype(auto) // { // return detected_objects; From d936a209dfe95ca000d9c2c42eb4742f51f3a021 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 1 Mar 2024 08:32:11 +0900 Subject: [PATCH 11/12] Replace the word `TheEntity` with `EgoEntity` Signed-off-by: yamacir-kit --- .../detection_sensor/detection_sensor.hpp | 4 ++-- .../detection_sensor/detection_sensor.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index 90b9d9e3d6c..eb477a9d661 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -41,10 +41,10 @@ class DetectionSensorBase { } - auto isTheEntityStatusToWhichThisSensorIsAttached( + auto isEgoEntityStatusToWhichThisSensorIsAttached( const traffic_simulator_msgs::EntityStatus &) const -> bool; - auto findTheEntityStatusToWhichThisSensorIsAttached( + auto findEgoEntityStatusToWhichThisSensorIsAttached( const std::vector &) const -> std::vector::const_iterator; diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index e43f422ce21..83c65b36114 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -40,21 +40,21 @@ auto distance(const geometry_msgs::Pose & pose1, const geometry_msgs::Pose & pos pose1.position().z() - pose2.position().z()); } -auto DetectionSensorBase::isTheEntityStatusToWhichThisSensorIsAttached( +auto DetectionSensorBase::isEgoEntityStatusToWhichThisSensorIsAttached( const traffic_simulator_msgs::EntityStatus & status) const -> bool { return status.name() == configuration_.entity() and status.type().type() == traffic_simulator_msgs::EntityType::EGO; } -auto DetectionSensorBase::findTheEntityStatusToWhichThisSensorIsAttached( +auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( const std::vector & statuses) const -> std::vector::const_iterator { if (auto iter = std::find_if( statuses.begin(), statuses.end(), [this](const auto & status) { - return isTheEntityStatusToWhichThisSensorIsAttached(status); + return isEgoEntityStatusToWhichThisSensorIsAttached(status); }); iter != statuses.end()) { return iter; @@ -318,10 +318,10 @@ auto DetectionSensor::updat autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; ground_truth_objects.header = detected_objects.header; - const auto ego_entity_status = findTheEntityStatusToWhichThisSensorIsAttached(statuses); + const auto ego_entity_status = findEgoEntityStatusToWhichThisSensorIsAttached(statuses); auto is_in_range = [&](const auto & status) { - return not isTheEntityStatusToWhichThisSensorIsAttached(status) and + return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and distance(status.pose(), ego_entity_status->pose()) <= std::min(configuration_.range(), 300.0) and (configuration_.detect_all_objects_in_range() or From fc83e5ba0e151f9b568907510a51ccd678f649c6 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 1 Mar 2024 10:44:52 +0900 Subject: [PATCH 12/12] Remove the upper limit `300.0` of the detection sensor range Signed-off-by: yamacir-kit --- .../sensor_simulation/detection_sensor/detection_sensor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 83c65b36114..b7f32b6b57b 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -322,8 +322,7 @@ auto DetectionSensor::updat auto is_in_range = [&](const auto & status) { return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and - distance(status.pose(), ego_entity_status->pose()) <= - std::min(configuration_.range(), 300.0) and + distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and (configuration_.detect_all_objects_in_range() or std::find( lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) !=