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

Update v2x_node

This commit is contained in:
Yu Asabe 2021-11-10 17:14:09 +09:00
parent dad222be44
commit ed2656a482
2 changed files with 58 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
include/autoware_v2x/v2x_node.hpp
View File

@ -3,6 +3,7 @@
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp> #include <boost/asio/io_service.hpp>
#include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/cpm_application.hpp" #include "autoware_v2x/cpm_application.hpp"
#include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/link_layer.hpp"
@ -17,32 +18,35 @@ namespace v2x
{ {
public: public:
explicit V2XNode(const rclcpp::NodeOptions &node_options); explicit V2XNode(const rclcpp::NodeOptions &node_options);
V2XApp *app;
private: private:
void objectsCallback( void objectsCallback(
const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
friend class CpmApplication; // friend class CpmApplication;
friend class Application; // friend class Application;
rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_; rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_; rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
std::unique_ptr<CpmApplication> cp_; // std::unique_ptr<CpmApplication> cp_;
std::unique_ptr<Application> app_; // std::unique_ptr<Application> app_;
boost::asio::io_service io_service_; // boost::asio::io_service io_service_;
TimeTrigger trigger_; // TimeTrigger trigger_;
const char *device_name_; // vanetza::Runtime& runtime_;
EthernetDevice device_; // const char *device_name_;
vanetza::MacAddress mac_address_; // EthernetDevice device_;
std::unique_ptr<LinkLayer> link_layer_; // vanetza::MacAddress mac_address_;
vanetza::geonet::MIB mib_; // std::unique_ptr<LinkLayer> link_layer_;
std::unique_ptr<vanetza::PositionProvider> positioning_; // vanetza::geonet::MIB mib_;
std::unique_ptr<vanetza::security::SecurityEntity> security_; // std::unique_ptr<vanetza::PositionProvider> positioning_;
RouterContext context_; // std::unique_ptr<vanetza::security::SecurityEntity> security_;
// RouterContext context_;
double pos_lat_; double pos_lat_;
double pos_lon_; double pos_lon_;
// vanetza::PositionFix reference_position_;
}; };
} }

91
src/v2x_node.cpp
View File

@ -1,4 +1,5 @@
#include "autoware_v2x/v2x_node.hpp" #include "autoware_v2x/v2x_node.hpp"
#include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/router_context.hpp" #include "autoware_v2x/router_context.hpp"
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
@ -13,9 +14,8 @@
#include <vanetza/facilities/cpm_functions.hpp> #include <vanetza/facilities/cpm_functions.hpp>
#include <sstream> #include <sstream>
#include <memory> #include <memory>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace gn = vanetza::geonet; namespace gn = vanetza::geonet;
@ -25,23 +25,23 @@ using namespace std::chrono;
namespace v2x namespace v2x
{ {
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options)
rclcpp::Node("autoware_v2x_node", node_options), // io_service_(),
io_service_(), //trigger_(io_service_),
trigger_(io_service_), // runtime_(new Runtime(Clock::at(boost::posix_time::microsec_clock::universal_time()))),
device_name_("wlp4s0"), // device_name_("wlp4s0"),
device_(device_name_), // device_(device_name_),
mac_address_(device_.address()), // mac_address_(device_.address()),
link_layer_(create_link_layer(io_service_, device_, "ethernet")), // link_layer_(create_link_layer(io_service_, device_, "ethernet")),
positioning_(create_position_provider(io_service_, trigger_.runtime())), // positioning_(create_position_provider(io_service_, trigger_.runtime())),
security_(create_security_entity(trigger_.runtime(), *positioning_)), // security_(create_security_entity(trigger_.runtime(), *positioning_)),
mib_(), // mib_(),
cp_(new CpmApplication(this)), // cp_(new CpmApplication(this, runtime_)),
context_(mib_, trigger_, *positioning_, security_.get()), // context_(mib_, trigger_, *positioning_, security_.get()),
app_() // app_()
{ {
using std::placeholders::_1; using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
RCLCPP_INFO(get_logger(), "V2X Node Launched"); RCLCPP_INFO(get_logger(), "V2X Node Launched");
@ -50,38 +50,41 @@ namespace v2x
// device_(device_name_); // device_(device_name_);
// mac_address_ = device_.address(); // mac_address_ = device_.address();
std::stringstream sout; // std::stringstream sout;
sout << mac_address_; // sout << mac_address_;
RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
// trigger_(io_service_); // trigger_(io_service_);
// link_layer_ = create_link_layer(io_service_, device_, "ethernet"); // link_layer_ = create_link_layer(io_service_, device_, "ethernet");
mib_.itsGnLocalGnAddr.mid(mac_address_); // mib_.itsGnLocalGnAddr.mid(mac_address_);
mib_.itsGnLocalGnAddr.is_manually_configured(true); // mib_.itsGnLocalGnAddr.is_manually_configured(true);
mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed; // mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
mib_.itsGnSecurity = false; // mib_.itsGnSecurity = false;
mib_.itsGnProtocolVersion = 1; // mib_.itsGnProtocolVersion = 1;
// context_(mib_, trigger_, *positioning_, security_.get()), // context_(mib_, trigger_, *positioning_, security_.get()),
// context_.router_.set_address(mib_.itsGnLocalGnAddr); // context_.router_.set_address(mib_.itsGnLocalGnAddr);
context_.updateMIB(mib_); // context_.updateMIB(mib_);
// positioning_ = create_position_provider(io_service_, trigger_.runtime()); // positioning_ = create_position_provider(io_service_, trigger_.runtime());
// security_ = create_security_entity(trigger_.runtime(), *positioning_); // security_ = create_security_entity(trigger_.runtime(), *positioning_);
// RouterContext context_(mib_, trigger_, *positioning_, security_.get()); // RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// RouterContext context_(mib_, trigger_, *positioning_, security_.get()); // RouterContext context_(mib_, trigger_, *positioning_, security_.get());
context_.set_link_layer(link_layer_.get()); // context_.set_link_layer(link_layer_.get());
// std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) }; // std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
// app_ = std::move(cp_); // app_ = std::move(cp_);
context_.enable(cp_.get()); // context_.enable(cp_.get());
// io_service_.run(); // io_service_.run();
boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_)); // boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
app = new V2XApp(this);
boost::thread v2xApp(boost::bind(&V2XApp::start, app));
// boost::thread v2xApp(V2XApp);
// boost::thread v2xApp(&V2XApp, this);
// // Print MAC Address to logger // // Print MAC Address to logger
// std::stringstream sout; // std::stringstream sout;
@ -94,31 +97,17 @@ namespace v2x
// RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str());
RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size());
// Send CPM // Send CPM
cp_->send(msg, this, pos_lat_, pos_lon_); // cp_->send(msg, this, pos_lat_, pos_lon_);
// cp_->updateObjectsStack(msg, this);
app->objectsCallback(msg);
} }
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg)
{
// RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z); // RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z);
float x = msg->transforms[0].transform.translation.x; app->tfCallback(msg);
float y = msg->transforms[0].transform.translation.y;
float z = msg->transforms[0].transform.translation.z;
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(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);
pos_lat_ = lat;
pos_lon_ = lon;
RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
} }
} }