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

Debug receiver, integration

This commit is contained in:
Yu Asabe 2021-11-11 05:30:21 +09:00
parent 6bc9151b8f
commit 1360510b2a
3 changed files with 49 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/autoware_v2x/v2x_app.hpp
View File

@ -34,6 +34,7 @@ namespace v2x
friend class Application; friend class Application;
V2XNode* node_; V2XNode* node_;
bool tf_received_; bool tf_received_;
int tf_interval_;
bool cp_started_; bool cp_started_;
}; };
} }

12
src/cpm_application.cpp
View File

@ -41,7 +41,7 @@ namespace v2x
sending_(false) sending_(false)
{ {
RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(1000)); set_interval(milliseconds(100));
} }
void CpmApplication::set_interval(Clock::duration interval) void CpmApplication::set_interval(Clock::duration interval)
@ -108,8 +108,12 @@ namespace v2x
CpmApplication::Object object; CpmApplication::Object object;
double x1 = poc->list.array[i]->xDistance.value; double x1 = poc->list.array[i]->xDistance.value;
double y1 = poc->list.array[i]->yDistance.value; double y1 = poc->list.array[i]->yDistance.value;
object.position_x = x_mgrs - (cos(orientation) * x1 - sin(orientation) * y1); RCLCPP_INFO(node_->get_logger(), "cpm object: xDistance: %f, yDistance: %f", x1, y1);
object.position_y = y_mgrs - (sin(orientation) * x1 + cos(orientation) * y1); x1 = x1 / 100.0;
y1 = y1 / 100.0;
object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1);
object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
RCLCPP_INFO(node_->get_logger(), "cpm object: %f, %f, %f, %f", x1, y1, object.position_x, object.position_y);
receivedObjectsStack.push_back(object); receivedObjectsStack.push_back(object);
} }
node_->publishObjects(&receivedObjectsStack); node_->publishObjects(&receivedObjectsStack);
@ -253,6 +257,8 @@ namespace v2x
for (CpmApplication::Object object : objectsStack) for (CpmApplication::Object object : objectsStack)
{ {
if (object.xDistance > 10000) continue;
if (object.yDistance > 10000) continue;
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>(); PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
pObj->objectID = object.objectID; pObj->objectID = object.objectID;
pObj->timeOfMeasurement = object.timeOfMeasurement; pObj->timeOfMeasurement = object.timeOfMeasurement;

73
src/v2x_app.cpp
View File

@ -33,6 +33,7 @@ namespace v2x
V2XApp::V2XApp(V2XNode *node) : V2XApp::V2XApp(V2XNode *node) :
node_(node), node_(node),
tf_received_(false), tf_received_(false),
tf_interval_(0),
cp_started_(false) cp_started_(false)
{ {
} }
@ -45,49 +46,53 @@ namespace v2x
} }
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
// RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); if (tf_interval_ == 4) {
tf_received_ = true; RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
tf_received_ = true;
double x = msg->transforms[0].transform.translation.x; double x = msg->transforms[0].transform.translation.x;
double y = msg->transforms[0].transform.translation.y; double y = msg->transforms[0].transform.translation.y;
double z = msg->transforms[0].transform.translation.z; double z = msg->transforms[0].transform.translation.z;
int timestamp = msg->transforms[0].header.stamp.sec; int timestamp = msg->transforms[0].header.stamp.sec;
int gdt = timestamp % 65536; int gdt = timestamp % 65536;
double rot_x = msg->transforms[0].transform.rotation.x; double rot_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y; double rot_y = msg->transforms[0].transform.rotation.y;
double rot_z = msg->transforms[0].transform.rotation.z; double rot_z = msg->transforms[0].transform.rotation.z;
double rot_w = msg->transforms[0].transform.rotation.w; double rot_w = msg->transforms[0].transform.rotation.w;
// Convert the quarternion to euler (yaw, pitch, roll) // Convert the quarternion to euler (yaw, pitch, roll)
tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w); tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w);
tf2::Matrix3x3 matrix(quat); tf2::Matrix3x3 matrix(quat);
double roll, pitch, yaw; double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw); matrix.getRPY(roll, pitch, yaw);
char mgrs[20]; char mgrs[20];
int zone, prec; int zone, prec;
bool northp; bool northp;
double x_mgrs, y_mgrs; double x_mgrs, y_mgrs;
double lat, lon; double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
// RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs); // RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon);
// RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); // RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
// RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w); // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w);
// RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw); RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw);
// RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt); // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt);
if (cp && cp_started_) { if (cp && cp_started_) {
cp->updateMGRS(&x, &y); cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z); cp->updateRP(&lat, &lon, &z);
cp->updateHeading(&yaw); cp->updateHeading(&yaw);
cp->updateGenerationDeltaTime(&gdt); cp->updateGenerationDeltaTime(&gdt);
}
tf_interval_ = 0;
} }
tf_interval_ += 1;
} }
void V2XApp::start() { void V2XApp::start() {