├── db └── .gitplaceholder ├── docs ├── about │ └── document-build.md ├── design │ ├── architecture.md │ ├── .pages │ └── index.md ├── how-to-guides │ ├── .pages │ └── index.md ├── support │ ├── .pages │ ├── index.md │ └── docs-guide.md ├── tutorials │ ├── .pages │ ├── planning-simulation │ │ ├── .pages │ │ ├── docker-env.png │ │ ├── add-v2x-rviz-1.png │ │ ├── add-v2x-rviz-2.png │ │ ├── add-v2x-rviz-3.png │ │ ├── autoware_1_rviz.png │ │ ├── tools-properties.png │ │ ├── wireshark_awv2x.png │ │ ├── autowarev2x_sender.png │ │ ├── autowarev2x_receiver.png │ │ ├── network-emulation.md │ │ ├── rosbag-and-analysis.md │ │ └── index.md │ ├── index.md │ └── actual-devices │ │ └── index.md ├── installation │ ├── .pages │ ├── index.md │ ├── source-installation.md │ └── docker-installation.md ├── figs │ ├── architecture.png │ ├── exp-setup_v2.png │ ├── high-level-image-v4.png │ └── autowarev2x_architecture_v2.png ├── stylesheets │ └── extra.css ├── .pages └── index.md ├── .gitignore ├── setup.sh ├── config └── autoware_v2x.param.yaml ├── launch └── v2x.launch.xml ├── cyclonedds.xml ├── include └── autoware_v2x │ ├── security.hpp │ ├── positioning.hpp │ ├── time_trigger.hpp │ ├── ethernet_device.hpp │ ├── dcc_passthrough.hpp │ ├── link_layer.hpp │ ├── v2x_app.hpp │ ├── raw_socket_link.hpp │ ├── application.hpp │ ├── v2x_node.hpp │ ├── router_context.hpp │ └── cpm_application.hpp ├── src ├── positioning.cpp ├── link_layer.cpp ├── time_trigger.cpp ├── dcc_passthrough.cpp ├── ethernet_device.cpp ├── application.cpp ├── raw_socket_link.cpp ├── security.cpp ├── router_context.cpp ├── v2x_app.cpp ├── v2x_node.cpp └── cpm_application.cpp ├── package.xml ├── .github └── workflows │ └── deploy-docs.yaml ├── README.md ├── CMakeLists.txt ├── mkdocs.yaml └── cmake └── FindGeographicLib.cmake /db/.gitplaceholder: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/about/document-build.md: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/design/architecture.md: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/design/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md -------------------------------------------------------------------------------- /docs/how-to-guides/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md -------------------------------------------------------------------------------- /docs/support/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md 3 | - docs-guide.md -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | db/*.db 2 | doxygen/html/ 3 | site/ 4 | 5 | .DS_Store -------------------------------------------------------------------------------- /docs/support/index.md: -------------------------------------------------------------------------------- 1 | # Support 2 | 3 | - Contact: yuasabe[at]hongo.wide.ad.jp -------------------------------------------------------------------------------- /docs/how-to-guides/index.md: -------------------------------------------------------------------------------- 1 | # How-to-guides 2 | 3 | !!! Warning 4 | Under Construction -------------------------------------------------------------------------------- /docs/tutorials/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md 3 | - planning-simulation 4 | - actual-devices -------------------------------------------------------------------------------- /setup.sh: -------------------------------------------------------------------------------- 1 | # export CYCLONEDDS_URI=file://$PWD/cyclonedds.xml 2 | export ROS_DOMAIN_ID="$AWID" -------------------------------------------------------------------------------- /docs/installation/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md 3 | - docker-installation.md 4 | - source-installation.md -------------------------------------------------------------------------------- /docs/figs/architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/figs/architecture.png -------------------------------------------------------------------------------- /docs/figs/exp-setup_v2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/figs/exp-setup_v2.png -------------------------------------------------------------------------------- /docs/stylesheets/extra.css: -------------------------------------------------------------------------------- 1 | .md-grid { 2 | max-width: 1920px; 3 | } 4 | 5 | img[alt~=diagram] { width: 60%; } -------------------------------------------------------------------------------- /config/autoware_v2x.param.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | network_interface: "wlp4s0" 4 | is_sender: true -------------------------------------------------------------------------------- /docs/figs/high-level-image-v4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/figs/high-level-image-v4.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - index.md 3 | - rosbag-and-analysis.md 4 | - network-emulation.md -------------------------------------------------------------------------------- /docs/.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - Introduction: index.md 3 | - installation 4 | - tutorials 5 | - how-to-guides 6 | - design 7 | - support -------------------------------------------------------------------------------- /docs/figs/autowarev2x_architecture_v2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/figs/autowarev2x_architecture_v2.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/docker-env.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/docker-env.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/add-v2x-rviz-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/add-v2x-rviz-1.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/add-v2x-rviz-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/add-v2x-rviz-2.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/add-v2x-rviz-3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/add-v2x-rviz-3.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/autoware_1_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/autoware_1_rviz.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/tools-properties.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/tools-properties.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/wireshark_awv2x.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/wireshark_awv2x.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/autowarev2x_sender.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/autowarev2x_sender.png -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/autowarev2x_receiver.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tlab-wide/AutowareV2X/HEAD/docs/tutorials/planning-simulation/autowarev2x_receiver.png -------------------------------------------------------------------------------- /docs/support/docs-guide.md: -------------------------------------------------------------------------------- 1 | # How to generate Documentation 2 | 3 | ## mkdocs 4 | 5 | Run development server. 6 | ``` 7 | mkdocs serve 8 | ``` 9 | 10 | Deploy to gh-pages. 11 | ``` 12 | mkdocs gh-deploy 13 | ``` 14 | 15 | 16 | ## Doxygen 17 | 18 | ``` 19 | doxygen Doxyfile 20 | ``` -------------------------------------------------------------------------------- /launch/v2x.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /cyclonedds.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | eth0 6 | 7 | 8 | -------------------------------------------------------------------------------- /docs/tutorials/index.md: -------------------------------------------------------------------------------- 1 | # Tutorials 2 | 3 | AutowareV2X can be first verified using [Autoware's Planning Simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/). 4 | 5 | - [Running Autoware in the Planning Simulator](./planning-simulation/index.md) 6 | 7 | Then, after you are familiar with how AutowareV2X works, you can use actual devices to test AutowareV2X. 8 | 9 | - [Running AutowareV2X on Actual Devices](./actual-devices/index.md) -------------------------------------------------------------------------------- /include/autoware_v2x/security.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SECURITY_HPP_FV13ZIYA 2 | #define SECURITY_HPP_FV13ZIYA 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | std::unique_ptr 12 | create_security_entity(const vanetza::Runtime&, vanetza::PositionProvider&); 13 | 14 | #endif /* SECURITY_HPP_FV13ZIYA */ 15 | 16 | -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/network-emulation.md: -------------------------------------------------------------------------------- 1 | # Network Emulation 2 | 3 | The `tc` command can be used within the Docker container to emulate various network parameters. 4 | 5 | In `autoware_1` for example, use `tc` to add a delay of 100ms. 6 | ``` 7 | sudo tc qdisc add dev eth0 root netem delay 100ms 8 | ``` 9 | 10 | To remove delay, simply: 11 | ``` 12 | sudo tc qdisc delete dev eth0 root netem delay 100ms 13 | ``` 14 | 15 | To show all qdisc: 16 | ``` 17 | sudo tc qdisc show 18 | ``` 19 | 20 | Documentation about [tc-netem](https://man7.org/linux/man-pages/man8/tc-netem.8.html). -------------------------------------------------------------------------------- /docs/installation/index.md: -------------------------------------------------------------------------------- 1 | # Installing AutowareV2X 2 | 3 | AutowareV2X is used as an add-on module to the open-source autonomous driving stack called [Autoware](https://autowarefoundation.github.io/autoware-documentation/main/). Therefore, in order to properly use AutowareV2X, Autoware must first be installed on the system. 4 | 5 | You can install AutowareV2X directly onto a PC through the [Source Installation](./source-installation.md) or use it as a Docker container through the [Docker Installation](./docker-installation.md). 6 | 7 | !!! Note 8 | In order to proceed with the [Tutorials](../tutorials/index.md), you will need to continue with the [Docker Installatioon](./docker-installation.md). -------------------------------------------------------------------------------- /include/autoware_v2x/positioning.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSITIONING_HPP_VZRIW7PB 2 | #define POSITIONING_HPP_VZRIW7PB 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class PositioningException : public std::runtime_error 13 | { 14 | using std::runtime_error::runtime_error; 15 | }; 16 | 17 | std::unique_ptr 18 | create_position_provider(boost::asio::io_service &, const vanetza::Runtime &); 19 | 20 | #endif /* POSITIONING_HPP_VZRIW7PB */ 21 | -------------------------------------------------------------------------------- /include/autoware_v2x/time_trigger.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TIME_TRIGGER_HPP_XRPGDYXO 2 | #define TIME_TRIGGER_HPP_XRPGDYXO 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class TimeTrigger 10 | { 11 | public: 12 | TimeTrigger(boost::asio::io_service &); 13 | vanetza::Runtime & runtime() { return runtime_; } 14 | void schedule(); 15 | 16 | private: 17 | boost::posix_time::ptime now() const; 18 | void on_timeout(const boost::system::error_code &); 19 | void update_runtime(); 20 | 21 | boost::asio::io_service & io_service_; 22 | boost::asio::deadline_timer timer_; 23 | vanetza::ManualRuntime runtime_; 24 | }; 25 | 26 | #endif /* TIME_TRIGGER_HPP_XRPGDYXO */ 27 | -------------------------------------------------------------------------------- /include/autoware_v2x/ethernet_device.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ETHERNET_DEVICE_HPP_NEVC5DAY 2 | #define ETHERNET_DEVICE_HPP_NEVC5DAY 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class EthernetDevice 9 | { 10 | public: 11 | using protocol = boost::asio::generic::raw_protocol; 12 | 13 | EthernetDevice(const char* devname); 14 | EthernetDevice(const EthernetDevice&) = delete; 15 | EthernetDevice& operator=(const EthernetDevice&) = delete; 16 | ~EthernetDevice(); 17 | 18 | protocol::endpoint endpoint(int family) const; 19 | vanetza::MacAddress address() const; 20 | 21 | private: 22 | int index() const; 23 | 24 | int local_socket_; 25 | std::string interface_name_; 26 | }; 27 | 28 | #endif /* ETHERNET_DEVICE_HPP_NEVC5DAY */ 29 | 30 | -------------------------------------------------------------------------------- /src/positioning.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/positioning.hpp" 2 | #include 3 | 4 | using namespace vanetza; 5 | namespace po = boost::program_options; 6 | 7 | std::unique_ptr create_position_provider(boost::asio::io_service &io_service, const Runtime &runtime) 8 | { 9 | std::unique_ptr positioning; 10 | 11 | std::unique_ptr stored{new StoredPositionProvider()}; 12 | PositionFix fix; 13 | fix.timestamp = runtime.now(); 14 | fix.latitude = 10.0 * units::degree; 15 | fix.longitude = 10.0 * units::degree; 16 | fix.confidence.semi_major = 1.0 * units::si::meter; 17 | fix.confidence.semi_minor = fix.confidence.semi_major; 18 | stored->position_fix(fix); 19 | positioning = std::move(stored); 20 | 21 | return positioning; 22 | } -------------------------------------------------------------------------------- /src/link_layer.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/link_layer.hpp" 2 | #include 3 | #include 4 | #include "autoware_v2x/raw_socket_link.hpp" 5 | 6 | std::unique_ptr create_link_layer( 7 | boost::asio::io_service & io_service, const EthernetDevice & device, const std::string & name) 8 | { 9 | std::unique_ptr link_layer; 10 | 11 | if (name == "ethernet") { 12 | boost::asio::generic::raw_protocol raw_protocol( 13 | AF_PACKET, vanetza::access::ethertype::GeoNetworking.net()); 14 | boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol); 15 | raw_socket.bind(device.endpoint(AF_PACKET)); 16 | link_layer.reset(new RawSocketLink{std::move(raw_socket)}); 17 | } 18 | 19 | // Removed Cohda and UDP Support 20 | 21 | return link_layer; 22 | } 23 | -------------------------------------------------------------------------------- /include/autoware_v2x/dcc_passthrough.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DCC_PASSTHROUGH_HPP_GSDFESAE 2 | #define DCC_PASSTHROUGH_HPP_GSDFESAE 3 | 4 | #include "autoware_v2x/time_trigger.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class DccPassthrough : public vanetza::dcc::RequestInterface 11 | { 12 | public: 13 | DccPassthrough(vanetza::access::Interface&, TimeTrigger& trigger); 14 | 15 | void request(const vanetza::dcc::DataRequest& request, std::unique_ptr packet) override; 16 | 17 | void allow_packet_flow(bool allow); 18 | bool allow_packet_flow(); 19 | 20 | private: 21 | vanetza::access::Interface& access_; 22 | TimeTrigger& trigger_; 23 | bool allow_packet_flow_ = true; 24 | }; 25 | 26 | #endif /* DCC_PASSTHROUGH_HPP_GSDFESAE */ 27 | -------------------------------------------------------------------------------- /include/autoware_v2x/link_layer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LINK_LAYER_HPP_FGEK0QTH 2 | #define LINK_LAYER_HPP_FGEK0QTH 3 | 4 | #include "autoware_v2x/ethernet_device.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class LinkLayerIndication 13 | { 14 | public: 15 | using IndicationCallback = std::function; 16 | 17 | virtual void indicate(IndicationCallback) = 0; 18 | virtual ~LinkLayerIndication() = default; 19 | }; 20 | 21 | class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication 22 | { 23 | }; 24 | 25 | std::unique_ptr 26 | create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name); 27 | 28 | #endif /* LINK_LAYER_HPP_FGEK0QTH */ 29 | 30 | -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/rosbag-and-analysis.md: -------------------------------------------------------------------------------- 1 | # Recording Rosbag and Tcpdump for Analysis 2 | 3 | ## Record both rosbag and tcpdump 4 | 5 | In `autoware_1`: 6 | ``` 7 | docker exec -it autoware_1 bash 8 | cd ~/workspace/autoware_docker 9 | ros2 bag record -o test_sender_rosbag /perception/object_recognition/objects /tf 10 | 11 | sudo apt update 12 | sudo apt install tcpdump 13 | sudo tcpdump -i eth0 -w test_sender_tcpdump.pcap 14 | ``` 15 | 16 | In `autoware_2`: 17 | ``` 18 | docker exec -it autoware_2 bash 19 | cd ~/workspace/autoware_docker 20 | ros2 bag record -o test_receiver_rosbag /v2x/cpm/objects /tf 21 | 22 | sudo apt update 23 | sudo apt install tcpdump 24 | sudo tcpdump -i eth0 -w test_receiver_tcpdump.pcap 25 | ``` 26 | 27 | ## Open PCAP file in Wireshark 28 | 29 | ![](./wireshark_awv2x.png) 30 | 31 | ## Analyze in JupyteLab 32 | 33 | ### Plot the x,y coordinates of objects in test_sender_rosbag 34 | 35 | 1. Export the necessary topics of the Rosbag to a CSV file -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | autoware_v2x 5 | 0.0.0 6 | TODO: Package description 7 | yuasabe 8 | TODO: License declaration 9 | 10 | ament_cmake_auto 11 | 12 | autoware_auto_perception_msgs 13 | tf2_msgs 14 | tf2 15 | geometry_msgs 16 | rclcpp 17 | std_msgs 18 | rclcpp_components 19 | Vanetza 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # AutowareV2X Documentation 2 | 3 | ## About AutowareV2X 4 | 5 | AutowareV2X is an open-source module that can be added onto the newest [Autoware.universe](https://github.com/autowarefoundation/autoware) to enable V2X communication. 6 | 7 | It utilizes [Vanetza](https://github.com/riebl/vanetza) as the protocol suite for ETSI C-ITS standards. 8 | 9 | We have also provided a working example of a CPM application, where Collective Perception Messages can be used to exchange perception information in Autoware. 10 | 11 | ![AutowareV2X Architecture diagram](./figs/autowarev2x_architecture_v2.png) 12 | 13 | AutowareV2X can be used in various V2X scenarios across different radio access channels. 14 | 15 | ![High Level V2X Image diagram](./figs/high-level-image-v4.png) 16 | 17 | ## Getting started 18 | 19 | - [Installation](./installation/index.md) pages explain the installation steps of AutowareV2X and its prerequisites. 20 | - [Tutorials](./tutorials/index.md) pages provide several tutorials to follow after installation. 21 | - [Design](./design/index.md) pages explain the design concept and architecture of AutowareV2X. 22 | - [Support](./support/index.md) pages are the place to go if you need additional help. -------------------------------------------------------------------------------- /.github/workflows/deploy-docs.yaml: -------------------------------------------------------------------------------- 1 | name: deploy-docs 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | paths: 8 | - mkdocs.yaml 9 | - "**/*.md" 10 | - "**/*.svg" 11 | - "**/*.png" 12 | - "**/*.jpg" 13 | pull_request_target: 14 | types: 15 | - opened 16 | - synchronize 17 | - labeled 18 | workflow_dispatch: 19 | 20 | jobs: 21 | prevent-no-label-execution: 22 | uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 23 | with: 24 | label: deploy-docs 25 | 26 | deploy-docs: 27 | needs: prevent-no-label-execution 28 | if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} 29 | runs-on: ubuntu-latest 30 | steps: 31 | - name: Check out repository 32 | uses: actions/checkout@v3 33 | with: 34 | fetch-depth: 0 35 | ref: ${{ github.event.pull_request.head.sha }} 36 | 37 | - name: Deploy docs 38 | uses: autowarefoundation/autoware-github-actions/deploy-docs@v1 39 | with: 40 | token: ${{ secrets.GITHUB_TOKEN }} 41 | latest: ${{ github.event_name != 'pull_request_target' && github.ref_name == github.event.repository.default_branch }} -------------------------------------------------------------------------------- /src/time_trigger.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/time_trigger.hpp" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace asio = boost::asio; 7 | namespace posix_time = boost::posix_time; 8 | using namespace vanetza; 9 | 10 | TimeTrigger::TimeTrigger(asio::io_service & io_service) 11 | : io_service_(io_service), timer_(io_service), runtime_(Clock::at(now())) 12 | { 13 | std::cout << "Starting runtime at " << now() << "\n"; 14 | schedule(); 15 | } 16 | 17 | posix_time::ptime TimeTrigger::now() const { return posix_time::microsec_clock::universal_time(); } 18 | 19 | void TimeTrigger::schedule() 20 | { 21 | update_runtime(); 22 | auto next = runtime_.next(); 23 | if (next < Clock::time_point::max()) { 24 | timer_.expires_at(Clock::at(next)); 25 | timer_.async_wait(std::bind(&TimeTrigger::on_timeout, this, std::placeholders::_1)); 26 | } else { 27 | timer_.cancel(); 28 | } 29 | } 30 | 31 | void TimeTrigger::on_timeout(const boost::system::error_code & ec) 32 | { 33 | if (asio::error::operation_aborted != ec) { 34 | schedule(); 35 | } 36 | } 37 | 38 | void TimeTrigger::update_runtime() 39 | { 40 | auto current_time = now(); 41 | runtime_.trigger(Clock::at(current_time)); 42 | } 43 | -------------------------------------------------------------------------------- /include/autoware_v2x/v2x_app.hpp: -------------------------------------------------------------------------------- 1 | #ifndef V2X_APP_HPP_EUIC2VFR 2 | #define V2X_APP_HPP_EUIC2VFR 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "std_msgs/msg/string.hpp" 6 | #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" 7 | #include "tf2_msgs/msg/tf_message.hpp" 8 | #include 9 | #include "autoware_v2x/cpm_application.hpp" 10 | #include "autoware_v2x/time_trigger.hpp" 11 | #include "autoware_v2x/link_layer.hpp" 12 | #include "autoware_v2x/ethernet_device.hpp" 13 | #include "autoware_v2x/positioning.hpp" 14 | #include "autoware_v2x/security.hpp" 15 | #include "autoware_v2x/router_context.hpp" 16 | // #include "autoware_v2x/v2x_node.hpp" 17 | 18 | namespace v2x 19 | { 20 | class V2XNode; 21 | class V2XApp 22 | { 23 | public: 24 | V2XApp(V2XNode *); 25 | void start(); 26 | void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); 27 | void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); 28 | 29 | CpmApplication *cp; 30 | // V2XNode *v2x_node; 31 | 32 | private: 33 | friend class CpmApplication; 34 | friend class Application; 35 | V2XNode* node_; 36 | bool tf_received_; 37 | int tf_interval_; 38 | bool cp_started_; 39 | }; 40 | } 41 | 42 | #endif -------------------------------------------------------------------------------- /src/dcc_passthrough.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/dcc_passthrough.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "autoware_v2x/time_trigger.hpp" 9 | 10 | using namespace vanetza; 11 | 12 | DccPassthrough::DccPassthrough(access::Interface & access, TimeTrigger & trigger) 13 | : access_(access), trigger_(trigger) 14 | { 15 | } 16 | 17 | void DccPassthrough::request(const dcc::DataRequest & request, std::unique_ptr packet) 18 | { 19 | if (!allow_packet_flow_) { 20 | std::cout << "ignored request because packet flow is suppressed\n"; 21 | return; 22 | } 23 | 24 | trigger_.schedule(); 25 | 26 | access::DataRequest acc_req; 27 | acc_req.ether_type = request.ether_type; 28 | acc_req.source_addr = request.source; 29 | acc_req.destination_addr = request.destination; 30 | acc_req.access_category = dcc::map_profile_onto_ac(request.dcc_profile); 31 | access_.request(acc_req, std::move(packet)); 32 | } 33 | 34 | void DccPassthrough::allow_packet_flow(bool allow) { allow_packet_flow_ = allow; } 35 | 36 | bool DccPassthrough::allow_packet_flow() { return allow_packet_flow_; } 37 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AutowareV2X 2 | [![Collective_Perception_between_RSU_and_Vehicle_using_Autoware (2)](https://github-production-user-asset-6210df.s3.amazonaws.com/3140505/253303730-8662b65b-99af-4793-8c4c-62d457fc9e62.gif)](https://youtu.be/57fx3-gUNxU) 3 | 4 | AutowareV2X is an open-source module that can be added onto the newest Autoware.universe to enable V2X communication. If you find this code useful in your research, please consider citing : 5 | 6 | @inproceedings{Asabe2023b, 7 | title = {AutowareV2X: Reliable V2X Communication and Collective Perception for Autonomous Driving}, 8 | author = {Yu Asabe and Ehsan Javanmardi and Jin Nakazato and Manabu Tsukada and Hiroshi Esaki}, 9 | year = {2023}, 10 | date = {2023-06-20}, 11 | booktitle = {The 2023 IEEE 97th Vehicular Technology Conference (VTC2023-Spring)}, 12 | address = {Florence, Italy}, 13 | } 14 | 15 | It utilizes Vanetza as the protocol suite for ETSI C-ITS standards. 16 | 17 | We have also provided a working example of a CPM application where Collective Perception Messages can be used to exchange perception information in Autoware. 18 | 19 | 20 | 21 | 22 | 23 | ![AutowareV2X Architecture](./docs/figs/autowarev2x_architecture_v2.png) 24 | 25 | See the [official documentation](https://tlab-wide.github.io/AutowareV2X/) 26 | -------------------------------------------------------------------------------- /include/autoware_v2x/raw_socket_link.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RAW_SOCKET_LINK_HPP_VUXH507U 2 | #define RAW_SOCKET_LINK_HPP_VUXH507U 3 | 4 | #include "autoware_v2x/link_layer.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class RawSocketLink : public LinkLayer 13 | { 14 | public: 15 | RawSocketLink(boost::asio::generic::raw_protocol::socket&&); 16 | void request(const vanetza::access::DataRequest&, std::unique_ptr) override; 17 | void indicate(IndicationCallback) override; 18 | 19 | protected: 20 | std::size_t transmit(std::unique_ptr); 21 | virtual boost::optional parse_ethernet_header(vanetza::CohesivePacket&) const; 22 | 23 | private: 24 | void do_receive(); 25 | void on_read(const boost::system::error_code&, std::size_t); 26 | void pass_up(vanetza::CohesivePacket&&); 27 | 28 | static constexpr std::size_t layers_ = num_osi_layers(vanetza::OsiLayer::Physical, vanetza::OsiLayer::Application); 29 | 30 | boost::asio::generic::raw_protocol::socket socket_; 31 | std::array buffers_; 32 | IndicationCallback callback_; 33 | vanetza::ByteBuffer receive_buffer_; 34 | boost::asio::generic::raw_protocol::endpoint receive_endpoint_; 35 | }; 36 | 37 | #endif /* RAW_SOCKET_LINK_HPP_VUXH507U */ 38 | 39 | -------------------------------------------------------------------------------- /include/autoware_v2x/application.hpp: -------------------------------------------------------------------------------- 1 | #ifndef APPLICATION_HPP_PSIGPUTG 2 | #define APPLICATION_HPP_PSIGPUTG 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "rclcpp/rclcpp.hpp" 11 | 12 | class Application : public vanetza::btp::IndicationInterface 13 | { 14 | public: 15 | using DataConfirm = vanetza::geonet::DataConfirm; 16 | using DataIndication = vanetza::btp::DataIndication; 17 | using DataRequest = vanetza::btp::DataRequestGeoNetParams; 18 | using DownPacketPtr = vanetza::geonet::Router::DownPacketPtr; 19 | using PortType = vanetza::btp::port_type; 20 | using PromiscuousHook = vanetza::btp::PortDispatcher::PromiscuousHook; 21 | using UpPacketPtr = vanetza::geonet::Router::UpPacketPtr; 22 | 23 | Application() = default; 24 | Application(const Application &) = delete; 25 | Application &operator=(const Application &) = delete; 26 | virtual ~Application() = default; 27 | 28 | virtual PortType port() = 0; 29 | virtual PromiscuousHook *promiscuous_hook(); 30 | 31 | protected: 32 | DataConfirm request(const DataRequest &, DownPacketPtr, rclcpp::Node*); 33 | 34 | private: 35 | friend class RouterContext; 36 | vanetza::geonet::GbcDataRequest request_gbc(const DataRequest &); 37 | vanetza::geonet::ShbDataRequest request_shb(const DataRequest &); 38 | vanetza::geonet::Router *router_; 39 | }; 40 | 41 | #endif /* APPLICATION_HPP_PSIGPUTG */ 42 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(autoware_v2x) 3 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 4 | 5 | # Default to C99 6 | if(NOT CMAKE_C_STANDARD) 7 | set(CMAKE_C_STANDARD 99) 8 | endif() 9 | 10 | # Default to C++14 11 | if(NOT CMAKE_CXX_STANDARD) 12 | set(CMAKE_CXX_STANDARD 14) 13 | endif() 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | add_compile_options(-Wall -Wextra -Wpedantic) 17 | endif() 18 | 19 | # find dependencies 20 | find_package(ament_cmake_auto REQUIRED) 21 | set(VANETZA_INSTALL ON) 22 | option(BUILD_SHARED_LIBS "Build shared libraries" ON) 23 | find_package(Vanetza REQUIRED) 24 | find_package(GeographicLib 1.37 REQUIRED) 25 | find_package(Boost COMPONENTS thread REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | find_package(std_msgs REQUIRED) 28 | 29 | include_directories( 30 | include 31 | ) 32 | 33 | ament_auto_add_library(autoware_v2x SHARED 34 | src/v2x_node.cpp 35 | src/v2x_app.cpp 36 | src/application.cpp 37 | src/cpm_application.cpp 38 | src/ethernet_device.cpp 39 | src/link_layer.cpp 40 | src/raw_socket_link.cpp 41 | src/router_context.cpp 42 | src/dcc_passthrough.cpp 43 | src/time_trigger.cpp 44 | src/positioning.cpp 45 | src/security.cpp 46 | ) 47 | 48 | target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3) 49 | 50 | rclcpp_components_register_node(autoware_v2x 51 | PLUGIN "v2x::V2XNode" 52 | EXECUTABLE autoware_v2x_node 53 | ) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | ament_lint_auto_find_test_dependencies() 58 | endif() 59 | 60 | ament_auto_package(INSTALL_TO_SHARE 61 | launch 62 | config 63 | ) -------------------------------------------------------------------------------- /include/autoware_v2x/v2x_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef V2X_NODE_HPP_EUIC2VFR 2 | #define V2X_NODE_HPP_EUIC2VFR 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "std_msgs/msg/string.hpp" 6 | #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" 7 | #include "tf2_msgs/msg/tf_message.hpp" 8 | #include 9 | #include "autoware_v2x/v2x_app.hpp" 10 | #include "autoware_v2x/cpm_application.hpp" 11 | #include "autoware_v2x/time_trigger.hpp" 12 | #include "autoware_v2x/link_layer.hpp" 13 | #include "autoware_v2x/ethernet_device.hpp" 14 | #include "autoware_v2x/positioning.hpp" 15 | #include "autoware_v2x/security.hpp" 16 | #include "autoware_v2x/router_context.hpp" 17 | #include 18 | 19 | namespace v2x 20 | { 21 | class V2XNode : public rclcpp::Node 22 | { 23 | public: 24 | explicit V2XNode(const rclcpp::NodeOptions &node_options); 25 | V2XApp *app; 26 | void publishObjects(std::vector *, int cpm_num); 27 | void publishCpmSenderObject(double, double, double); 28 | 29 | std::ofstream latency_log_file; 30 | 31 | private: 32 | void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); 33 | void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); 34 | 35 | rclcpp::Subscription::SharedPtr objects_sub_; 36 | rclcpp::Subscription::SharedPtr tf_sub_; 37 | rclcpp::Publisher::SharedPtr cpm_objects_pub_; 38 | rclcpp::Publisher::SharedPtr cpm_sender_pub_; 39 | 40 | double pos_lat_; 41 | double pos_lon_; 42 | }; 43 | } 44 | 45 | #endif -------------------------------------------------------------------------------- /src/ethernet_device.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/ethernet_device.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | static void initialize(ifreq& request, const char* interface_name) 11 | { 12 | std::memset(&request, 0, sizeof(ifreq)); 13 | std::strncpy(request.ifr_name, interface_name, IF_NAMESIZE); 14 | request.ifr_name[IF_NAMESIZE - 1] = '\0'; 15 | } 16 | 17 | EthernetDevice::EthernetDevice(const char* devname) : 18 | local_socket_(::socket(AF_LOCAL, SOCK_DGRAM, 0)), 19 | interface_name_(devname) 20 | { 21 | if (!local_socket_) { 22 | throw std::system_error(errno, std::system_category()); 23 | } 24 | } 25 | 26 | EthernetDevice::~EthernetDevice() 27 | { 28 | if (local_socket_ >= 0) 29 | ::close(local_socket_); 30 | } 31 | 32 | EthernetDevice::protocol::endpoint EthernetDevice::endpoint(int family) const 33 | { 34 | sockaddr_ll socket_address = {0}; 35 | socket_address.sll_family = family; 36 | socket_address.sll_protocol = htons(ETH_P_ALL); 37 | socket_address.sll_ifindex = index(); 38 | return protocol::endpoint(&socket_address, sizeof(sockaddr_ll)); 39 | } 40 | 41 | int EthernetDevice::index() const 42 | { 43 | ifreq data; 44 | initialize(data, interface_name_.c_str()); 45 | ::ioctl(local_socket_, SIOCGIFINDEX, &data); 46 | return data.ifr_ifindex; 47 | } 48 | 49 | vanetza::MacAddress EthernetDevice::address() const 50 | { 51 | ifreq data; 52 | initialize(data, interface_name_.c_str()); 53 | ::ioctl(local_socket_, SIOCGIFHWADDR, &data); 54 | 55 | vanetza::MacAddress addr; 56 | std::copy_n(data.ifr_hwaddr.sa_data, addr.octets.size(), addr.octets.data()); 57 | return addr; 58 | } 59 | -------------------------------------------------------------------------------- /include/autoware_v2x/router_context.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROUTER_CONTEXT_HPP_KIPUYBY2 2 | #define ROUTER_CONTEXT_HPP_KIPUYBY2 3 | 4 | #include "autoware_v2x/dcc_passthrough.hpp" 5 | #include "autoware_v2x/link_layer.hpp" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class Application; 15 | class TimeTrigger; 16 | 17 | class RouterContext 18 | { 19 | public: 20 | RouterContext(const vanetza::geonet::MIB&, TimeTrigger&, vanetza::PositionProvider&, vanetza::security::SecurityEntity*); 21 | ~RouterContext(); 22 | void enable(Application*); 23 | void disable(Application*); 24 | 25 | /** 26 | * Allow/disallow transmissions without GNSS position fix 27 | * 28 | * \param flag true if transmissions shall be dropped when no GNSS position fix is available 29 | */ 30 | void require_position_fix(bool flag); 31 | 32 | void set_link_layer(LinkLayer*); 33 | void updateMIB(const vanetza::geonet::MIB&); 34 | 35 | private: 36 | void indicate(vanetza::CohesivePacket&& packet, const vanetza::EthernetHeader& hdr); 37 | void log_packet_drop(vanetza::geonet::Router::PacketDropReason); 38 | void update_position_vector(); 39 | void update_packet_flow(const vanetza::geonet::LongPositionVector&); 40 | 41 | vanetza::geonet::MIB mib_; 42 | vanetza::geonet::Router router_; 43 | TimeTrigger& trigger_; 44 | vanetza::PositionProvider& positioning_; 45 | vanetza::btp::PortDispatcher dispatcher_; 46 | std::unique_ptr request_interface_; 47 | std::list applications_; 48 | bool require_position_fix_ = false; 49 | }; 50 | 51 | #endif /* ROUTER_CONTEXT_HPP_KIPUYBY2 */ 52 | -------------------------------------------------------------------------------- /mkdocs.yaml: -------------------------------------------------------------------------------- 1 | site_name: AutowareV2X Documentation 2 | site_url: https://tlab-wide.github.io/AutowareV2X 3 | repo_url: https://github.com/tlab-wide/AutowareV2X 4 | docs_dir: docs 5 | copyright: Copyright © 2023 Yu Asabe 6 | 7 | theme: 8 | name: material 9 | features: 10 | - navigation.expand 11 | - navigation.indexes 12 | - navigation.instant 13 | - navigation.sections 14 | - navigation.tabs 15 | - navigation.tabs.sticky 16 | - navigation.top 17 | palette: 18 | - scheme: default 19 | primary: white 20 | toggle: 21 | icon: material/weather-sunny 22 | name: Switch to dark mode 23 | - scheme: slate 24 | primary: grey 25 | toggle: 26 | icon: material/weather-night 27 | name: Switch to light mode 28 | language: en 29 | 30 | extra: 31 | font: 32 | text: Roboto 33 | code: Roboto Mono 34 | version: 35 | provider: mike 36 | 37 | extra_css: 38 | - https://use.fontawesome.com/releases/v5.15.4/css/all.css 39 | - stylesheets/extra.css 40 | 41 | extra_javascript: 42 | - https://cdn.jsdelivr.net/npm/mathjax@2/MathJax.js?config=TeX-AMS-MML_HTMLorMML 43 | 44 | plugins: 45 | - awesome-pages 46 | - search 47 | 48 | markdown_extensions: 49 | - abbr 50 | - admonition 51 | - attr_list 52 | - codehilite: 53 | guess_lang: false 54 | - fontawesome_markdown 55 | - footnotes 56 | - mdx_math 57 | - plantuml_markdown: 58 | server: http://www.plantuml.com/plantuml 59 | format: svg 60 | - pymdownx.arithmatex 61 | - pymdownx.details 62 | - pymdownx.highlight 63 | - pymdownx.snippets: 64 | auto_append: 65 | - includes/abbreviations.md 66 | - pymdownx.superfences: 67 | custom_fences: 68 | - name: mermaid 69 | class: mermaid 70 | format: !!python/name:pymdownx.superfences.fence_code_format 71 | - toc: 72 | permalink: "#" 73 | toc_depth: 3 -------------------------------------------------------------------------------- /docs/design/index.md: -------------------------------------------------------------------------------- 1 | # Design 2 | 3 | !!! Warning 4 | More to come 5 | 6 | A V2X communication software stack called [Vanetza](https://github.com/riebl/vanetza) is integrated into the standalone autonomous driving software stack, [Autoware](https://github.com/autowarefoundation/autoware). The V2X stack and the autonomous driving stack can be decoupled, allowing other applications to utilize the V2X router as well. A high-level overview of the architecture is shown below. 7 | 8 | ![AutowareV2X Architecture diagram](../figs/autowarev2x_architecture_v2.png) 9 | 10 | Autoware is responsible for the perception task, while AutowareV2X manages the transmission and reception of messages over the V2X channel. Services that are necessary for the integration of Vanetza into Autoware were newly developed. 11 | 12 | The V2XApp is responsible for managing the various facilities such as DENM, CAM, CPM, while the V2XNode handles the conversion of information between the V2X messages and ROS2 messages. 13 | 14 | ## V2XNode 15 | 16 | The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as the bridge interface between Autoware and AutowareV2X. Information that is to be utilized in V2X Applications are retreived from Autoware in the form of ROS2 topics. Similarily, information that is received by AutowareV2X through V2X communications is published as ROS2 topics in order to feed it back into Autoware. 17 | 18 | ### Input 19 | 20 | | Name | Type | Description | 21 | | -------------------- | ------------------------------- | ---------------- | 22 | | `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware | 23 | | `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle | 24 | 25 | ### Output 26 | 27 | | Name | Type | Description | 28 | | -------------------- | ------------------------------- | ---------------- | 29 | | `/v2x/cpm/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs | 30 | 31 | ### Functions 32 | 33 | | Name | Description | 34 | | -------------------- | ---------------- | 35 | | `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` | 36 | | `tfCallback` | Call `V2XApp::tfCallback` | 37 | | `publishObjects(std::vector *objectsStack, int cpm_num)` | | 38 | | `publishCpmSenderObject` | Not used now | 39 | 40 | 41 | ## V2XApp 42 | 43 | ## CPM Facility -------------------------------------------------------------------------------- /src/application.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/application.hpp" 2 | #include "rclcpp/rclcpp.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace vanetza; 8 | 9 | Application::DataConfirm Application::request(const DataRequest &request, DownPacketPtr packet, rclcpp::Node* node) 10 | { 11 | // RCLCPP_INFO(node->get_logger(), "Inside Application::request 0"); 12 | DataConfirm confirm(DataConfirm::ResultCode::Rejected_Unspecified); 13 | // RCLCPP_INFO(node->get_logger(), "Inside Application::request 1"); 14 | if (packet) { 15 | // RCLCPP_INFO(node->get_logger(), "Inside Application::request 2"); 16 | } 17 | if (router_ && packet) 18 | { 19 | // RCLCPP_INFO(node->get_logger(), "Inside Application::request 3"); 20 | btp::HeaderB btp_header; 21 | btp_header.destination_port = this->port(); 22 | btp_header.destination_port_info = host_cast(0); 23 | packet->layer(OsiLayer::Transport) = btp_header; 24 | 25 | switch (request.transport_type) 26 | { 27 | case geonet::TransportType::SHB: 28 | // RCLCPP_INFO(node->get_logger(), "Inside Application::request 4"); 29 | confirm = router_->request(request_shb(request), std::move(packet)); 30 | break; 31 | case geonet::TransportType::GBC: 32 | confirm = router_->request(request_gbc(request), std::move(packet)); 33 | break; 34 | default: 35 | // TODO remaining transport types are not implemented 36 | break; 37 | } 38 | } 39 | 40 | return confirm; 41 | } 42 | 43 | void initialize_request(const Application::DataRequest &generic, geonet::DataRequest &geonet) 44 | { 45 | geonet.upper_protocol = geonet::UpperProtocol::BTP_B; 46 | geonet.communication_profile = generic.communication_profile; 47 | geonet.its_aid = generic.its_aid; 48 | if (generic.maximum_lifetime) 49 | { 50 | geonet.maximum_lifetime = generic.maximum_lifetime.get(); 51 | } 52 | geonet.repetition = generic.repetition; 53 | geonet.traffic_class = generic.traffic_class; 54 | } 55 | 56 | geonet::GbcDataRequest Application::request_gbc(const DataRequest &generic) 57 | { 58 | assert(router_); 59 | geonet::GbcDataRequest gbc(router_->get_mib()); 60 | initialize_request(generic, gbc); 61 | gbc.destination = boost::get(generic.destination); 62 | return gbc; 63 | } 64 | 65 | geonet::ShbDataRequest Application::request_shb(const DataRequest &generic) 66 | { 67 | assert(router_); 68 | geonet::ShbDataRequest shb(router_->get_mib()); 69 | initialize_request(generic, shb); 70 | return shb; 71 | } 72 | 73 | Application::PromiscuousHook *Application::promiscuous_hook() 74 | { 75 | return nullptr; 76 | } 77 | -------------------------------------------------------------------------------- /src/raw_socket_link.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/raw_socket_link.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace vanetza; 8 | 9 | RawSocketLink::RawSocketLink(boost::asio::generic::raw_protocol::socket && socket) 10 | : socket_(std::move(socket)), 11 | receive_buffer_(2048, 0x00), 12 | receive_endpoint_(socket_.local_endpoint()) 13 | { 14 | do_receive(); 15 | } 16 | 17 | void RawSocketLink::request( 18 | const access::DataRequest & request, std::unique_ptr packet) 19 | { 20 | packet->layer(OsiLayer::Link) = 21 | create_ethernet_header(request.destination_addr, request.source_addr, request.ether_type); 22 | transmit(std::move(packet)); 23 | } 24 | 25 | std::size_t RawSocketLink::transmit(std::unique_ptr packet) 26 | { 27 | std::array const_buffers; 28 | for (auto & layer : osi_layer_range()) { 29 | const auto index = distance(OsiLayer::Physical, layer); 30 | packet->layer(layer).convert(buffers_[index]); 31 | const_buffers[index] = boost::asio::buffer(buffers_[index]); 32 | } 33 | 34 | return socket_.send(const_buffers); 35 | } 36 | 37 | void RawSocketLink::indicate(IndicationCallback callback) { 38 | callback_ = callback; 39 | } 40 | 41 | void RawSocketLink::do_receive() 42 | { 43 | namespace sph = std::placeholders; 44 | socket_.async_receive_from( 45 | boost::asio::buffer(receive_buffer_), receive_endpoint_, 46 | std::bind(&RawSocketLink::on_read, this, sph::_1, sph::_2)); 47 | } 48 | 49 | void RawSocketLink::on_read(const boost::system::error_code & ec, std::size_t read_bytes) 50 | { 51 | if (!ec) { 52 | ByteBuffer buffer(receive_buffer_.begin(), receive_buffer_.begin() + read_bytes); 53 | CohesivePacket packet(std::move(buffer), OsiLayer::Physical); 54 | boost::optional eth = parse_ethernet_header(packet); 55 | if (callback_ && eth) { 56 | callback_(std::move(packet), *eth); 57 | } 58 | do_receive(); 59 | } 60 | } 61 | 62 | boost::optional RawSocketLink::parse_ethernet_header( 63 | vanetza::CohesivePacket & packet) const 64 | { 65 | packet.set_boundary(OsiLayer::Physical, 0); 66 | if (packet.size(OsiLayer::Link) < EthernetHeader::length_bytes) { 67 | std::cerr << "Router dropped invalid packet (too short for Ethernet header)\n"; 68 | } else { 69 | packet.set_boundary(OsiLayer::Link, EthernetHeader::length_bytes); 70 | auto link_range = packet[OsiLayer::Link]; 71 | return decode_ethernet_header(link_range.begin(), link_range.end()); 72 | } 73 | 74 | return boost::none; 75 | } 76 | -------------------------------------------------------------------------------- /cmake/FindGeographicLib.cmake: -------------------------------------------------------------------------------- 1 | option(GeographicLib_PREFER_PACKAGE_CONFIG "Prefer CMake config mode" ON) 2 | mark_as_advanced(GeographicLib_PREFER_PACKAGE_CONFIG) 3 | 4 | if(GeographicLib_PREFER_PACKAGE_CONFIG) 5 | find_package(GeographicLib ${GeographicLib_FIND_VERSION} QUIET CONFIG) 6 | if (GeographicLib_FOUND) 7 | message(STATUS "GeographicLib: using found CMake configuration") 8 | return() 9 | endif() 10 | endif() 11 | message(STATUS "GeographicLib: using lookup code by Vanetza") 12 | 13 | find_path(GeographicLib_INCLUDE_DIR NAMES GeographicLib/Config.h DOC "GeographicLib include directory") 14 | find_library(GeographicLib_LIBRARY_RELEASE NAMES Geographic DOC "GeographicLib library (release)") 15 | find_library(GeographicLib_LIBRARY_DEBUG NAMES Geographic_d DOC "GeographicLib library (debug)") 16 | 17 | include(SelectLibraryConfigurations) 18 | select_library_configurations(GeographicLib) 19 | 20 | if(GeographicLib_INCLUDE_DIR) 21 | file(STRINGS ${GeographicLib_INCLUDE_DIR}/GeographicLib/Config.h _config_version REGEX "GEOGRAPHICLIB_VERSION_STRING") 22 | string(REGEX MATCH "\"([0-9.]+)\"$" _match_version ${_config_version}) 23 | set(GeographicLib_VERSION_STRING ${CMAKE_MATCH_1}) 24 | endif() 25 | 26 | include(FindPackageHandleStandardArgs) 27 | find_package_handle_standard_args(GeographicLib 28 | REQUIRED_VARS GeographicLib_INCLUDE_DIR GeographicLib_LIBRARY 29 | FOUND_VAR GeographicLib_FOUND 30 | VERSION_VAR GeographicLib_VERSION_STRING) 31 | 32 | if(GeographicLib_FOUND AND NOT TARGET GeographicLib::GeographicLib) 33 | add_library(GeographicLib::GeographicLib UNKNOWN IMPORTED) 34 | set_target_properties(GeographicLib::GeographicLib PROPERTIES 35 | INTERFACE_INCLUDE_DIRECTORIES "${GeographicLib_INCLUDE_DIR}" 36 | IMPORTED_LOCATION "${GeographicLib_LIBRARY}" 37 | IMPORTED_LINK_INTERFACE_LANGUAGES "CXX") 38 | if(EXISTS "${GeographicLib_LIBRARY_RELEASE}") 39 | set_property(TARGET GeographicLib::GeographicLib APPEND PROPERTY 40 | IMPORTED_CONFIGURATIONS RELEASE) 41 | set_target_properties(GeographicLib::GeographicLib PROPERTIES 42 | IMPORTED_LOCATION_RELEASE "${GeographicLib_LIBRARY_RELEASE}") 43 | endif() 44 | if(EXISTS "${GeographicLib_LIBRARY_DEBUG}") 45 | set_property(TARGET GeographicLib::GeographicLib APPEND PROPERTY 46 | IMPORTED_CONFIGURATIONS DEBUG) 47 | set_target_properties(GeographicLib::GeographicLib PROPERTIES 48 | IMPORTED_LOCATION_DEBUG "${GeographicLib_LIBRARY_DEBUG}") 49 | endif() 50 | endif() 51 | 52 | mark_as_advanced(GeographicLib_INCLUDE_DIR GeographicLib_LIBRARY) 53 | set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) 54 | # GeographicLib_LIBRARIES is set by SelectLibraryConfigurations 55 | -------------------------------------------------------------------------------- /src/security.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/security.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace vanetza; 14 | namespace po = boost::program_options; 15 | 16 | class SecurityContext : public security::SecurityEntity 17 | { 18 | public: 19 | SecurityContext(const Runtime &runtime, PositionProvider &positioning) : runtime(runtime), positioning(positioning), 20 | backend(security::create_backend("default")), 21 | sign_header_policy(runtime, positioning), 22 | cert_cache(runtime), 23 | cert_validator(*backend, cert_cache, trust_store) 24 | { 25 | } 26 | 27 | security::EncapConfirm encapsulate_packet(security::EncapRequest &&request) override 28 | { 29 | if (!entity) 30 | { 31 | throw std::runtime_error("security entity is not ready"); 32 | } 33 | return entity->encapsulate_packet(std::move(request)); 34 | } 35 | 36 | security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override 37 | { 38 | if (!entity) 39 | { 40 | throw std::runtime_error("security entity is not ready"); 41 | } 42 | return entity->decapsulate_packet(std::move(request)); 43 | } 44 | 45 | void build_entity() 46 | { 47 | if (!cert_provider) 48 | { 49 | throw std::runtime_error("certificate provider is missing"); 50 | } 51 | security::SignService sign_service = straight_sign_service(*cert_provider, *backend, sign_header_policy); 52 | security::VerifyService verify_service = straight_verify_service(runtime, *cert_provider, cert_validator, 53 | *backend, cert_cache, sign_header_policy, positioning); 54 | entity.reset(new security::DelegatingSecurityEntity{sign_service, verify_service}); 55 | } 56 | 57 | const Runtime &runtime; 58 | PositionProvider &positioning; 59 | std::unique_ptr backend; 60 | std::unique_ptr entity; 61 | std::unique_ptr cert_provider; 62 | security::DefaultSignHeaderPolicy sign_header_policy; 63 | security::TrustStore trust_store; 64 | security::CertificateCache cert_cache; 65 | security::DefaultCertificateValidator cert_validator; 66 | }; 67 | 68 | std::unique_ptr 69 | create_security_entity(const Runtime &runtime, PositionProvider &positioning) 70 | { 71 | std::unique_ptr security; 72 | 73 | return security; 74 | } 75 | -------------------------------------------------------------------------------- /include/autoware_v2x/cpm_application.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CPM_APPLICATION_HPP_EUIC2VFR 2 | #define CPM_APPLICATION_HPP_EUIC2VFR 3 | 4 | #include "autoware_v2x/application.hpp" 5 | #include "rclcpp/rclcpp.hpp" 6 | #include 7 | #include 8 | #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" 9 | #include "autoware_v2x/positioning.hpp" 10 | #include 11 | 12 | namespace v2x 13 | { 14 | class V2XNode; 15 | class CpmApplication : public Application 16 | { 17 | public: 18 | CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); 19 | PortType port() override; 20 | std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&); 21 | void indicate(const DataIndication &, UpPacketPtr) override; 22 | void set_interval(vanetza::Clock::duration); 23 | void setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); 24 | void updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); 25 | void updateMGRS(double *, double *); 26 | void updateRP(double *, double *, double *); 27 | void updateGenerationTime(int *, long *); 28 | void updateHeading(double *); 29 | void printObjectsList(int); 30 | void send(); 31 | 32 | /** Creates the database schema. 33 | * Creates tables for cpm_sent, cpm_received 34 | */ 35 | void createTables(); 36 | 37 | /** Inserts CPM into the database. 38 | * Constructs and executes queries for inserting the specified CPM data. 39 | * @param cpm The CPM to be inserted 40 | * @param table_name The table to insert the CPM into (cpm_sent or cpm_received) 41 | */ 42 | void insertCpmToCpmTable(vanetza::asn1::Cpm, char*); 43 | 44 | struct Object { 45 | std::string uuid; 46 | int objectID; // 0-255 for CPM 47 | vanetza::Clock::time_point timestamp; 48 | rclcpp::Time timestamp_ros; 49 | double position_x; 50 | double position_y; 51 | double position_z; 52 | double orientation_x; 53 | double orientation_y; 54 | double orientation_z; 55 | double orientation_w; 56 | double twist_linear_x; 57 | double twist_linear_y; 58 | double twist_angular_x; 59 | double twist_angular_y; 60 | int shape_x; 61 | int shape_y; 62 | int shape_z; 63 | int xDistance; 64 | int yDistance; 65 | int xSpeed; 66 | int ySpeed; 67 | int yawAngle; 68 | vanetza::PositionFix position; 69 | int timeOfMeasurement; 70 | bool to_send; 71 | int to_send_trigger; 72 | }; 73 | std::vector objectsList; 74 | std::vector receivedObjectsStack; 75 | 76 | private: 77 | void schedule_timer(); 78 | void on_timer(vanetza::Clock::time_point); 79 | 80 | V2XNode *node_; 81 | vanetza::Runtime &runtime_; 82 | vanetza::Clock::duration cpm_interval_; 83 | 84 | struct Ego_station { 85 | double mgrs_x; 86 | double mgrs_y; 87 | double latitude; 88 | double longitude; 89 | double altitude; 90 | double heading; 91 | }; 92 | 93 | Ego_station ego_; 94 | 95 | int generationTime_; 96 | long gdt_timestamp_; 97 | 98 | double objectConfidenceThreshold_; 99 | 100 | bool updating_objects_list_; 101 | bool sending_; 102 | bool is_sender_; 103 | bool reflect_packet_; 104 | bool include_all_persons_and_animals_; 105 | 106 | int cpm_num_; 107 | int received_cpm_num_; 108 | 109 | int cpm_object_id_; 110 | 111 | bool use_dynamic_generation_rules_; 112 | }; 113 | } 114 | 115 | #endif /* CPM_APPLICATION_HPP_EUIC2VFR */ 116 | -------------------------------------------------------------------------------- /src/router_context.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/application.hpp" 2 | #include "autoware_v2x/dcc_passthrough.hpp" 3 | #include "autoware_v2x/ethernet_device.hpp" 4 | #include "autoware_v2x/router_context.hpp" 5 | #include "autoware_v2x/time_trigger.hpp" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace vanetza; 13 | 14 | RouterContext::RouterContext(const geonet::MIB &mib, TimeTrigger &trigger, vanetza::PositionProvider &positioning, vanetza::security::SecurityEntity *security_entity) : mib_(mib), router_(trigger.runtime(), mib_), trigger_(trigger), positioning_(positioning) 15 | { 16 | router_.packet_dropped = std::bind(&RouterContext::log_packet_drop, this, std::placeholders::_1); 17 | router_.set_address(mib_.itsGnLocalGnAddr); 18 | router_.set_transport_handler(geonet::UpperProtocol::BTP_B, &dispatcher_); 19 | router_.set_security_entity(security_entity); 20 | mib_.vanetzaDisableBeaconing = true; 21 | update_position_vector(); 22 | 23 | trigger_.schedule(); 24 | } 25 | 26 | RouterContext::~RouterContext() 27 | { 28 | for (auto *app : applications_) 29 | { 30 | disable(app); 31 | } 32 | } 33 | 34 | void RouterContext::updateMIB(const geonet::MIB &mib) { 35 | mib_ = mib; 36 | router_.set_address(mib_.itsGnLocalGnAddr); 37 | } 38 | 39 | void RouterContext::log_packet_drop(geonet::Router::PacketDropReason reason) 40 | { 41 | auto reason_string = stringify(reason); 42 | std::cout << "Router dropped packet because of " << reason_string << " (" << static_cast(reason) << ")\n"; 43 | } 44 | 45 | void RouterContext::set_link_layer(LinkLayer *link_layer) 46 | { 47 | namespace dummy = std::placeholders; 48 | 49 | if (link_layer) 50 | { 51 | request_interface_.reset(new DccPassthrough{*link_layer, trigger_}); 52 | router_.set_access_interface(request_interface_.get()); 53 | link_layer->indicate(std::bind(&RouterContext::indicate, this, dummy::_1, dummy::_2)); 54 | update_packet_flow(router_.get_local_position_vector()); 55 | } 56 | else 57 | { 58 | router_.set_access_interface(nullptr); 59 | request_interface_.reset(); 60 | } 61 | } 62 | 63 | void RouterContext::indicate(CohesivePacket &&packet, const EthernetHeader &hdr) 64 | { 65 | if (hdr.source != mib_.itsGnLocalGnAddr.mid() && hdr.type == access::ethertype::GeoNetworking) 66 | { 67 | std::cout << "received packet from " << hdr.source << " (" << packet.size() << " bytes)\n"; 68 | std::unique_ptr up{new PacketVariant(std::move(packet))}; 69 | trigger_.schedule(); // ensure the clock is up-to-date for the security entity 70 | router_.indicate(std::move(up), hdr.source, hdr.destination); 71 | trigger_.schedule(); // schedule packet forwarding 72 | } 73 | } 74 | 75 | void RouterContext::enable(Application *app) 76 | { 77 | app->router_ = &router_; 78 | 79 | dispatcher_.add_promiscuous_hook(app->promiscuous_hook()); 80 | if (app->port() != btp::port_type(0)) 81 | { 82 | dispatcher_.set_non_interactive_handler(app->port(), app); 83 | } 84 | } 85 | 86 | void RouterContext::disable(Application *app) 87 | { 88 | if (app->port() != btp::port_type(0)) 89 | { 90 | dispatcher_.set_non_interactive_handler(app->port(), nullptr); 91 | } 92 | dispatcher_.remove_promiscuous_hook(app->promiscuous_hook()); 93 | 94 | app->router_ = nullptr; 95 | } 96 | 97 | void RouterContext::require_position_fix(bool flag) 98 | { 99 | require_position_fix_ = flag; 100 | update_packet_flow(router_.get_local_position_vector()); 101 | } 102 | 103 | void RouterContext::update_position_vector() 104 | { 105 | router_.update_position(positioning_.position_fix()); 106 | vanetza::Runtime::Callback callback = [this](vanetza::Clock::time_point) 107 | { this->update_position_vector(); }; 108 | vanetza::Clock::duration next = std::chrono::seconds(1); 109 | trigger_.runtime().schedule(next, callback); 110 | trigger_.schedule(); 111 | 112 | update_packet_flow(router_.get_local_position_vector()); 113 | } 114 | 115 | void RouterContext::update_packet_flow(const geonet::LongPositionVector &lpv) 116 | { 117 | if (request_interface_) 118 | { 119 | if (require_position_fix_) 120 | { 121 | // Skip all requests until a valid GPS position is available 122 | request_interface_->allow_packet_flow(lpv.position_accuracy_indicator); 123 | } 124 | else 125 | { 126 | request_interface_->allow_packet_flow(true); 127 | } 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /src/v2x_app.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/v2x_app.hpp" 2 | #include "autoware_v2x/v2x_node.hpp" 3 | #include "autoware_v2x/time_trigger.hpp" 4 | #include "autoware_v2x/router_context.hpp" 5 | #include "autoware_v2x/positioning.hpp" 6 | #include "autoware_v2x/security.hpp" 7 | #include "autoware_v2x/link_layer.hpp" 8 | #include "autoware_v2x/cpm_application.hpp" 9 | 10 | #include "rclcpp/rclcpp.hpp" 11 | #include "std_msgs/msg/string.hpp" 12 | 13 | #include "tf2/LinearMath/Quaternion.h" 14 | #include "tf2/LinearMath/Matrix3x3.h" 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace gn = vanetza::geonet; 25 | 26 | using namespace vanetza; 27 | using namespace std::chrono; 28 | 29 | namespace v2x 30 | { 31 | V2XApp::V2XApp(V2XNode *node) : 32 | node_(node), 33 | tf_received_(false), 34 | tf_interval_(0), 35 | cp_started_(false) 36 | { 37 | } 38 | 39 | void V2XApp::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { 40 | // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); 41 | if (!tf_received_) { 42 | RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); 43 | } 44 | if (tf_received_ && cp_started_) { 45 | cp->updateObjectsList(msg); 46 | } 47 | } 48 | 49 | void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { 50 | 51 | tf_received_ = true; 52 | 53 | double x = msg->transforms[0].transform.translation.x; 54 | double y = msg->transforms[0].transform.translation.y; 55 | double z = msg->transforms[0].transform.translation.z; 56 | 57 | long timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds 58 | long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds 59 | timestamp_sec -= 1072915200; // convert to etsi-epoch 60 | long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; 61 | int gdt = timestamp_msec % 65536; 62 | 63 | double rot_x = msg->transforms[0].transform.rotation.x; 64 | double rot_y = msg->transforms[0].transform.rotation.y; 65 | double rot_z = msg->transforms[0].transform.rotation.z; 66 | double rot_w = msg->transforms[0].transform.rotation.w; 67 | 68 | // Convert the quarternion to euler (yaw, pitch, roll) 69 | tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w); 70 | tf2::Matrix3x3 matrix(quat); 71 | double roll, pitch, yaw; 72 | matrix.getRPY(roll, pitch, yaw); 73 | 74 | int zone = 54; 75 | int grid_num_x = 4; 76 | int grid_num_y = 39; 77 | int grid_size = 100000; 78 | bool northp = true; 79 | double lat, lon; 80 | 81 | // Reverse projection from UTM to geographic. 82 | GeographicLib::UTMUPS::Reverse( 83 | zone, 84 | northp, 85 | grid_num_x * grid_size + x, 86 | grid_num_y * grid_size + y, 87 | lat, 88 | lon 89 | ); 90 | 91 | if (cp && cp_started_) { 92 | cp->updateMGRS(&x, &y); 93 | cp->updateRP(&lat, &lon, &z); 94 | cp->updateHeading(&yaw); 95 | cp->updateGenerationTime(&gdt, ×tamp_msec); 96 | } 97 | } 98 | 99 | void V2XApp::start() { 100 | RCLCPP_INFO(node_->get_logger(), "V2X App Launched"); 101 | 102 | boost::asio::io_service io_service; 103 | TimeTrigger trigger(io_service); 104 | 105 | std::string network_interface; 106 | node_->get_parameter("network_interface", network_interface); 107 | RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str()); 108 | 109 | EthernetDevice device(network_interface.c_str()); 110 | vanetza::MacAddress mac_address = device.address(); 111 | 112 | std::stringstream sout; 113 | sout << mac_address; 114 | RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str()); 115 | 116 | // Geonetworking Management Infirmation Base (MIB) defines the GN protocol constants. 117 | gn::MIB mib; 118 | mib.itsGnLocalGnAddr.mid(mac_address); 119 | mib.itsGnLocalGnAddr.is_manually_configured(true); 120 | mib.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed; 121 | mib.itsGnSecurity = false; 122 | mib.itsGnProtocolVersion = 1; 123 | 124 | // Create raw socket on device and LinkLayer object 125 | auto link_layer = create_link_layer(io_service, device, "ethernet"); 126 | auto positioning = create_position_provider(io_service, trigger.runtime()); 127 | auto security = create_security_entity(trigger.runtime(), *positioning); 128 | RouterContext context(mib, trigger, *positioning, security.get()); 129 | 130 | context.set_link_layer(link_layer.get()); 131 | 132 | bool is_sender; 133 | node_->get_parameter("is_sender", is_sender); 134 | cp = new CpmApplication(node_, trigger.runtime(), is_sender); 135 | 136 | context.enable(cp); 137 | 138 | cp_started_ = true; 139 | 140 | io_service.run(); 141 | } 142 | } -------------------------------------------------------------------------------- /docs/installation/source-installation.md: -------------------------------------------------------------------------------- 1 | # Source Installation 2 | 3 | ## Prerequisites 4 | 5 | - OS 6 | - Ubuntu 20.04 7 | - Ubuntu 22.04 8 | - ROS 9 | - ROS2 Galactic 10 | 11 | ## Installing Autoware 12 | 13 | Refer to the [Official Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/) for the newest installation procedures. In a nutshell, you can run the following commands: 14 | 15 | ```bash 16 | # Clone repository 17 | git clone https://github.com/autowarefoundation/autoware.git 18 | cd autoware 19 | 20 | # Install dependencies using Ansible 21 | ./setup-dev-env.sh 22 | 23 | # Use vcstool to import more repositories 24 | mkdir src 25 | vcs import src < autoware.repos 26 | 27 | # Install dependent ROS packages 28 | source /opt/ros/galactic/setup.bash 29 | rosdep update 30 | rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 31 | 32 | # Build the workspace 33 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 34 | ``` 35 | 36 | ## Adding AutowareV2X 37 | 38 | 1. Edit the `autoware.repos` file and replace the following repositories. 39 | ``` 40 | repositories: 41 | core/autoware.core: 42 | type: git 43 | url: https://github.com/autowarefoundation/autoware.core.git 44 | version: 6bafedfb24fb34157ed65bfe3f6f4c1ed0fbc80b 45 | core/autoware_adapi_msgs: 46 | type: git 47 | url: https://github.com/autowarefoundation/autoware_adapi_msgs.git 48 | version: 9679b5a7a1f4cfff2fa50b80d2759d3937f2f953 49 | core/autoware_common: 50 | type: git 51 | url: https://github.com/autowarefoundation/autoware_common.git 52 | version: 6916df26fafe6749db4b1d5bd6636a92444fc48d 53 | core/autoware_msgs: 54 | type: git 55 | url: https://github.com/autowarefoundation/autoware_msgs.git 56 | version: 4f13d4b8b465ed7f424fce9af17882dbe1752875 57 | core/external/autoware_auto_msgs: 58 | type: git 59 | url: https://github.com/tier4/autoware_auto_msgs.git 60 | version: 6b5bc4365f9a2fc913bc11afa74ec21ffa2dbf32 61 | launcher/autoware_launch: 62 | type: git 63 | url: https://github.com/autowarefoundation/autoware_launch.git 64 | version: e4abe673667a8d4f2d783ed22edacbf5d4784b8f 65 | param/autoware_individual_params: 66 | type: git 67 | url: https://github.com/autowarefoundation/autoware_individual_params.git 68 | version: 79cff0ba014808050be6f5cb3b4764ba2c96c21c 69 | sensor_component/external/sensor_component_description: 70 | type: git 71 | url: https://github.com/tier4/sensor_component_description.git 72 | version: 475857daeb4c4883ab0295336713364b326e8278 73 | sensor_component/external/tamagawa_imu_driver: 74 | type: git 75 | url: https://github.com/tier4/tamagawa_imu_driver.git 76 | version: 28ad3cd4fb043e5f92353a540c3531cd4cb7bef3 77 | sensor_component/external/velodyne_vls: 78 | type: git 79 | url: https://github.com/tier4/velodyne_vls.git 80 | version: baeafaf9a376c5798f7b67a77211890c33900f84 81 | sensor_kit/external/awsim_sensor_kit_launch: 82 | type: git 83 | url: https://github.com/RobotecAI/awsim_sensor_kit_launch.git 84 | version: d9022ee9bbfd958c239b673cfbb230eea50607be 85 | sensor_kit/sample_sensor_kit_launch: 86 | type: git 87 | url: https://github.com/autowarefoundation/sample_sensor_kit_launch.git 88 | version: 03decbd31bb954eb9f52daaf3a3fa2b921dbb0c3 89 | universe/autoware.universe: 90 | type: git 91 | url: https://github.com/autowarefoundation/autoware.universe.git 92 | version: febbc135b8e09e993ed345ee6d3cd7e65b6c1d68 93 | universe/external/morai_msgs: 94 | type: git 95 | url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git 96 | version: 6fd6a711e4bbf8a9989b54028e8074acabbbce6f 97 | universe/external/muSSP: 98 | type: git 99 | url: https://github.com/tier4/muSSP.git 100 | version: c79e98fd5e658f4f90c06d93472faa977bc873b9 101 | universe/external/ndt_omp: 102 | type: git 103 | url: https://github.com/tier4/ndt_omp.git 104 | version: f59e1667390fe66d72c5c3aa0b25385b5b6dd8cf 105 | universe/external/pointcloud_to_laserscan: 106 | type: git 107 | url: https://github.com/tier4/pointcloud_to_laserscan.git 108 | version: 948a4fca35dcb03c6c8fbfa610a686f7c919fe0b 109 | universe/external/tier4_ad_api_adaptor: 110 | type: git 111 | url: https://github.com/tier4/tier4_ad_api_adaptor.git 112 | version: 5084f9c8eaf03458a216060798da2b1e4fa96f28 113 | universe/external/tier4_autoware_msgs: 114 | type: git 115 | url: https://github.com/tier4/tier4_autoware_msgs.git 116 | version: a360ee9f5235a0d426427813f26e43027e32139d 117 | vehicle/external/pacmod_interface: 118 | type: git 119 | url: https://github.com/tier4/pacmod_interface.git 120 | version: b5ae20345f2551da0c6e4140a3dc3479d64efd1f 121 | vehicle/sample_vehicle_launch: 122 | type: git 123 | url: https://github.com/autowarefoundation/sample_vehicle_launch.git 124 | version: 157238ca77de7b0a59f71a0b28f456741fab3ca2 125 | v2x/autowarev2x: 126 | type: git 127 | url: https://github.com/tlab-wide/AutowareV2X.git 128 | version: 48a1f2d3db6ae59e92febb93aad7cde760f4f3ec 129 | v2x/vanetza: 130 | type: git 131 | url: https://github.com/yuasabe/vanetza.git 132 | version: cfffe9afda177297c59bbb804d3e8f66120c8453 133 | ``` 134 | 135 | !!! Note 136 | If you want to　follow the latest ver, edit the `autoware.repos` file and add the following two repositories to the end. 137 | 138 | ``` 139 | v2x/autowarev2x: 140 | type: git 141 | url: https://github.com/tlab-wide/AutowareV2X.git 142 | version: cpm-tr 143 | v2x/vanetza: 144 | type: git 145 | url: https://github.com/yuasabe/vanetza.git 146 | version: master 147 | 148 | ``` 149 | 150 | 2. Update the repository 151 | ``` 152 | vcs import src < autoware.repos 153 | vcs pull src 154 | ``` 155 | 156 | 3. Install dependent ROS packages 157 | ```bash 158 | source /opt/ros/galactic/setup.bash 159 | rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 160 | ``` 161 | 162 | 4. Build the workspace 163 | ``` 164 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 165 | ``` 166 | -------------------------------------------------------------------------------- /docs/tutorials/planning-simulation/index.md: -------------------------------------------------------------------------------- 1 | # Running AutowareV2X in the Planning Simulator 2 | 3 | Simulations can be an easy way of verifying the functionality of AutowareV2X before an actual field test. 4 | 5 | AutowareV2X can be run in a simulation environment using [Autoware's Planning Simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/). An ITS-S is composed of Autoware as the autonomous driving stack and AutowareV2X as its V2X communication stack. Each ITS-S is executed inside a Docker container, and the wireless communication medium between ITS-Ss are modeled with Docker networks. A dynamic ITS-S is defined as a CAV, while a static ITS-S is considered a RSU. Perceived Objects are sent out on the network as CPMs. 6 | 7 | !!! Note 8 | Make sure you have completed [Autoware's Planning Simulator Tutorial](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/) before continuing. 9 | 10 | In order to test both the sender and receiver functionalities, we will need at least two ITS-S instances. 11 | 12 | ## The Docker environment to create 13 | 14 | We will be creating the Docker environment as depicted in the figure below. There will be two Docker containers to represent the two ITS-Ss, each of which includes Autoware.universe and AutowareV2X. They will both be a part of the Docker network called `v2x_net` with the subnet `10.0.0.0/24`. "Autoware Container #1" and "Autoware Container #2" will be described as `autoware_1` and `autoware_2` respectively. 15 | 16 | ![Docker Environment](./docker-env.png) 17 | 18 | ### Create a Docker network for V2X communication 19 | 20 | ```bash 21 | docker network create --driver=bridge --subnet=10.0.0.0/24 v2x_net -o com.docker.network.bridge.name="v2x_net" 22 | ``` 23 | 24 | 25 | ### Launch two ITS-S containers 26 | 27 | !!! Note 28 | Here, we will use a Rocker extension called [off-your-rocker](https://github.com/sloretz/off-your-rocker). 29 | Install `off-your-rocker` by running the below: 30 | ```bash 31 | python3 -m pip install off-your-rocker 32 | ``` 33 | 34 | In one terminal, use rocker to launch container `autoware_1`: 35 | ```bash 36 | rocker --nvidia --x11 --user --privileged --volume $HOME/workspace/autoware_docker --volume $HOME/data --network=v2x_net --name autoware_1 --oyr-run-arg "--ip 10.0.0.2 --hostname autoware_1" -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda 37 | ``` 38 | 39 | In another terminal, use rocker to launch container `autoware_2`: 40 | ``` 41 | rocker --nvidia --x11 --user --privileged --volume $HOME/workspace/autoware_docker --volume $HOME/data --network=v2x_net --name autoware_2 --oyr-run-arg "--ip 10.0.0.3 --hostname autoware_2" -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda 42 | ``` 43 | 44 | ## Run Planning Simulator 45 | 46 | Run the Planning Simulator in both `autoware_1` and `autoware_2`. 47 | 48 | In `autoware_1`: 49 | 50 | ``` 51 | cd ~/workspace/autoware_docker 52 | source install/setup.bash 53 | export AWID=1 # autoware_1 54 | source ~/workspace/autoware_docker/src/v2x/autowarev2x/setup.sh 55 | ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/data/maps/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit 56 | ``` 57 | 58 | Also, in `autoware_1`, set the ego-vehicle position by clicking `2D Pose Estimate`. 59 | 60 | Try adding some dummy cars by clicking `2D Dummy Car`. 61 | Note that you can make the dummy cars to be static by changing its `Velocity` to `0` in the `Tool Properties` pane. 62 | 63 | ![](./tools-properties.png) 64 | 65 | ![](./autoware_1_rviz.png) 66 | 67 | In `autoware_2`: 68 | 69 | ``` 70 | cd ~/workspace/autoware_docker 71 | source install/setup.bash 72 | export AWID=2 # autoware_2 73 | source ~/workspace/autoware_docker/src/v2x/autowarev2x/setup.sh 74 | ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/data/maps/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit 75 | ``` 76 | 77 | 78 | ## Run AutowareV2X 79 | 80 | In another terminal, connect to the `autoware_1` and `autoware_2` containers, and start AutowareV2X in both of them. We will set `autoware_1` to be the CPM sender, and `autoware_2` to be the CPM receiver. 81 | 82 | In `autoware_1`: 83 | ``` 84 | docker exec -it autoware_1 bash 85 | sudo su 86 | cd workspace/autoware_docker 87 | source install/setup.bash 88 | export AWID=1 89 | source ./src/v2x/autowarev2x/setup.sh 90 | ros2 launch autoware_v2x v2x.launch.xml network_interface:=eth0 91 | ``` 92 | 93 | You should see the command output like below. 94 | It shows that you are "Sending CPM with n objects", and the `[objectsList]` line describes the following information: `cpm_num, objectID, object.uuid, object.to_send, object.to_send_trigger`. 95 | ![](./autowarev2x_sender.png) 96 | 97 | In `autoware_2`: 98 | ``` 99 | docker exec -it autoware_2 bash 100 | sudo su 101 | cd workspace/autoware_docker 102 | source install/setup.bash 103 | export AWID=2 104 | source ./src/v2x/autowarev2x/setup.sh 105 | ros2 launch autoware_v2x v2x.launch.xml network_interface:=eth0 is_sender:=false 106 | ``` 107 | 108 | When both the sender and receiver is launched, you should see that the receiver (`autoware_2`) will start receiving CPMs like below. 109 | 110 | ![](./autowarev2x_receiver.png) 111 | 112 | ## Show CPM-shared objects in RViz 113 | 114 | 1. Press "Add" from the Displays Panel
115 | ![](./add-v2x-rviz-1.png) 116 | 2. Choose "By topic", then select PredictedObjects from /v2x/cpm/objects
117 | ![](./add-v2x-rviz-2.png) 118 | 3. The CPM-shared objects are shown in Rviz for `autoware_2`!
119 | ![](./add-v2x-rviz-3.png) 120 | 121 | ## Run scenarios 122 | 123 | In order to run scenarios, the [scenario_simulator_v2](https://github.com/tier4/scenario_simulator_v2.git) must be installed: 124 | 125 | 1. Launch new Autoware container 126 | ``` 127 | rocker --nvidia --x11 --user --volume $HOME/workspace/autoware_docker --volume $HOME/data -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda 128 | ``` 129 | 2. Add `simulator.repos` 130 | ``` 131 | cd workspace/autoware_docker 132 | vcs import src < simulator.repos 133 | ``` 134 | 3. Install dependent ROS packages 135 | ``` 136 | sudo apt update 137 | rosdep update 138 | rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -r 139 | ``` 140 | 4. Rebuild workspace 141 | ``` 142 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 143 | ``` 144 | 5. Download scenario. 145 | ```bash 146 | gdown -O ~/data/scenarios/ 'https://drive.google.com/uc?id=1FXwSSWeFDTMz7qsG-J7pyJA6RgjksqCy' 147 | ``` 148 | 6. Launch `scenario_test_runner` and specify scenario. 149 | ``` 150 | ros2 launch scenario_test_runner scenario_test_runner.launch.py map_path:=$HOME/data/maps/sample-map-planning sensor_model:=sample_sensor_kit vehicle_model:=sample_vehicle scenario:=$HOME/data/scenarios/busy_kashiwa_scenario.yaml launch_autoware:=true 151 | ``` -------------------------------------------------------------------------------- /docs/installation/docker-installation.md: -------------------------------------------------------------------------------- 1 | # Docker Installation 2 | 3 | In order to run the simulations explained in the [Tutorials](/tutorials) section, you will need to proceed with the Docker installation. 4 | 5 | !!! Note 6 | Also refer to [Autoware's Docker Installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/docker-installation/) for the Docker-based installation of Autoware.universe. 7 | 8 | ## Installing Autoware (Docker version) 9 | 10 | For the newest documentation for the Docker installation of Autoware, see their [official documentation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/docker-installation/). 11 | 12 | ### Prerequisites 13 | - git 14 | 15 | ### Setup 16 | 17 | 1. Prepare the repository. 18 | ```bash 19 | mkdir -p ~/workspace && cd ~/workspace 20 | git clone https://github.com/autowarefoundation/autoware.git autoware_docker 21 | cd autoware_docker 22 | ``` 23 | 24 | 2. Run the setup script for docker installation. 25 | ``` 26 | ./setup-dev-env.sh docker 27 | ``` 28 | You will need to restart your PC after the script is finished running. 29 | 30 | 3. Make directory to store maps 31 | ``` 32 | mkdir -p ~/data/maps 33 | ``` 34 | 35 | 36 | ### Launch container 37 | ``` 38 | # Launch Autoware container (with NVIDIA GPU) 39 | rocker --nvidia --x11 --user --privileged --volume $HOME/workspace/autoware_docker --volume $HOME/data -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda 40 | 41 | # Launch Autoware container (without NVIDIA GPU) 42 | rocker -e LIBGL_ALWAYS_SOFTWARE=1 --x11 --user --privileged --volume $HOME/workspace/autoware_docker --volume $HOME/data -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda 43 | ``` 44 | 45 | ## Adding AutowareV2X 46 | 47 | !!! Note 48 | From here, run commands inside the container. 49 | 50 | 1. Move into `autoware_docker` directory. 51 | ```bash 52 | cd ~/workspace/autoware_docker 53 | ``` 54 | 55 | 2. Edit the `autoware.repos` file and replace the following repositories. 56 | ``` 57 | repositories: 58 | core/autoware.core: 59 | type: git 60 | url: https://github.com/autowarefoundation/autoware.core.git 61 | version: 6bafedfb24fb34157ed65bfe3f6f4c1ed0fbc80b 62 | core/autoware_adapi_msgs: 63 | type: git 64 | url: https://github.com/autowarefoundation/autoware_adapi_msgs.git 65 | version: 9679b5a7a1f4cfff2fa50b80d2759d3937f2f953 66 | core/autoware_common: 67 | type: git 68 | url: https://github.com/autowarefoundation/autoware_common.git 69 | version: 6916df26fafe6749db4b1d5bd6636a92444fc48d 70 | core/autoware_msgs: 71 | type: git 72 | url: https://github.com/autowarefoundation/autoware_msgs.git 73 | version: 4f13d4b8b465ed7f424fce9af17882dbe1752875 74 | core/external/autoware_auto_msgs: 75 | type: git 76 | url: https://github.com/tier4/autoware_auto_msgs.git 77 | version: 6b5bc4365f9a2fc913bc11afa74ec21ffa2dbf32 78 | launcher/autoware_launch: 79 | type: git 80 | url: https://github.com/autowarefoundation/autoware_launch.git 81 | version: e4abe673667a8d4f2d783ed22edacbf5d4784b8f 82 | param/autoware_individual_params: 83 | type: git 84 | url: https://github.com/autowarefoundation/autoware_individual_params.git 85 | version: 79cff0ba014808050be6f5cb3b4764ba2c96c21c 86 | sensor_component/external/sensor_component_description: 87 | type: git 88 | url: https://github.com/tier4/sensor_component_description.git 89 | version: 475857daeb4c4883ab0295336713364b326e8278 90 | sensor_component/external/tamagawa_imu_driver: 91 | type: git 92 | url: https://github.com/tier4/tamagawa_imu_driver.git 93 | version: 28ad3cd4fb043e5f92353a540c3531cd4cb7bef3 94 | sensor_component/external/velodyne_vls: 95 | type: git 96 | url: https://github.com/tier4/velodyne_vls.git 97 | version: baeafaf9a376c5798f7b67a77211890c33900f84 98 | sensor_kit/external/awsim_sensor_kit_launch: 99 | type: git 100 | url: https://github.com/RobotecAI/awsim_sensor_kit_launch.git 101 | version: d9022ee9bbfd958c239b673cfbb230eea50607be 102 | sensor_kit/sample_sensor_kit_launch: 103 | type: git 104 | url: https://github.com/autowarefoundation/sample_sensor_kit_launch.git 105 | version: 03decbd31bb954eb9f52daaf3a3fa2b921dbb0c3 106 | universe/autoware.universe: 107 | type: git 108 | url: https://github.com/autowarefoundation/autoware.universe.git 109 | version: febbc135b8e09e993ed345ee6d3cd7e65b6c1d68 110 | universe/external/morai_msgs: 111 | type: git 112 | url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git 113 | version: 6fd6a711e4bbf8a9989b54028e8074acabbbce6f 114 | universe/external/muSSP: 115 | type: git 116 | url: https://github.com/tier4/muSSP.git 117 | version: c79e98fd5e658f4f90c06d93472faa977bc873b9 118 | universe/external/ndt_omp: 119 | type: git 120 | url: https://github.com/tier4/ndt_omp.git 121 | version: f59e1667390fe66d72c5c3aa0b25385b5b6dd8cf 122 | universe/external/pointcloud_to_laserscan: 123 | type: git 124 | url: https://github.com/tier4/pointcloud_to_laserscan.git 125 | version: 948a4fca35dcb03c6c8fbfa610a686f7c919fe0b 126 | universe/external/tier4_ad_api_adaptor: 127 | type: git 128 | url: https://github.com/tier4/tier4_ad_api_adaptor.git 129 | version: 5084f9c8eaf03458a216060798da2b1e4fa96f28 130 | universe/external/tier4_autoware_msgs: 131 | type: git 132 | url: https://github.com/tier4/tier4_autoware_msgs.git 133 | version: a360ee9f5235a0d426427813f26e43027e32139d 134 | vehicle/external/pacmod_interface: 135 | type: git 136 | url: https://github.com/tier4/pacmod_interface.git 137 | version: b5ae20345f2551da0c6e4140a3dc3479d64efd1f 138 | vehicle/sample_vehicle_launch: 139 | type: git 140 | url: https://github.com/autowarefoundation/sample_vehicle_launch.git 141 | version: 157238ca77de7b0a59f71a0b28f456741fab3ca2 142 | v2x/autowarev2x: 143 | type: git 144 | url: https://github.com/tlab-wide/AutowareV2X.git 145 | version: 48a1f2d3db6ae59e92febb93aad7cde760f4f3ec 146 | v2x/vanetza: 147 | type: git 148 | url: https://github.com/yuasabe/vanetza.git 149 | version: cfffe9afda177297c59bbb804d3e8f66120c8453 150 | ``` 151 | 152 | !!! Note 153 | If you want to　follow the latest ver, edit the `autoware.repos` file and add the following two repositories to the end. 154 | 155 | ``` 156 | v2x/autowarev2x: 157 | type: git 158 | url: https://github.com/tlab-wide/AutowareV2X.git 159 | version: cpm-tr 160 | v2x/vanetza: 161 | type: git 162 | url: https://github.com/yuasabe/vanetza.git 163 | version: master 164 | 165 | ``` 166 | 167 | 3. Update the repository 168 | ``` 169 | mkdir src 170 | vcs import src < autoware.repos 171 | vcs pull src 172 | ``` 173 | 174 | 4. Install dependent ROS packages 175 | ```bash 176 | sudo apt update 177 | rosdep update 178 | rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -r 179 | ``` 180 | 181 | 5. Build the workspace 182 | ``` 183 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 184 | ``` 185 | -------------------------------------------------------------------------------- /docs/tutorials/actual-devices/index.md: -------------------------------------------------------------------------------- 1 | # Running AutowareV2X on Actual Devices 2 | 3 | After testing functions in the simulator, it is time to install AutowareV2X on actual hardware. 4 | 5 | The setup that we consider is shown in the figure below. The Roadside Unit (RSU) acts as the CPM sender, and the CAV (Connected Autonomous Vehicle) is the CPM receiver. 6 | In terms of the software that comprises the RSU and CAV, they are both very similar. 7 | 8 | Take a look at the right hand side that shows the setup for the RSU. The sensing component for the RSU is a LiDAR (VLP-16) mounted on top of a tripod. The point cloud from the LiDAR is inputed into the Autoware PC with a normal Ethernet interface. The Autoware PC used here is an off-the-shelf gaming laptop. AutowareV2X is run on a separate machine, the Intel NUC, and it is connected to the Autoware PC via an Ethernet cable as well. All network interfaces, including the Wi-Fi module and 4G router in this case, is connected to the AutowareV2X PC. For the sake of simplicity, we will only focus on one Wi-Fi interface for this documentation. 9 | 10 | The set up for the vehicle is very similiar. The only difference is that the Autoware PC is now connected to the sensors and actuators of the vehicle. 11 | 12 | So, the **bare minimum for a CPM sender and receiver setup using AutowareV2X** is **TWO sets** of the following: 13 | 14 | - Autoware PC (Running the latest Autoware.universe) 15 | - AutowareV2X PC (Running the latest AutowareV2X) 16 | - LiDAR sensor (Autoware compatible, only necessary at the CPM Sender) 17 | 18 | In this tutorial, we will explain how to set this up. Depending on the number of CPM senders or receivers you need, you will need to make more copies of the same setup. 19 | 20 | 21 | 22 | ## Equipments 23 | 24 | The details for the equipment used in the setup is shown in the tables below. 25 | 26 | ### Hardware for the RSU 27 | 28 | | Purpose | Device | Specification | 29 | | ----------- | ----------- | ----------- | 30 | | AutowareV2X PC | Intel NUC, Model: 11Pro | OS: Ubuntu 20.04, ROS: Galactic, Software: AutowareV2X | 31 | | Autoware PC | Gtune PC, Model: H5| OS: Ubuntu 20.04, ROS: Galactic, Software: Autoware.universe | 32 | | Wi-Fi Network Interface | Netgear Wi-Fi Dongle, Model: A6210 | Dual-band: 802.11b/g/n (2.4GHz), 802.11a/n/ac (5GHz) | 33 | | 4G/LTE Network Interface | IDY IoM 5G Gateway, Model: iR730B | Bands: 3G, 4G, 5G, nano PSIM, 4x high-performance active antennas | 34 | | Sensor | VLP16 3D LiDAR | Range: 100 m, Accuracy: +/- 3 cm, Rotation rate: 5-20 Hz, 16 channels | 35 | 36 | ### Hardware for the CAV 37 | 38 | | Purpose | Device | Specification | 39 | | ----------- | ----------- | ----------- | 40 | | AutowareV2X PC | Intel NUC, Model: 11Pro | OS: Ubuntu 20.04, ROS: Galactic, Software: AutowareV2X | 41 | | Autoware PC | Gigabyte PC, Model: AERO-15 | OS: Ubuntu 20.04, ROS: Galactic, Software: Autoware.universe | 42 | | Wi-Fi Network Interface | Netgear Wi-Fi Dongle, Model: A6210 | Dual-band: 802.11b/g/n (2.4GHz), 802.11a/n/ac (5GHz) | 43 | | 4G/LTE Network Interface | IDY IoM 5G Gateway, Model: iR730B | Bands: 3G, 4G, 5G, nano PSIM, 4x high-performance active antennas | 44 | | Vehicle | Model: Yamaha G30Es-Li | Speed: less than 20 km/h, Sensor: VLP16 3D LiDAR (Rooftop) | 45 | 46 | ## Autoware PC 47 | 48 | 1. For the Autoware PC, you can follow the [normal installation procedures for Autoware.universe](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/). A PC with a discrete GPU is recommended for better performance with sensor processing and object detection. 49 | 50 | 2. Attach sensors to the Autoware PC (such as LiDARs) and make sure that you can start and run Autoware. 51 | 52 | 3. Confirm that the following two ROS2 topics are being published: 53 | - `/perception/object_recognition/objects` 54 | - `/tf` 55 | 56 | ## AutowareV2X PC 57 | 58 | ### Install AutowareV2X 59 | 60 | 1. Install Ubuntu 20.04 or 22.04 on a new PC. 61 | 62 | 2. Follow the [Source Installation Guide](../../installation/source-installation.md) for AutowareV2X to set up AutowareV2X. Do note that here, you will be installing Autoware again onto this PC, alongside AutowareV2X. But in practice, we will not be using the Autoware here and will rather use the Autoware on the Autoware PC. 63 | 64 | ### ROS2 Connectivity of Autoware PC and AutowareV2X PC 65 | 66 | 1. Connect the Autoware PC and AutowareV2X PC with an Ethernet cable (preferably Cat5e or above). Setup local IP addresses and IP routing on both the PCs so that they have IP reachability to each other. Confirm this by pinging each PC from the other. 67 | 68 | 2. Once this is setup, when you run Autoware on the Autoware PC, you should see all the ROS2 topics on the AutowareV2X PC as well. Confirm this by running `ros2 topic list` on the AutowareV2X PC after running Autoware or the Planning Simulator on the Autoware PC. 69 | 70 | !!! Note 71 | At this point, it may be wise to create at least two sets of the Autoware PC and AutowareV2X PC setup. One for the sender and the other for the receiver. 72 | ### Wi-Fi interface for CPM transmission 73 | 74 | 1. Attach a network interface to the AutowareV2X PC. You can use the default Wi-Fi interface or attach an USB dongle if necessary. 75 | 76 | 2. Create an Wi-Fi adhoc network to send the CPMs in. Sample commands are as below. Substitute the name of the network interface, and remember to change the IP address of the sender and receiver. 77 | 78 | ``` 79 | sudo systemctl stop network-manager 80 | sudo ifconfig wlx94a67e5d6f4d down 81 | sudo iwconfig wlx94a67e5d6f4d mode ad-hoc 82 | sudo iwconfig wlx94a67e5d6f4d essid CITS-EXP 83 | sudo ip addr add 10.0.0.2/24 dev wlx94a67e5d6f4d 84 | sudo ifconfig wlx94a67e5d6f4d up 85 | ``` 86 | 87 | 3. Once the adhoc network is up, you should be able to ping from the sender AutowareV2X PC to the receiver AutowareV2X PC if they are nearby. 88 | 89 | The `iwconfig` command can be used to check the adhoc network connection. 90 | 91 | ```bash 92 | # Sender AutowareV2X 93 | wlx94a67e5d6f4d IEEE 802.11 ESSID:"CITS-EXP" 94 | Mode:Ad-Hoc Frequency:2.412 GHz Cell: 0E:06:A2:BE:F5:6C 95 | Tx-Power=18 dBm 96 | Retry short limit:7 RTS thr:off Fragment thr:off 97 | Power Management:off 98 | 99 | # Receiver AutowareV2X 100 | wlx94a67e5d6f47 IEEE 802.11 ESSID:"CITS-EXP" 101 | Mode:Ad-Hoc Frequency:2.412 GHz Cell: 0E:06:A2:BE:F5:6C 102 | Tx-Power=18 dBm 103 | Retry short limit:7 RTS thr:off Fragment thr:off 104 | Power Management:off 105 | ``` 106 | 107 | ## Run Autoware and AutowareV2X 108 | 109 | 1. Run Autoware on the Autoware PC. 110 | 111 | 2. At this point, if you run AutowareV2X as explained in the [Run AutowareV2X Tutorial](../planning-simulation/index.md#run-autowarev2x), you should see that AutowareV2X will generate CPMs from the object information it receives from the Autoware PC and send it out the network interface. `autoware_1` in the linked tutorial is the CPM Sender AutowareV2X PC, and `autoware_2` is the CPM Receiver AutowareV2X PC in this case. 112 | 113 | 3. Check that the CPMs can be received on the receiver side. -------------------------------------------------------------------------------- /src/v2x_node.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/v2x_node.hpp" 2 | #include "autoware_v2x/v2x_app.hpp" 3 | #include "autoware_v2x/time_trigger.hpp" 4 | #include "autoware_v2x/router_context.hpp" 5 | #include "autoware_v2x/positioning.hpp" 6 | #include "autoware_v2x/security.hpp" 7 | #include "autoware_v2x/link_layer.hpp" 8 | #include "autoware_v2x/cpm_application.hpp" 9 | 10 | #include "rclcpp/rclcpp.hpp" 11 | #include "std_msgs/msg/string.hpp" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "tf2/LinearMath/Quaternion.h" 20 | #include 21 | #include 22 | #include 23 | 24 | namespace gn = vanetza::geonet; 25 | 26 | using namespace vanetza; 27 | using namespace std::chrono; 28 | 29 | namespace v2x 30 | { 31 | V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { 32 | using std::placeholders::_1; 33 | 34 | objects_sub_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); 35 | tf_sub_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); 36 | 37 | cpm_objects_pub_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); 38 | // cpm_sender_pub_ = create_publisher("/v2x/cpm/sender", rclcpp::QoS{10}); 39 | 40 | // Declare Parameters 41 | this->declare_parameter("network_interface", "vmnet1"); 42 | this->declare_parameter("is_sender", true); 43 | 44 | // Launch V2XApp in a new thread 45 | app = new V2XApp(this); 46 | boost::thread v2xApp(boost::bind(&V2XApp::start, app)); 47 | 48 | RCLCPP_INFO(get_logger(), "V2X Node Launched"); 49 | 50 | // Make latency_log file from current timestamp 51 | time_t t = time(nullptr); 52 | const tm* lt = localtime(&t); 53 | std::stringstream s; 54 | s << "20" << lt->tm_year-100 <<"-" << lt->tm_mon+1 << "-" << lt->tm_mday << "_" << lt->tm_hour << ":" << lt->tm_min << ":" << lt->tm_sec; 55 | std::string timestamp = s.str(); 56 | char cur_dir[1024]; 57 | getcwd(cur_dir, 1024); 58 | std::string latency_log_filename = std::string(cur_dir) + "/latency_logs/latency_log_file_" + timestamp + ".csv"; 59 | latency_log_file.open(latency_log_filename, std::ios::out); 60 | } 61 | 62 | void V2XNode::publishCpmSenderObject(double x_mgrs, double y_mgrs, double orientation) { 63 | autoware_auto_perception_msgs::msg::PredictedObjects cpm_sender_object_msg; 64 | std_msgs::msg::Header header; 65 | rclcpp::Time current_time = this->now(); 66 | cpm_sender_object_msg.header.frame_id = "map"; 67 | cpm_sender_object_msg.header.stamp = current_time; 68 | 69 | autoware_auto_perception_msgs::msg::PredictedObject object; 70 | autoware_auto_perception_msgs::msg::ObjectClassification classification; 71 | autoware_auto_perception_msgs::msg::Shape shape; 72 | autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics; 73 | 74 | classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; 75 | classification.probability = 0.99; 76 | 77 | shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; 78 | shape.dimensions.x = 5.0; 79 | shape.dimensions.y = 2.0; 80 | shape.dimensions.z = 1.7; 81 | 82 | kinematics.initial_pose_with_covariance.pose.position.x = x_mgrs; 83 | kinematics.initial_pose_with_covariance.pose.position.y = y_mgrs; 84 | kinematics.initial_pose_with_covariance.pose.position.z = 0.1; 85 | 86 | tf2::Quaternion quat; 87 | quat.setRPY(0, 0, orientation); 88 | 89 | kinematics.initial_pose_with_covariance.pose.orientation.x = quat.x(); 90 | kinematics.initial_pose_with_covariance.pose.orientation.y = quat.y(); 91 | kinematics.initial_pose_with_covariance.pose.orientation.z = quat.z(); 92 | kinematics.initial_pose_with_covariance.pose.orientation.w = quat.w(); 93 | 94 | object.classification.emplace_back(classification); 95 | object.shape = shape; 96 | object.kinematics = kinematics; 97 | 98 | cpm_sender_object_msg.objects.push_back(object); 99 | 100 | // publisher_v2x_cpm_sender_->publish(cpm_sender_object_msg); 101 | 102 | } 103 | 104 | void V2XNode::publishObjects(std::vector *objectsStack, int cpm_num) { 105 | autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg; 106 | std_msgs::msg::Header header; 107 | rclcpp::Time current_time = this->now(); 108 | output_dynamic_object_msg.header.frame_id = "map"; 109 | output_dynamic_object_msg.header.stamp = current_time; 110 | 111 | for (CpmApplication::Object obj : *objectsStack) { 112 | autoware_auto_perception_msgs::msg::PredictedObject object; 113 | autoware_auto_perception_msgs::msg::ObjectClassification classification; 114 | autoware_auto_perception_msgs::msg::Shape shape; 115 | autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics; 116 | 117 | classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; 118 | classification.probability = 0.99; 119 | 120 | shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; 121 | shape.dimensions.x = obj.shape_x / 10.0; 122 | shape.dimensions.y = obj.shape_y / 10.0; 123 | shape.dimensions.z = obj.shape_z / 10.0; 124 | 125 | kinematics.initial_pose_with_covariance.pose.position.x = obj.position_x; 126 | kinematics.initial_pose_with_covariance.pose.position.y = obj.position_y; 127 | kinematics.initial_pose_with_covariance.pose.position.z = 0.1; 128 | 129 | kinematics.initial_pose_with_covariance.pose.orientation.x = obj.orientation_x; 130 | kinematics.initial_pose_with_covariance.pose.orientation.y = obj.orientation_y; 131 | kinematics.initial_pose_with_covariance.pose.orientation.z = obj.orientation_z; 132 | kinematics.initial_pose_with_covariance.pose.orientation.w = obj.orientation_w; 133 | 134 | object.classification.emplace_back(classification); 135 | object.shape = shape; 136 | object.kinematics = kinematics; 137 | 138 | std::mt19937 gen(std::random_device{}()); 139 | std::independent_bits_engine bit_eng(gen); 140 | std::generate(object.object_id.uuid.begin(), object.object_id.uuid.end(), bit_eng); 141 | 142 | output_dynamic_object_msg.objects.push_back(object); 143 | } 144 | 145 | std::chrono::milliseconds ms = std::chrono::duration_cast ( 146 | std::chrono::system_clock::now().time_since_epoch() 147 | ); 148 | latency_log_file << "T_publish," << cpm_num << "," << ms.count() << std::endl; 149 | 150 | cpm_objects_pub_->publish(output_dynamic_object_msg); 151 | } 152 | 153 | void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { 154 | rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. 155 | 156 | std::chrono::milliseconds ms = std::chrono::duration_cast ( 157 | std::chrono::system_clock::now().time_since_epoch() 158 | ); 159 | latency_log_file << "T_rosmsg,," << ms.count() << std::endl; 160 | 161 | app->objectsCallback(msg); 162 | } 163 | 164 | void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { 165 | app->tfCallback(msg); 166 | } 167 | } 168 | 169 | #include "rclcpp_components/register_node_macro.hpp" 170 | RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) -------------------------------------------------------------------------------- /src/cpm_application.cpp: -------------------------------------------------------------------------------- 1 | #include "autoware_v2x/cpm_application.hpp" 2 | #include "autoware_v2x/positioning.hpp" 3 | #include "autoware_v2x/security.hpp" 4 | #include "autoware_v2x/link_layer.hpp" 5 | #include "autoware_v2x/v2x_node.hpp" 6 | 7 | #include "tf2/LinearMath/Quaternion.h" 8 | #include "tf2/LinearMath/Matrix3x3.h" 9 | 10 | #include "rclcpp/rclcpp.hpp" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #define _USE_MATH_DEFINES 29 | #include 30 | 31 | using namespace vanetza; 32 | using namespace std::chrono; 33 | 34 | namespace v2x { 35 | CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) : 36 | node_(node), 37 | runtime_(rt), 38 | ego_(), 39 | generationTime_(0), 40 | updating_objects_list_(false), 41 | sending_(false), 42 | is_sender_(is_sender), 43 | reflect_packet_(false), 44 | objectConfidenceThreshold_(0.0), 45 | include_all_persons_and_animals_(false), 46 | cpm_num_(0), 47 | received_cpm_num_(0), 48 | cpm_object_id_(0), 49 | use_dynamic_generation_rules_(false) 50 | { 51 | RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); 52 | set_interval(milliseconds(100)); 53 | createTables(); 54 | } 55 | 56 | void CpmApplication::set_interval(Clock::duration interval) { 57 | cpm_interval_ = interval; 58 | runtime_.cancel(this); 59 | schedule_timer(); 60 | } 61 | 62 | void CpmApplication::schedule_timer() { 63 | runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this); 64 | } 65 | 66 | void CpmApplication::on_timer(Clock::time_point) { 67 | schedule_timer(); 68 | send(); 69 | } 70 | 71 | CpmApplication::PortType CpmApplication::port() { 72 | return btp::ports::CPM; 73 | } 74 | 75 | std::string CpmApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) { 76 | std::stringstream ss; 77 | for (auto i = 0; i < 16; ++i) { 78 | ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; 79 | } 80 | return ss.str(); 81 | } 82 | 83 | void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) { 84 | 85 | asn1::PacketVisitor visitor; 86 | std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); 87 | 88 | if (cpm) { 89 | RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received CPM #%d", received_cpm_num_); 90 | 91 | rclcpp::Time current_time = node_->now(); 92 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); 93 | 94 | asn1::Cpm message = *cpm; 95 | ItsPduHeader_t &header = message->header; 96 | 97 | std::chrono::milliseconds ms = std::chrono::duration_cast ( 98 | std::chrono::system_clock::now().time_since_epoch() 99 | ); 100 | node_->latency_log_file << "T_received," << header.stationID << "," << ms.count() << std::endl; 101 | 102 | 103 | // Calculate GDT and get GDT from CPM and calculate the "Age of CPM" 104 | TimestampIts_t gt_cpm = message->cpm.generationTime; 105 | // const auto time_now = duration_cast (runtime_.now().time_since_epoch()); 106 | // uint16_t gdt = time_now.count(); 107 | // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; 108 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm); 109 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt); 110 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff); 111 | 112 | 113 | CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; 114 | double lat = management.referencePosition.latitude / 1.0e7; 115 | double lon = management.referencePosition.longitude / 1.0e7; 116 | 117 | int zone; 118 | int grid_num_x = 4; 119 | int grid_num_y = 39; 120 | int grid_size = 100000; 121 | bool northp; 122 | double x, y; 123 | 124 | GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); 125 | double x_mgrs = x - grid_num_x * grid_size; 126 | double y_mgrs = y - grid_num_y * grid_size; 127 | 128 | OriginatingVehicleContainer_t &ovc = message->cpm.cpmParameters.stationDataContainer->choice.originatingVehicleContainer; 129 | 130 | // Calculate ego-vehicle orientation (radians) from heading (degree). 131 | // orientation: True-East, counter-clockwise angle. (0.1 degree accuracy) 132 | int heading = ovc.heading.headingValue; 133 | double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0; 134 | if (orientation < 0.0) orientation += (2.0 * M_PI); 135 | // double orientation = heading / 10.0; 136 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading); 137 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); 138 | 139 | 140 | // Publish CPM Sender info to /v2x/cpm/sender through V2XNode function 141 | node_->publishCpmSenderObject(x_mgrs, y_mgrs, orientation); 142 | 143 | 144 | // Get PerceivedObjects 145 | receivedObjectsStack.clear(); 146 | 147 | PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer; 148 | 149 | if (poc != NULL) { 150 | for (int i = 0; i < poc->list.count; ++i) { 151 | // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); 152 | 153 | CpmApplication::Object object; 154 | double x1 = poc->list.array[i]->xDistance.value; 155 | double y1 = poc->list.array[i]->yDistance.value; 156 | x1 = x1 / 100.0; 157 | y1 = y1 / 100.0; 158 | object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1); 159 | object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); 160 | object.shape_x = poc->list.array[i]->planarObjectDimension2->value; 161 | object.shape_y = poc->list.array[i]->planarObjectDimension1->value; 162 | object.shape_z = poc->list.array[i]->verticalObjectDimension->value; 163 | 164 | object.yawAngle = poc->list.array[i]->yawAngle->value; 165 | double yaw_radian = (M_PI * object.yawAngle / 10.0) / 180.0; 166 | 167 | tf2::Quaternion quat; 168 | quat.setRPY(0, 0, yaw_radian); 169 | object.orientation_x = quat.x(); 170 | object.orientation_y = quat.y(); 171 | object.orientation_z = quat.z(); 172 | object.orientation_w = quat.w(); 173 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); 174 | 175 | receivedObjectsStack.push_back(object); 176 | } 177 | node_->publishObjects(&receivedObjectsStack, header.stationID); 178 | } else { 179 | // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); 180 | } 181 | 182 | insertCpmToCpmTable(message, (char*) "cpm_received"); 183 | 184 | if (reflect_packet_) { 185 | Application::DownPacketPtr packet{new DownPacket()}; 186 | std::unique_ptr payload{new geonet::DownPacket()}; 187 | 188 | payload->layer(OsiLayer::Application) = std::move(message); 189 | 190 | Application::DataRequest request; 191 | request.its_aid = aid::CP; 192 | request.transport_type = geonet::TransportType::SHB; 193 | request.communication_profile = geonet::CommunicationProfile::ITS_G5; 194 | 195 | Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); 196 | 197 | if (!confirm.accepted()) { 198 | throw std::runtime_error("[CpmApplication::indicate] Packet reflection failed"); 199 | } 200 | } 201 | 202 | ++received_cpm_num_; 203 | 204 | } else { 205 | RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content"); 206 | } 207 | } 208 | 209 | void CpmApplication::updateMGRS(double *x, double *y) { 210 | ego_.mgrs_x = *x; 211 | ego_.mgrs_y = *y; 212 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateMGRS] ego-vehicle.position: %.10f, %.10f", ego_.mgrs_x, ego_.mgrs_y); 213 | } 214 | 215 | void CpmApplication::updateRP(double *lat, double *lon, double *altitude) { 216 | ego_.latitude = *lat; 217 | ego_.longitude = *lon; 218 | ego_.altitude = *altitude; 219 | } 220 | 221 | void CpmApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) { 222 | generationTime_ = *gdt; 223 | gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp 224 | } 225 | 226 | void CpmApplication::updateHeading(double *yaw) { 227 | ego_.heading = *yaw; 228 | } 229 | 230 | void CpmApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { 231 | if (msg->objects.size() > 0) { 232 | for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { 233 | std::string object_uuid = uuidToHexString(obj.object_id); 234 | auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) { 235 | return !strcmp(e.uuid.c_str(), object_uuid.c_str()); 236 | }); 237 | 238 | if (found_object == objectsList.end()) { 239 | 240 | } else { 241 | found_object->to_send = true; 242 | } 243 | } 244 | 245 | } 246 | } 247 | 248 | void CpmApplication::updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { 249 | updating_objects_list_ = true; 250 | 251 | // Flag all objects as NOT_SEND 252 | if (objectsList.size() > 0) { 253 | for (auto& object : objectsList) { 254 | object.to_send = false; 255 | object.to_send_trigger = -1; 256 | } 257 | } 258 | 259 | if (msg->objects.size() > 0) { 260 | for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { 261 | 262 | // RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label); 263 | double existence_probability = obj.existence_probability; 264 | // RCLCPP_INFO(node_->get_logger(), "existence_probability: %f", existence_probability); 265 | 266 | std::string object_uuid = uuidToHexString(obj.object_id); 267 | // RCLCPP_INFO(node_->get_logger(), "received object_id: %s", object_uuid.c_str()); 268 | 269 | // RCLCPP_INFO(node_->get_logger(), "ObjectsList count: %d", objectsList.size()); 270 | 271 | if (existence_probability >= objectConfidenceThreshold_) { 272 | // ObjectConfidence > ObjectConfidenceThreshold 273 | 274 | // Object tracked in internal memory? (i.e. Is object included in ObjectsList?) 275 | auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) { 276 | return !strcmp(e.uuid.c_str(), object_uuid.c_str()); 277 | }); 278 | 279 | if (found_object == objectsList.end()) { 280 | // Object is new to internal memory 281 | 282 | if (cpm_object_id_ > 255) { 283 | cpm_object_id_ = 0; 284 | } 285 | 286 | // Add new object to ObjectsList 287 | CpmApplication::Object object; 288 | object.objectID = cpm_object_id_; 289 | object.uuid = object_uuid; 290 | object.timestamp_ros = msg->header.stamp; 291 | object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; 292 | object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; 293 | object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; 294 | object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; 295 | object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; 296 | object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; 297 | object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; 298 | object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); 299 | object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); 300 | object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); 301 | 302 | long long msg_timestamp_sec = msg->header.stamp.sec; 303 | long long msg_timestamp_nsec = msg->header.stamp.nanosec; 304 | msg_timestamp_sec -= 1072915200; // convert to etsi-epoch 305 | long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; 306 | object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; 307 | if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { 308 | RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement); 309 | continue; 310 | } 311 | 312 | object.to_send = true; 313 | object.to_send_trigger = 0; 314 | object.timestamp = runtime_.now(); 315 | 316 | objectsList.push_back(object); 317 | ++cpm_object_id_; 318 | 319 | } else { 320 | 321 | // Object was already in internal memory 322 | 323 | // Object belongs to class person or animal 324 | if (obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { 325 | 326 | if (include_all_persons_and_animals_) { 327 | found_object->to_send = true; 328 | found_object->to_send_trigger = 5; 329 | } 330 | 331 | // Object has not been included in a CPM in the past 500 ms. 332 | if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) { 333 | // Include all objects of class person or animal in the current CPM 334 | include_all_persons_and_animals_ = true; 335 | found_object->to_send = true; 336 | found_object->to_send_trigger = 5; 337 | setAllObjectsOfPersonsAnimalsToSend(msg); 338 | RCLCPP_INFO(node_->get_logger(), "Include all objects of person/animal class"); 339 | } 340 | 341 | } else { 342 | // Object does not belong to class person or animal 343 | 344 | // Euclidean absolute distance has changed by more than 4m 345 | double dist = pow(obj.kinematics.initial_pose_with_covariance.pose.position.x - found_object->position_x, 2) + pow(obj.kinematics.initial_pose_with_covariance.pose.position.y - found_object->position_y, 2); 346 | dist = sqrt(dist); 347 | // RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist); 348 | if (dist > 4) { 349 | found_object->to_send = true; 350 | found_object->to_send_trigger = 1; 351 | } else { 352 | 353 | } 354 | 355 | // Absolute speed changed by more than 0.5 m/s 356 | double speed = pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x - found_object->twist_linear_x, 2) + pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x- found_object->twist_linear_y, 2); 357 | speed = sqrt(speed); 358 | // RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist); 359 | if (speed > 0.5) { 360 | found_object->to_send = true; 361 | found_object->to_send_trigger = 2; 362 | } 363 | 364 | // Orientation of speed vector changed by more than 4 degrees 365 | double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI; 366 | double twist_angular_y_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.y - found_object->twist_angular_y) * 180 / M_PI; 367 | // RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed x: %f", twist_angular_x_diff); 368 | // RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff); 369 | if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) { 370 | found_object->to_send = true; 371 | found_object->to_send_trigger = 3; 372 | } 373 | 374 | 375 | // It has been more than 1 s since last transmission of this object 376 | if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) { 377 | found_object->to_send = true; 378 | found_object->to_send_trigger = 4; 379 | // RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count()); 380 | } 381 | 382 | } 383 | 384 | // Update found_object 385 | found_object->timestamp_ros = msg->header.stamp; 386 | found_object->position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; 387 | found_object->position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; 388 | found_object->position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; 389 | found_object->orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; 390 | found_object->orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; 391 | found_object->orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; 392 | found_object->orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; 393 | found_object->shape_x = std::lround(obj.shape.dimensions.x * 10.0); 394 | found_object->shape_y = std::lround(obj.shape.dimensions.y * 10.0); 395 | found_object->shape_z = std::lround(obj.shape.dimensions.z * 10.0); 396 | 397 | long long msg_timestamp_sec = msg->header.stamp.sec; 398 | long long msg_timestamp_nsec = msg->header.stamp.nanosec; 399 | msg_timestamp_sec -= 1072915200; // convert to etsi-epoch 400 | long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; 401 | found_object->timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; 402 | if (found_object->timeOfMeasurement < -1500 || found_object->timeOfMeasurement > 1500) { 403 | RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", found_object->timeOfMeasurement); 404 | continue; 405 | } 406 | 407 | found_object->timestamp = runtime_.now(); 408 | 409 | // if use_dymanic_generation_rules_ == false, then always include object in CPM 410 | if (!use_dynamic_generation_rules_) { 411 | found_object->to_send = true; 412 | found_object->to_send_trigger = 0; 413 | } 414 | 415 | } 416 | } 417 | } 418 | } else { 419 | // No objects detected 420 | } 421 | 422 | // RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); 423 | rclcpp::Time current_time = node_->now(); 424 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); 425 | updating_objects_list_ = false; 426 | } 427 | 428 | void CpmApplication::printObjectsList(int cpm_num) { 429 | // RCLCPP_INFO(node_->get_logger(), "------------------------"); 430 | if (objectsList.size() > 0) { 431 | for (auto& object : objectsList) { 432 | RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,%d,%s,%d,%d", cpm_num, object.objectID, object.uuid.c_str(), object.to_send, object.to_send_trigger); 433 | } 434 | } else { 435 | RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num); 436 | } 437 | 438 | // RCLCPP_INFO(node_->get_logger(), "------------------------"); 439 | } 440 | 441 | void CpmApplication::send() { 442 | 443 | if (is_sender_) { 444 | 445 | sending_ = true; 446 | 447 | printObjectsList(cpm_num_); 448 | 449 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); 450 | 451 | vanetza::asn1::Cpm message; 452 | 453 | // ITS PDU Header 454 | ItsPduHeader_t &header = message->header; 455 | header.protocolVersion = 1; 456 | header.messageID = 14; 457 | header.stationID = cpm_num_; 458 | 459 | CollectivePerceptionMessage_t &cpm = message->cpm; 460 | 461 | // Set GenerationTime 462 | RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); 463 | asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_); 464 | 465 | CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; 466 | management.stationType = StationType_passengerCar; 467 | management.referencePosition.latitude = ego_.latitude * 1e7; 468 | management.referencePosition.longitude = ego_.longitude * 1e7; 469 | 470 | StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; 471 | sdc = vanetza::asn1::allocate(); 472 | sdc->present = StationDataContainer_PR_originatingVehicleContainer; 473 | 474 | OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; 475 | ovc.speed.speedValue = 0; 476 | ovc.speed.speedConfidence = 1; 477 | 478 | // Calculate headingValue of ego-vehicle. 479 | // Convert ego-vehicle yaw to True-North clockwise angle (heading). 0.1 degree accuracy. 480 | int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); 481 | if (heading < 0) heading += 3600; 482 | ovc.heading.headingValue = heading; 483 | ovc.heading.headingConfidence = 1; 484 | 485 | int perceivedObjectsCount = 0; 486 | 487 | if (objectsList.size() > 0) { 488 | PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; 489 | poc = vanetza::asn1::allocate(); 490 | 491 | for (auto& object : objectsList) { 492 | 493 | // RCLCPP_INFO(node_->get_logger(), "object.to_send: %d", object.to_send); 494 | 495 | if (object.to_send) { 496 | 497 | PerceivedObject *pObj = vanetza::asn1::allocate(); 498 | 499 | // Update CPM-specific values for Object 500 | object.xDistance = std::lround(( 501 | (object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading) 502 | ) * 100.0); 503 | object.yDistance = std::lround(( 504 | (object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading) 505 | ) * 100.0); 506 | if (object.xDistance < -132768 || object.xDistance > 132767) { 507 | RCLCPP_WARN(node_->get_logger(), "xDistance out of bounds. objectID: #%d", object.objectID); 508 | continue; 509 | } 510 | if (object.yDistance < -132768 || object.yDistance > 132767) { 511 | RCLCPP_WARN(node_->get_logger(), "yDistance out of bounds. objectID: #%d", object.objectID); 512 | continue; 513 | } 514 | 515 | // Calculate orientation of detected object 516 | tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); 517 | tf2::Matrix3x3 matrix(quat); 518 | double roll, pitch, yaw; 519 | matrix.getRPY(roll, pitch, yaw); 520 | if (yaw < 0) { 521 | object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600 522 | } else { 523 | object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 524 | } 525 | object.xSpeed = 0; 526 | object.ySpeed = 0; 527 | pObj->objectID = object.objectID; 528 | pObj->timeOfMeasurement = object.timeOfMeasurement; 529 | pObj->xDistance.value = object.xDistance; 530 | pObj->xDistance.confidence = 1; 531 | pObj->yDistance.value = object.yDistance; 532 | pObj->yDistance.confidence = 1; 533 | pObj->xSpeed.value = object.xSpeed; 534 | pObj->xSpeed.confidence = 1; 535 | pObj->ySpeed.value = object.ySpeed; 536 | pObj->ySpeed.confidence = 1; 537 | 538 | pObj->planarObjectDimension1 = vanetza::asn1::allocate(); 539 | pObj->planarObjectDimension2 = vanetza::asn1::allocate(); 540 | pObj->verticalObjectDimension = vanetza::asn1::allocate(); 541 | 542 | (*(pObj->planarObjectDimension1)).value = object.shape_y; 543 | (*(pObj->planarObjectDimension1)).confidence = 1; 544 | (*(pObj->planarObjectDimension2)).value = object.shape_x; 545 | (*(pObj->planarObjectDimension2)).confidence = 1; 546 | (*(pObj->verticalObjectDimension)).value = object.shape_z; 547 | (*(pObj->verticalObjectDimension)).confidence = 1; 548 | 549 | pObj->yawAngle = vanetza::asn1::allocate(); 550 | (*(pObj->yawAngle)).value = object.yawAngle; 551 | (*(pObj->yawAngle)).confidence = 1; 552 | 553 | ASN_SEQUENCE_ADD(poc, pObj); 554 | 555 | // object.to_send = false; 556 | // object.to_send_trigger = -1; 557 | // RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); 558 | 559 | ++perceivedObjectsCount; 560 | 561 | } else { 562 | // Object.to_send is set to False 563 | // RCLCPP_INFO(node_->get_logger(), "Object: %s not being sent.", object.uuid.c_str()); 564 | } 565 | } 566 | 567 | cpm.cpmParameters.numberOfPerceivedObjects = perceivedObjectsCount; 568 | 569 | if (perceivedObjectsCount == 0) { 570 | cpm.cpmParameters.perceivedObjectContainer = NULL; 571 | } 572 | 573 | } else { 574 | cpm.cpmParameters.perceivedObjectContainer = NULL; 575 | RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); 576 | } 577 | 578 | RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount); 579 | 580 | insertCpmToCpmTable(message, (char*) "cpm_sent"); 581 | 582 | std::unique_ptr payload{new geonet::DownPacket()}; 583 | 584 | payload->layer(OsiLayer::Application) = std::move(message); 585 | 586 | Application::DataRequest request; 587 | request.its_aid = aid::CP; 588 | request.transport_type = geonet::TransportType::SHB; 589 | request.communication_profile = geonet::CommunicationProfile::ITS_G5; 590 | 591 | Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); 592 | 593 | if (!confirm.accepted()) { 594 | throw std::runtime_error("[CpmApplication::send] CPM application data request failed"); 595 | } 596 | 597 | sending_ = false; 598 | // rclcpp::Time current_time = node_->now(); 599 | // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); 600 | 601 | std::chrono::milliseconds ms = std::chrono::duration_cast ( 602 | std::chrono::system_clock::now().time_since_epoch() 603 | ); 604 | node_->latency_log_file << "T_depart," << cpm_num_ << "," << ms.count() << std::endl; 605 | 606 | ++cpm_num_; 607 | } 608 | } 609 | 610 | void CpmApplication::createTables() { 611 | sqlite3 *db = NULL; 612 | char* err = NULL; 613 | 614 | int ret = sqlite3_open("autoware_v2x.db", &db); 615 | if (ret != SQLITE_OK) { 616 | RCLCPP_INFO(node_->get_logger(), "DB File Open Error"); 617 | return; 618 | } 619 | 620 | char* sql_command; 621 | 622 | sql_command = (char*) "create table if not exists cpm_sent(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);"; 623 | 624 | ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); 625 | if (ret != SQLITE_OK) { 626 | RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_sent)"); 627 | sqlite3_close(db); 628 | sqlite3_free(err); 629 | return; 630 | } 631 | 632 | sql_command = (char*) "create table if not exists cpm_received(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);"; 633 | 634 | ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); 635 | if (ret != SQLITE_OK) { 636 | RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_received)"); 637 | sqlite3_close(db); 638 | sqlite3_free(err); 639 | return; 640 | } 641 | 642 | sqlite3_close(db); 643 | RCLCPP_INFO(node_->get_logger(), "CpmApplication::createTables Finished"); 644 | } 645 | 646 | void CpmApplication::insertCpmToCpmTable(vanetza::asn1::Cpm cpm, char* table_name) { 647 | sqlite3 *db = NULL; 648 | char* err = NULL; 649 | 650 | int ret = sqlite3_open("autoware_v2x.db", &db); 651 | if (ret != SQLITE_OK) { 652 | RCLCPP_INFO(node_->get_logger(), "DB File Open Error"); 653 | return; 654 | } 655 | 656 | int64_t timestamp = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 657 | 658 | int perceivedObjectCount = 0; 659 | if (cpm->cpm.cpmParameters.numberOfPerceivedObjects) { 660 | perceivedObjectCount = cpm->cpm.cpmParameters.numberOfPerceivedObjects; 661 | } 662 | 663 | std::stringstream sql_command; 664 | 665 | sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");"; 666 | 667 | ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err); 668 | if (ret != SQLITE_OK) { 669 | RCLCPP_INFO(node_->get_logger(), "DB Execution Error (insertCpmToCpmTable)"); 670 | RCLCPP_INFO(node_->get_logger(), sql_command.str().c_str()); 671 | RCLCPP_INFO(node_->get_logger(), err); 672 | sqlite3_close(db); 673 | sqlite3_free(err); 674 | return; 675 | } 676 | 677 | sqlite3_close(db); 678 | // RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished"); 679 | } 680 | } --------------------------------------------------------------------------------