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

fixing positioning

This commit is contained in:
Yu Asabe 2022-02-16 16:53:45 +09:00
parent e53d089817
commit eb6a9b2040
4 changed files with 29 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/autoware_v2x/cpm_application.hpp
View File

@ -58,12 +58,17 @@ namespace v2x
vanetza::Runtime &runtime_;
vanetza::Clock::duration cpm_interval_;
double ego_x_;
double ego_y_;
double ego_lat_;
double ego_lon_;
double ego_altitude_;
double ego_heading_;
struct Ego_station {
double mgrs_x;
double mgrs_y;
double latitude;
double longitude;
double altitude;
double heading;
};
Ego_station ego_;
int generationDeltaTime_;
long long gdt_timestamp_;

2
launch/v2x.launch.xml
View File

@ -1,6 +1,6 @@
<launch>
<arg name="network_interface" default="wlp5s0"/>
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen" emulate_tty="true">
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
<param name="network_interface" value="$(var network_interface)"/>
</node>
</launch>

38
src/cpm_application.cpp
View File

@ -28,17 +28,11 @@ using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;
namespace v2x
{
namespace v2x {
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) :
node_(node),
runtime_(rt),
ego_x_(0),
ego_y_(0),
ego_lat_(0),
ego_lon_(0),
ego_altitude_(0),
ego_heading_(0),
ego_(),
generationDeltaTime_(0),
updating_objects_stack_(false),
sending_(false),
@ -181,14 +175,14 @@ namespace v2x
}
void CpmApplication::updateMGRS(double *x, double *y) {
ego_x_ = *x;
ego_y_ = *y;
ego_.mgrs_x = *x;
ego_.mgrs_y = *y;
}
void CpmApplication::updateRP(double *lat, double *lon, double *altitude) {
ego_lat_ = *lat;
ego_lon_ = *lon;
ego_altitude_ = *altitude;
ego_.latitude = *lat;
ego_.longitude = *lon;
ego_.altitude = *altitude;
}
void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
@ -197,7 +191,7 @@ namespace v2x
}
void CpmApplication::updateHeading(double *yaw) {
ego_heading_ = *yaw;
ego_.heading = *yaw;
}
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
@ -226,8 +220,10 @@ namespace v2x
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);
object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
// object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
// object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
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;
}
@ -260,9 +256,7 @@ namespace v2x
}
objectsStack.push_back(object);
++i;
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z);
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x * 10.0, obj.shape.dimensions.y * 10.0, obj.shape.dimensions.z * 10.0);
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %d %d %d", std::lround(obj.shape.dimensions.x * 10.0), std::lround(obj.shape.dimensions.y * 10.0), std::lround(obj.shape.dimensions.z * 10.0));
RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle);
}
}
@ -295,8 +289,8 @@ namespace v2x
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar;
PositionFix fix;
fix.latitude = ego_lat_ * units::degree;
fix.longitude = ego_lon_ * units::degree;
fix.latitude = ego_.latitude * units::degree;
fix.longitude = ego_.longitude * units::degree;
fix.confidence.semi_major = 1.0 * units::si::meter;
fix.confidence.semi_minor = fix.confidence.semi_major;
copy(fix, management.referencePosition);
@ -308,7 +302,7 @@ namespace v2x
OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10);
int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
if (heading < 0) heading += 3600;
ovc.heading.headingValue = heading;
ovc.heading.headingConfidence = 1;

13
src/v2x_app.cpp
View File

@ -49,7 +49,7 @@ namespace v2x
}
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
// RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
tf_received_ = true;
double x = msg->transforms[0].transform.translation.x;
@ -60,10 +60,6 @@ namespace v2x
long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds
timestamp_sec -= 1072915200; // convert to etsi-epoch
long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000;
// long long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds
// long long gdt_timestamp_msec = gdt_timestamp / 1000000;
// long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000;
// int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds
int gdt = timestamp_msec % 65536;
// RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
@ -78,23 +74,16 @@ namespace v2x
double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
char mgrs[20];
int zone, prec;
bool northp;
double x_mgrs, y_mgrs;
double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
// RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
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 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(), "Timestamp: %d, GDT: %d", timestamp, gdt);
if (cp && cp_started_) {
cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z);