diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index fb3de5d..6990263 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -3,6 +3,7 @@ #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include +#include "autoware_v2x/v2x_app.hpp" #include "autoware_v2x/cpm_application.hpp" #include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/link_layer.hpp" @@ -17,32 +18,35 @@ namespace v2x { public: explicit V2XNode(const rclcpp::NodeOptions &node_options); + V2XApp *app; private: void objectsCallback( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); - friend class CpmApplication; - friend class Application; + // friend class CpmApplication; + // friend class Application; rclcpp::Subscription::SharedPtr subscription_; rclcpp::Subscription::SharedPtr subscription_pos_; - std::unique_ptr cp_; - std::unique_ptr app_; - boost::asio::io_service io_service_; - TimeTrigger trigger_; - const char *device_name_; - EthernetDevice device_; - vanetza::MacAddress mac_address_; - std::unique_ptr link_layer_; - vanetza::geonet::MIB mib_; - std::unique_ptr positioning_; - std::unique_ptr security_; - RouterContext context_; + // std::unique_ptr cp_; + // std::unique_ptr app_; + // boost::asio::io_service io_service_; + // TimeTrigger trigger_; + // vanetza::Runtime& runtime_; + // const char *device_name_; + // EthernetDevice device_; + // vanetza::MacAddress mac_address_; + // std::unique_ptr link_layer_; + // vanetza::geonet::MIB mib_; + // std::unique_ptr positioning_; + // std::unique_ptr security_; + // RouterContext context_; double pos_lat_; double pos_lon_; + // vanetza::PositionFix reference_position_; }; } \ No newline at end of file diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 6ddc439..f80daf8 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -1,4 +1,5 @@ #include "autoware_v2x/v2x_node.hpp" +#include "autoware_v2x/v2x_app.hpp" #include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/router_context.hpp" #include "autoware_v2x/positioning.hpp" @@ -13,9 +14,8 @@ #include #include #include -#include -#include #include +#include namespace gn = vanetza::geonet; @@ -25,23 +25,23 @@ using namespace std::chrono; namespace v2x { - V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : - rclcpp::Node("autoware_v2x_node", node_options), - io_service_(), - trigger_(io_service_), - 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(this)), - context_(mib_, trigger_, *positioning_, security_.get()), - app_() + V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) + // 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(this, runtime_)), + // context_(mib_, trigger_, *positioning_, security_.get()), + // app_() { using std::placeholders::_1; - subscription_ = this->create_subscription("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); + subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); RCLCPP_INFO(get_logger(), "V2X Node Launched"); @@ -50,38 +50,41 @@ namespace v2x // device_(device_name_); // mac_address_ = device_.address(); - std::stringstream sout; - sout << mac_address_; - RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); + // std::stringstream sout; + // sout << mac_address_; + // RCLCPP_INFO(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; + // 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_); + // 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()); + // context_.set_link_layer(link_layer_.get()); // std::unique_ptr cp_ { new CpmApplication(this) }; // app_ = std::move(cp_); - context_.enable(cp_.get()); - + // context_.enable(cp_.get()); // 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 // std::stringstream sout; @@ -94,31 +97,17 @@ namespace v2x // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); // 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); - float x = msg->transforms[0].transform.translation.x; - 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); + app->tfCallback(msg); } }