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

Fix timeOfMeasurment bug

This commit is contained in:
Yu Asabe 2021-11-10 17:57:37 +09:00
parent 62d7576f74
commit cc438fcb8c
3 changed files with 36 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/autoware_v2x/cpm_application.hpp
View File

@ -39,8 +39,8 @@ private:
double orientation_y;
double orientation_z;
double orientation_w;
double xDistance;
double yDistance;
int xDistance;
int yDistance;
double xSpeed;
double ySpeed;
vanetza::PositionFix position;

59
src/cpm_application.cpp
View File

@ -17,23 +17,22 @@
#define _USE_MATH_DEFINES
#include <math.h>
// This is a very simple application that sends BTP-B messages with the content 0xc0ffee.
using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;
CpmApplication::CpmApplication(rclcpp::Node *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),
generationDeltaTime_(0),
updating_objects_stack_(false),
sending_(false)
CpmApplication::CpmApplication(rclcpp::Node *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),
generationDeltaTime_(0),
updating_objects_stack_(false),
sending_(false)
{
RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(1000));
@ -69,20 +68,18 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
if (cpm)
{
RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header;
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude;
double lon = management.referencePosition.longitude;
RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
}
else
{
RCLCPP_INFO(node_->get_logger(), "Received broken content");
}
asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header;
RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.messageId = %d", header.messageID);
RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.stationId = %d", header.stationID);
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude;
double lon = management.referencePosition.longitude;
RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
}
void CpmApplication::updateMGRS(double *x, double *y) {
@ -143,9 +140,10 @@ void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::Dyn
object.orientation_y = obj.state.pose_covariance.pose.orientation.y;
object.orientation_z = obj.state.pose_covariance.pose.orientation.z;
object.orientation_w = obj.state.pose_covariance.pose.orientation.w;
object.xDistance = ((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
object.yDistance = ((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
// RCLCPP_INFO(node_->get_logger(), "xDistance: %f, yDistance: %f", object.xDistance, object.yDistance);
object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance);
object.timeOfMeasurement = 100;
objectsStack.push_back(object);
++i;
}
@ -205,9 +203,11 @@ void CpmApplication::send()
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
// ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180;
RCLCPP_INFO(node_->get_logger(), "headingValue...");
ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10;
ovc.heading.headingConfidence = 1;
RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer...");
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
// cpm.cpmParameters.perceivedObjectContainer = poc;
@ -229,16 +229,21 @@ void CpmApplication::send()
pObj->ySpeed.confidence = 1;
ASN_SEQUENCE_ADD(poc, pObj);
// RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value);
}
RCLCPP_INFO(node_->get_logger(), "1");
Application::DownPacketPtr packet{new DownPacket()};
RCLCPP_INFO(node_->get_logger(), "2");
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
RCLCPP_INFO(node_->get_logger(), "3");
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
// std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
payload->layer(OsiLayer::Application) = std::move(message);
RCLCPP_INFO(node_->get_logger(), "4");
Application::DataRequest request;
request.its_aid = aid::CP;
request.transport_type = geonet::TransportType::SHB;
@ -247,7 +252,9 @@ void CpmApplication::send()
// RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
// RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
RCLCPP_INFO(node_->get_logger(), "5");
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
RCLCPP_INFO(node_->get_logger(), "6");
if (!confirm.accepted())
{
throw std::runtime_error("CPM application data request failed");

59
src/v2x_app.cpp
View File

@ -33,67 +33,12 @@ namespace v2x
node_(node),
tf_received_(false),
cp_started_(false)
// io_service_(),
// trigger_(io_service_),
// runtime_(new Runtime(Clock::at(boost::posix_time::microsec_clock::universal_time()))),
// device_name_("wlp4s0"),
// device_(device_name_),
// mac_address_(device_.address()),
// link_layer_(create_link_layer(io_service_, device_, "ethernet")),
// positioning_(create_position_provider(io_service_, trigger_.runtime())),
// security_(create_security_entity(trigger_.runtime(), *positioning_)),
// mib_(),
// cp_(new CpmApplication(node_, trigger_.runtime())),
// context_(mib_, trigger_, *positioning_, security_.get()),
// app_()
{
// device_name_ = "wlp4s0";
// device_(device_name_);
// mac_address_ = device_.address();
// std::stringstream sout;
// sout << mac_address_;
// RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str());
// trigger_(io_service_);
// link_layer_ = create_link_layer(io_service_, device_, "ethernet");
// mib_.itsGnLocalGnAddr.mid(mac_address_);
// mib_.itsGnLocalGnAddr.is_manually_configured(true);
// mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
// mib_.itsGnSecurity = false;
// mib_.itsGnProtocolVersion = 1;
// context_(mib_, trigger_, *positioning_, security_.get()),
// context_.router_.set_address(mib_.itsGnLocalGnAddr);
// context_.updateMIB(mib_);
// positioning_ = create_position_provider(io_service_, trigger_.runtime());
// security_ = create_security_entity(trigger_.runtime(), *positioning_);
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// context_.set_link_layer(link_layer_.get());
// std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
// app_ = std::move(cp_);
// context_.enable(cp_.get());
// io_service_.run();
// boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
// // Print MAC Address to logger
// std::stringstream sout;
// sout << mac_address;
// RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
}
void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
if (tf_received_) {
if (tf_received_ && cp_started_) {
cp->updateObjectsStack(msg);
}
}
@ -165,7 +110,6 @@ namespace v2x
mib.itsGnSecurity = false;
mib.itsGnProtocolVersion = 1;
// context_.updateMIB(mib_);
auto link_layer = create_link_layer(io_service, device, "ethernet");
auto positioning = create_position_provider(io_service, trigger.runtime());
auto security = create_security_entity(trigger.runtime(), *positioning);
@ -175,7 +119,6 @@ namespace v2x
cp = new CpmApplication(node_, trigger.runtime());
// context.enable(cp_.get());
context.enable(cp);
cp_started_ = true;