TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update Perceived Objects Inclusion Management

This commit is contained in:
Yu Asabe 2022-04-01 14:51:10 +09:00
parent 306f31835b
commit 65b69a0b1b
2 changed files with 44 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/autoware_v2x/cpm_application.hpp
View File

@ -25,7 +25,7 @@ namespace v2x
void updateRP(double *, double *, double *); void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *, long long *); void updateGenerationDeltaTime(int *, long long *);
void updateHeading(double *); void updateHeading(double *);
void printObjectsList(); void printObjectsList(int);
void send(); void send();
struct Object { struct Object {
@ -55,6 +55,7 @@ namespace v2x
vanetza::PositionFix position; vanetza::PositionFix position;
int timeOfMeasurement; int timeOfMeasurement;
bool to_send; bool to_send;
int to_send_trigger;
}; };
std::vector<CpmApplication::Object> objectsList; std::vector<CpmApplication::Object> objectsList;
std::vector<CpmApplication::Object> receivedObjectsStack; std::vector<CpmApplication::Object> receivedObjectsStack;
@ -88,6 +89,11 @@ namespace v2x
bool is_sender_; bool is_sender_;
bool reflect_packet_; bool reflect_packet_;
bool include_all_persons_and_animals_; bool include_all_persons_and_animals_;
int cpm_num_;
int received_cpm_num_;
int cpm_object_id_;
}; };
} }

60
src/cpm_application.cpp
View File

@ -42,10 +42,13 @@ namespace v2x {
is_sender_(is_sender), is_sender_(is_sender),
reflect_packet_(false), reflect_packet_(false),
objectConfidenceThreshold_(0.0), 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_); 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) { void CpmApplication::set_interval(Clock::duration interval) {
@ -81,7 +84,7 @@ namespace v2x {
std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet); std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
if (cpm) { 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::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
@ -132,7 +135,7 @@ namespace v2x {
if (poc != NULL) { if (poc != NULL) {
for (int i = 0; i < poc->list.count; ++i) { 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; CpmApplication::Object object;
double x1 = poc->list.array[i]->xDistance.value; double x1 = poc->list.array[i]->xDistance.value;
@ -181,6 +184,7 @@ namespace v2x {
} }
} }
++received_cpm_num_;
} else { } else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content"); RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content");
@ -237,7 +241,6 @@ namespace v2x {
} }
if (msg->objects.size() > 0) { if (msg->objects.size() > 0) {
int i = 0;
for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
// RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label); // RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label);
@ -259,11 +262,14 @@ namespace v2x {
if (found_object == objectsList.end()) { if (found_object == objectsList.end()) {
// Object is new to internal memory // 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 // Add new object to ObjectsList
CpmApplication::Object object; CpmApplication::Object object;
object.objectID = i; object.objectID = cpm_object_id_;
object.uuid = object_uuid; object.uuid = object_uuid;
object.timestamp_ros = msg->header.stamp; object.timestamp_ros = msg->header.stamp;
object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; 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 = false;
object.to_send_trigger = 0;
object.timestamp = runtime_.now(); object.timestamp = runtime_.now();
objectsList.push_back(object); objectsList.push_back(object);
++cpm_object_id_;
} else { } else {
// Object was already in internal memory // 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 // 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 (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_) { if (include_all_persons_and_animals_) {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 5;
} }
// Object has not been included in a CPM in the past 500 ms. // 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 objects of class person or animal in the current CPM
include_all_persons_and_animals_ = true; include_all_persons_and_animals_ = true;
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 5;
setAllObjectsOfPersonsAnimalsToSend(msg); setAllObjectsOfPersonsAnimalsToSend(msg);
RCLCPP_INFO(node_->get_logger(), "Include all objects of person/animal class"); 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); // RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist);
if (dist > 4) { if (dist > 4) {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 1;
} else { } else {
} }
@ -332,6 +342,7 @@ namespace v2x {
// RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); // RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist);
if (speed > 0.5) { if (speed > 0.5) {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 2;
} }
// Orientation of speed vector changed by more than 4 degrees // 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); // 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 ) { if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 3;
} }
// It has been more than 1 s since last transmission of this object // 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) { if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) {
found_object->to_send = true; 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()); // 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(); found_object->timestamp = runtime_.now();
} }
} }
++i;
} }
} else { } else {
// No objects detected // No objects detected
@ -394,25 +403,27 @@ namespace v2x {
updating_objects_list_ = false; updating_objects_list_ = false;
} }
void CpmApplication::printObjectsList() { void CpmApplication::printObjectsList(int cpm_num) {
int i = 0; // RCLCPP_INFO(node_->get_logger(), "------------------------");
RCLCPP_INFO(node_->get_logger(), "------------------------"); if (objectsList.size() > 0) {
for (auto& object : objectsList) { for (auto& object : objectsList) {
RCLCPP_INFO(node_->get_logger(), "%d: %s \t %d", i, object.uuid.c_str(), object.to_send); 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);
++i; }
} else {
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num);
} }
RCLCPP_INFO(node_->get_logger(), "------------------------");
// RCLCPP_INFO(node_->get_logger(), "------------------------");
} }
void CpmApplication::send() { void CpmApplication::send() {
if (is_sender_) { if (is_sender_) {
printObjectsList();
sending_ = true; sending_ = true;
printObjectsList(cpm_num_);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
vanetza::asn1::Cpm message; vanetza::asn1::Cpm message;
@ -467,9 +478,11 @@ namespace v2x {
(object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading) (object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)
) * 100.0); ) * 100.0);
if (object.xDistance < -132768 || object.xDistance > 132767) { if (object.xDistance < -132768 || object.xDistance > 132767) {
RCLCPP_WARN(node_->get_logger(), "xDistance out of bounds. objectID: #%d", object.objectID);
continue; continue;
} }
if (object.yDistance < -132768 || object.yDistance > 132767) { if (object.yDistance < -132768 || object.yDistance > 132767) {
RCLCPP_WARN(node_->get_logger(), "yDistance out of bounds. objectID: #%d", object.objectID);
continue; continue;
} }
@ -545,6 +558,7 @@ namespace v2x {
rclcpp::Time current_time = node_->now(); rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
++cpm_num_;
} }
} }
} }