From 4fa5ea4c7e792ad346d05ebeb6dcdfad8e6ef596 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 30 Mar 2022 14:51:02 +0900 Subject: [PATCH 1/5] Add Perceived Object Inclusion Management logic --- src/cpm_application.cpp | 313 +++++++++++++++++++++++++++++----------- 1 file changed, 229 insertions(+), 84 deletions(-) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index e1df7d4..ca728ac 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -37,10 +37,12 @@ namespace v2x { runtime_(rt), ego_(), generationDeltaTime_(0), - updating_objects_stack_(false), + updating_objects_list_(false), sending_(false), is_sender_(is_sender), - reflect_packet_(false) + reflect_packet_(false), + objectConfidenceThreshold_(0.0), + include_all_persons_and_animals_(false) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); set_interval(milliseconds(100)); @@ -206,14 +208,32 @@ namespace v2x { ego_.heading = *yaw; } - void CpmApplication::updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { - updating_objects_stack_ = true; + void CpmApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { + if (msg->objects.size() > 0) { + for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { + std::string object_uuid = uuidToHexString(obj.object_id); + auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) { + return !strcmp(e.uuid.c_str(), object_uuid.c_str()); + }); + + if (found_object == objectsList.end()) { + + } else { + found_object->to_send = true; + } + } + + } + } + + void CpmApplication::updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { + updating_objects_list_ = true; if (sending_) { RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped..."); return; } else { - objectsStack.clear(); + // objectsList.clear(); } if (msg->objects.size() > 0) { @@ -221,68 +241,157 @@ namespace v2x { for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { // RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label); + double existence_probability = obj.existence_probability; + // RCLCPP_INFO(node_->get_logger(), "existence_probability: %f", existence_probability); - CpmApplication::Object object; - object.objectID = i; - object.timestamp = msg->header.stamp; - object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; // MGRS - object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; - object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; - object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; - object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; - object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; - object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; - object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); - object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); - object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); + std::string object_uuid = uuidToHexString(obj.object_id); + RCLCPP_INFO(node_->get_logger(), "received object_id: %s", object_uuid.c_str()); - // xDistance, yDistance: Int (-132768..132767), 0.01 meter accuracy - object.xDistance = std::lround(( - (object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading) - ) * 100.0); - object.yDistance = std::lround(( - (object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading) - ) * 100.0); - if (object.xDistance < -132768 || object.xDistance > 132767) { - continue; - } - if (object.yDistance < -132768 || object.yDistance > 132767) { - continue; - } - object.xSpeed = 0; - object.ySpeed = 0; + RCLCPP_INFO(node_->get_logger(), "ObjectsList count: %d", objectsList.size()); - // Calculate orientation of detected object - tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); - tf2::Matrix3x3 matrix(quat); - double roll, pitch, yaw; - matrix.getRPY(roll, pitch, yaw); - if (yaw < 0) { - object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600 - } else { - object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 + if (existence_probability >= objectConfidenceThreshold_) { + // ObjectConfidence > ObjectConfidenceThreshold + + // Object tracked in internal memory? (i.e. Is object included in ObjectsList?) + auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) { + return !strcmp(e.uuid.c_str(), object_uuid.c_str()); + }); + + if (found_object == objectsList.end()) { + // Object is new to internal memory + RCLCPP_INFO(node_->get_logger(), "[%s] not in ObjectsList", object_uuid.c_str()); + + // Add new object to ObjectsList + CpmApplication::Object object; + object.objectID = i; + object.uuid = object_uuid; + object.timestamp_ros = msg->header.stamp; + object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; + object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; + object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; + object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; + object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; + object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; + object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; + object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); + object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); + object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); + + long long msg_timestamp_sec = msg->header.stamp.sec; + long long msg_timestamp_nsec = msg->header.stamp.nanosec; + msg_timestamp_sec -= 1072915200; // convert to etsi-epoch + long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; + object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; + if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { + RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement); + continue; + } + + object.to_send = false; + object.timestamp = runtime_.now(); + + objectsList.push_back(object); + + } else { + + // Object was already in internal memory + RCLCPP_INFO(node_->get_logger(), "[%s] already in ObjectsList",found_object->uuid.c_str()); + + // Object belongs to class person or animal + if (obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + + if (include_all_persons_and_animals_) { + found_object->to_send = true; + } + + // Object has not been included in a CPM in the past 500 ms. + if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500) { + // Include all objects of class person or animal in the current CPM + include_all_persons_and_animals_ = true; + found_object->to_send = true; + setAllObjectsOfPersonsAnimalsToSend(msg); + RCLCPP_INFO(node_->get_logger(), "Include all objects of person/animal class"); + } + + } else { + // Object does not belong to class person or animal + + // Euclidean absolute distance has changed by more than 4m + double dist = pow(obj.kinematics.initial_pose_with_covariance.pose.position.x - found_object->position_x, 2) + pow(obj.kinematics.initial_pose_with_covariance.pose.position.y - found_object->position_y, 2); + dist = sqrt(dist); + RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist); + if (dist > 4) { + found_object->to_send = true; + } else { + + } + + // Absolute speed changed by more than 0.5 m/s + double speed = pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x - found_object->twist_linear_x, 2) + pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x- found_object->twist_linear_y, 2); + speed = sqrt(speed); + RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); + if (speed > 0.5) { + found_object->to_send = true; + } + + // Orientation of speed vector changed by more than 4 degrees + double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI; + double twist_angular_y_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.y - found_object->twist_angular_y) * 180 / M_PI; + RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed x: %f", twist_angular_x_diff); + RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff); + if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) { + found_object->to_send = true; + } + + + // It has been more than 1 s since last transmission of this object + if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) { + found_object->to_send = true; + RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count()); + } + + } + + // Update found_object + found_object->timestamp_ros = msg->header.stamp; + found_object->position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; + found_object->position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; + found_object->position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; + found_object->orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; + found_object->orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; + found_object->orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; + found_object->orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; + found_object->shape_x = std::lround(obj.shape.dimensions.x * 10.0); + found_object->shape_y = std::lround(obj.shape.dimensions.y * 10.0); + found_object->shape_z = std::lround(obj.shape.dimensions.z * 10.0); + + long long msg_timestamp_sec = msg->header.stamp.sec; + long long msg_timestamp_nsec = msg->header.stamp.nanosec; + msg_timestamp_sec -= 1072915200; // convert to etsi-epoch + long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; + found_object->timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; + if (found_object->timeOfMeasurement < -1500 || found_object->timeOfMeasurement > 1500) { + RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", found_object->timeOfMeasurement); + continue; + } + + found_object->timestamp = runtime_.now(); + + + } } - - long long msg_timestamp_sec = msg->header.stamp.sec; - long long msg_timestamp_nsec = msg->header.stamp.nanosec; - msg_timestamp_sec -= 1072915200; // convert to etsi-epoch - long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; - // long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; - object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; - // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement); - if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { - RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement); - continue; - } - objectsStack.push_back(object); + + ++i; } + } else { + // No objects detected } // RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); - updating_objects_stack_ = false; + updating_objects_list_ = false; } void CpmApplication::send() { @@ -309,7 +418,7 @@ namespace v2x { management.stationType = StationType_passengerCar; management.referencePosition.latitude = ego_.latitude * 1e7; management.referencePosition.longitude = ego_.longitude * 1e7; - cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); + cpm.cpmParameters.numberOfPerceivedObjects = objectsList.size(); StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; sdc = vanetza::asn1::allocate(); @@ -325,41 +434,77 @@ namespace v2x { ovc.heading.headingValue = heading; ovc.heading.headingConfidence = 1; - if (objectsStack.size() > 0) { + if (objectsList.size() > 0) { PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; poc = vanetza::asn1::allocate(); - for (CpmApplication::Object object : objectsStack) { - PerceivedObject *pObj = vanetza::asn1::allocate(); - pObj->objectID = object.objectID; - pObj->timeOfMeasurement = object.timeOfMeasurement; - pObj->xDistance.value = object.xDistance; - pObj->xDistance.confidence = 1; - pObj->yDistance.value = object.yDistance; - pObj->yDistance.confidence = 1; - pObj->xSpeed.value = object.xSpeed; - pObj->xSpeed.confidence = 1; - pObj->ySpeed.value = object.ySpeed; - pObj->ySpeed.confidence = 1; + for (auto& object : objectsList) { - pObj->planarObjectDimension1 = vanetza::asn1::allocate(); - pObj->planarObjectDimension2 = vanetza::asn1::allocate(); - pObj->verticalObjectDimension = vanetza::asn1::allocate(); + // RCLCPP_INFO(node_->get_logger(), "object.to_send: %d", object.to_send); - (*(pObj->planarObjectDimension1)).value = object.shape_y; - (*(pObj->planarObjectDimension1)).confidence = 1; - (*(pObj->planarObjectDimension2)).value = object.shape_x; - (*(pObj->planarObjectDimension2)).confidence = 1; - (*(pObj->verticalObjectDimension)).value = object.shape_z; - (*(pObj->verticalObjectDimension)).confidence = 1; + if (object.to_send) { + PerceivedObject *pObj = vanetza::asn1::allocate(); - pObj->yawAngle = vanetza::asn1::allocate(); - (*(pObj->yawAngle)).value = object.yawAngle; - (*(pObj->yawAngle)).confidence = 1; + // Update CPM-specific values for Object + object.xDistance = std::lround(( + (object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading) + ) * 100.0); + object.yDistance = std::lround(( + (object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading) + ) * 100.0); + if (object.xDistance < -132768 || object.xDistance > 132767) { + continue; + } + if (object.yDistance < -132768 || object.yDistance > 132767) { + continue; + } - // RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); + // Calculate orientation of detected object + tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); + tf2::Matrix3x3 matrix(quat); + double roll, pitch, yaw; + matrix.getRPY(roll, pitch, yaw); + if (yaw < 0) { + object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600 + } else { + object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 + } + object.xSpeed = 0; + object.ySpeed = 0; + pObj->objectID = object.objectID; + pObj->timeOfMeasurement = object.timeOfMeasurement; + pObj->xDistance.value = object.xDistance; + pObj->xDistance.confidence = 1; + pObj->yDistance.value = object.yDistance; + pObj->yDistance.confidence = 1; + pObj->xSpeed.value = object.xSpeed; + pObj->xSpeed.confidence = 1; + pObj->ySpeed.value = object.ySpeed; + pObj->ySpeed.confidence = 1; - ASN_SEQUENCE_ADD(poc, pObj); + pObj->planarObjectDimension1 = vanetza::asn1::allocate(); + pObj->planarObjectDimension2 = vanetza::asn1::allocate(); + pObj->verticalObjectDimension = vanetza::asn1::allocate(); + + (*(pObj->planarObjectDimension1)).value = object.shape_y; + (*(pObj->planarObjectDimension1)).confidence = 1; + (*(pObj->planarObjectDimension2)).value = object.shape_x; + (*(pObj->planarObjectDimension2)).confidence = 1; + (*(pObj->verticalObjectDimension)).value = object.shape_z; + (*(pObj->verticalObjectDimension)).confidence = 1; + + pObj->yawAngle = vanetza::asn1::allocate(); + (*(pObj->yawAngle)).value = object.yawAngle; + (*(pObj->yawAngle)).confidence = 1; + + ASN_SEQUENCE_ADD(poc, pObj); + + object.to_send = false; + RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); + } else { + // Object.to_send is set to False + RCLCPP_INFO(node_->get_logger(), "Object: %s not being sent.", object.uuid.c_str()); + } } } else { cpm.cpmParameters.perceivedObjectContainer = NULL; From 92a0dcacf7dbea33fd08a6c89378a28a7fed426f Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 30 Mar 2022 14:51:13 +0900 Subject: [PATCH 2/5] Add new interfaces --- include/autoware_v2x/cpm_application.hpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index ce85419..6b8efae 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -19,7 +19,8 @@ namespace v2x std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&); void indicate(const DataIndication &, UpPacketPtr) override; void set_interval(vanetza::Clock::duration); - void updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); + void setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); + void updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void updateMGRS(double *, double *); void updateRP(double *, double *, double *); void updateGenerationDeltaTime(int *, long long *); @@ -27,8 +28,10 @@ namespace v2x void send(); struct Object { - int objectID; // 0-255 - rclcpp::Time timestamp; + std::string uuid; + int objectID; // 0-255 for CPM + vanetza::Clock::time_point timestamp; + rclcpp::Time timestamp_ros; double position_x; double position_y; double position_z; @@ -36,6 +39,10 @@ namespace v2x double orientation_y; double orientation_z; double orientation_w; + double twist_linear_x; + double twist_linear_y; + double twist_angular_x; + double twist_angular_y; int shape_x; int shape_y; int shape_z; @@ -46,8 +53,9 @@ namespace v2x int yawAngle; vanetza::PositionFix position; int timeOfMeasurement; + bool to_send; }; - std::vector objectsStack; + std::vector objectsList; std::vector receivedObjectsStack; private: @@ -72,10 +80,13 @@ namespace v2x int generationDeltaTime_; long long gdt_timestamp_; - bool updating_objects_stack_; + double objectConfidenceThreshold_; + + bool updating_objects_list_; bool sending_; bool is_sender_; bool reflect_packet_; + bool include_all_persons_and_animals_; }; } From 24a221cdc020b638fe7e63db6f2a6f4761787317 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 30 Mar 2022 14:51:25 +0900 Subject: [PATCH 3/5] Change to updateObjectsList --- src/v2x_app.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 89fd3fc..b6498b3 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -44,7 +44,7 @@ namespace v2x RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); } if (tf_received_ && cp_started_) { - cp->updateObjectsStack(msg); + cp->updateObjectsList(msg); } } From 306f31835b69794020db707e7c1bd7a35d43b474 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 30 Mar 2022 18:04:41 +0900 Subject: [PATCH 4/5] printObjectsList, change cpm-interval to 1000 ms --- include/autoware_v2x/cpm_application.hpp | 1 + src/cpm_application.cpp | 40 ++++++++++++++++-------- 2 files changed, 28 insertions(+), 13 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 6b8efae..87fa445 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -25,6 +25,7 @@ namespace v2x void updateRP(double *, double *, double *); void updateGenerationDeltaTime(int *, long long *); void updateHeading(double *); + void printObjectsList(); void send(); struct Object { diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index ca728ac..97e5990 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -45,7 +45,7 @@ namespace v2x { include_all_persons_and_animals_(false) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); - set_interval(milliseconds(100)); + set_interval(milliseconds(1000)); } void CpmApplication::set_interval(Clock::duration interval) { @@ -245,9 +245,9 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "existence_probability: %f", existence_probability); std::string object_uuid = uuidToHexString(obj.object_id); - RCLCPP_INFO(node_->get_logger(), "received object_id: %s", object_uuid.c_str()); + // RCLCPP_INFO(node_->get_logger(), "received object_id: %s", object_uuid.c_str()); - RCLCPP_INFO(node_->get_logger(), "ObjectsList count: %d", objectsList.size()); + // RCLCPP_INFO(node_->get_logger(), "ObjectsList count: %d", objectsList.size()); if (existence_probability >= objectConfidenceThreshold_) { // ObjectConfidence > ObjectConfidenceThreshold @@ -259,7 +259,7 @@ namespace v2x { if (found_object == objectsList.end()) { // Object is new to internal memory - RCLCPP_INFO(node_->get_logger(), "[%s] not in ObjectsList", object_uuid.c_str()); + // RCLCPP_INFO(node_->get_logger(), "[%s] not in ObjectsList", object_uuid.c_str()); // Add new object to ObjectsList CpmApplication::Object object; @@ -295,7 +295,7 @@ namespace v2x { } else { // Object was already in internal memory - RCLCPP_INFO(node_->get_logger(), "[%s] already in ObjectsList",found_object->uuid.c_str()); + // RCLCPP_INFO(node_->get_logger(), "[%s] already in ObjectsList",found_object->uuid.c_str()); // Object belongs to class person or animal if (obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { @@ -319,7 +319,7 @@ namespace v2x { // Euclidean absolute distance has changed by more than 4m double dist = pow(obj.kinematics.initial_pose_with_covariance.pose.position.x - found_object->position_x, 2) + pow(obj.kinematics.initial_pose_with_covariance.pose.position.y - found_object->position_y, 2); dist = sqrt(dist); - RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist); + // RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist); if (dist > 4) { found_object->to_send = true; } else { @@ -329,7 +329,7 @@ namespace v2x { // Absolute speed changed by more than 0.5 m/s double speed = pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x - found_object->twist_linear_x, 2) + pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x- found_object->twist_linear_y, 2); speed = sqrt(speed); - RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); + // RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); if (speed > 0.5) { found_object->to_send = true; } @@ -337,8 +337,8 @@ namespace v2x { // Orientation of speed vector changed by more than 4 degrees double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI; double twist_angular_y_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.y - found_object->twist_angular_y) * 180 / M_PI; - RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed x: %f", twist_angular_x_diff); - RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff); + // RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed x: %f", twist_angular_x_diff); + // RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff); if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) { found_object->to_send = true; } @@ -347,7 +347,7 @@ namespace v2x { // It has been more than 1 s since last transmission of this object if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) { found_object->to_send = true; - RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count()); + // RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count()); } } @@ -394,9 +394,23 @@ namespace v2x { updating_objects_list_ = false; } + void CpmApplication::printObjectsList() { + int i = 0; + RCLCPP_INFO(node_->get_logger(), "------------------------"); + for (auto& object : objectsList) { + RCLCPP_INFO(node_->get_logger(), "%d: %s \t %d", i, object.uuid.c_str(), object.to_send); + ++i; + } + RCLCPP_INFO(node_->get_logger(), "------------------------"); + } + void CpmApplication::send() { if (is_sender_) { + + printObjectsList(); + + sending_ = true; // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); @@ -500,15 +514,15 @@ namespace v2x { ASN_SEQUENCE_ADD(poc, pObj); object.to_send = false; - RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); + // RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); } else { // Object.to_send is set to False - RCLCPP_INFO(node_->get_logger(), "Object: %s not being sent.", object.uuid.c_str()); + // RCLCPP_INFO(node_->get_logger(), "Object: %s not being sent.", object.uuid.c_str()); } } } else { cpm.cpmParameters.perceivedObjectContainer = NULL; - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); } Application::DownPacketPtr packet{new DownPacket()}; From 65b69a0b1b339f7ec3711798b76babbdc996021c Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Fri, 1 Apr 2022 14:51:10 +0900 Subject: [PATCH 5/5] Update Perceived Objects Inclusion Management --- include/autoware_v2x/cpm_application.hpp | 8 +++- src/cpm_application.cpp | 60 +++++++++++++++--------- 2 files changed, 44 insertions(+), 24 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 87fa445..ea3aab5 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -25,7 +25,7 @@ namespace v2x void updateRP(double *, double *, double *); void updateGenerationDeltaTime(int *, long long *); void updateHeading(double *); - void printObjectsList(); + void printObjectsList(int); void send(); struct Object { @@ -55,6 +55,7 @@ namespace v2x vanetza::PositionFix position; int timeOfMeasurement; bool to_send; + int to_send_trigger; }; std::vector objectsList; std::vector receivedObjectsStack; @@ -88,6 +89,11 @@ namespace v2x bool is_sender_; bool reflect_packet_; bool include_all_persons_and_animals_; + + int cpm_num_; + int received_cpm_num_; + + int cpm_object_id_; }; } diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 97e5990..6cf4cf9 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -42,10 +42,13 @@ namespace v2x { is_sender_(is_sender), reflect_packet_(false), objectConfidenceThreshold_(0.0), - include_all_persons_and_animals_(false) + include_all_persons_and_animals_(false), + cpm_num_(0), + received_cpm_num_(0), + cpm_object_id_(0) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); - set_interval(milliseconds(1000)); + set_interval(milliseconds(100)); } void CpmApplication::set_interval(Clock::duration interval) { @@ -81,7 +84,7 @@ namespace v2x { std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); if (cpm) { - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received CPM"); + RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received CPM #%d", received_cpm_num_); rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); @@ -132,7 +135,7 @@ namespace v2x { if (poc != NULL) { for (int i = 0; i < poc->list.count; ++i) { - // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); + RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); CpmApplication::Object object; double x1 = poc->list.array[i]->xDistance.value; @@ -181,6 +184,7 @@ namespace v2x { } } + ++received_cpm_num_; } else { RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content"); @@ -237,7 +241,6 @@ namespace v2x { } if (msg->objects.size() > 0) { - int i = 0; for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { // RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label); @@ -259,11 +262,14 @@ namespace v2x { if (found_object == objectsList.end()) { // Object is new to internal memory - // RCLCPP_INFO(node_->get_logger(), "[%s] not in ObjectsList", object_uuid.c_str()); + + if (cpm_object_id_ > 255) { + cpm_object_id_ = 0; + } // Add new object to ObjectsList CpmApplication::Object object; - object.objectID = i; + object.objectID = cpm_object_id_; object.uuid = object_uuid; object.timestamp_ros = msg->header.stamp; object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; @@ -288,27 +294,30 @@ namespace v2x { } object.to_send = false; + object.to_send_trigger = 0; object.timestamp = runtime_.now(); objectsList.push_back(object); + ++cpm_object_id_; } else { // Object was already in internal memory - // RCLCPP_INFO(node_->get_logger(), "[%s] already in ObjectsList",found_object->uuid.c_str()); // Object belongs to class person or animal if (obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { if (include_all_persons_and_animals_) { found_object->to_send = true; + found_object->to_send_trigger = 5; } // Object has not been included in a CPM in the past 500 ms. - if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500) { + if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) { // Include all objects of class person or animal in the current CPM include_all_persons_and_animals_ = true; found_object->to_send = true; + found_object->to_send_trigger = 5; setAllObjectsOfPersonsAnimalsToSend(msg); RCLCPP_INFO(node_->get_logger(), "Include all objects of person/animal class"); } @@ -322,6 +331,7 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist); if (dist > 4) { found_object->to_send = true; + found_object->to_send_trigger = 1; } else { } @@ -332,6 +342,7 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); if (speed > 0.5) { found_object->to_send = true; + found_object->to_send_trigger = 2; } // Orientation of speed vector changed by more than 4 degrees @@ -341,12 +352,14 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff); if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) { found_object->to_send = true; + found_object->to_send_trigger = 3; } // It has been more than 1 s since last transmission of this object if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) { found_object->to_send = true; + found_object->to_send_trigger = 4; // RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count()); } @@ -377,12 +390,8 @@ namespace v2x { found_object->timestamp = runtime_.now(); - } } - - - ++i; } } else { // No objects detected @@ -394,24 +403,26 @@ namespace v2x { updating_objects_list_ = false; } - void CpmApplication::printObjectsList() { - int i = 0; - RCLCPP_INFO(node_->get_logger(), "------------------------"); - for (auto& object : objectsList) { - RCLCPP_INFO(node_->get_logger(), "%d: %s \t %d", i, object.uuid.c_str(), object.to_send); - ++i; + void CpmApplication::printObjectsList(int cpm_num) { + // RCLCPP_INFO(node_->get_logger(), "------------------------"); + if (objectsList.size() > 0) { + for (auto& object : objectsList) { + RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,%d,%s,%d,%d", cpm_num, object.objectID, object.uuid.c_str(), object.to_send, object.to_send_trigger); + } + } else { + RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num); } - RCLCPP_INFO(node_->get_logger(), "------------------------"); + + // RCLCPP_INFO(node_->get_logger(), "------------------------"); } void CpmApplication::send() { if (is_sender_) { - printObjectsList(); - - sending_ = true; + + printObjectsList(cpm_num_); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); @@ -467,9 +478,11 @@ namespace v2x { (object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading) ) * 100.0); if (object.xDistance < -132768 || object.xDistance > 132767) { + RCLCPP_WARN(node_->get_logger(), "xDistance out of bounds. objectID: #%d", object.objectID); continue; } if (object.yDistance < -132768 || object.yDistance > 132767) { + RCLCPP_WARN(node_->get_logger(), "yDistance out of bounds. objectID: #%d", object.objectID); continue; } @@ -545,6 +558,7 @@ namespace v2x { rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); + ++cpm_num_; } } } \ No newline at end of file