diff --git a/CMakeLists.txt b/CMakeLists.txt index 89987ad..66eacfc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,8 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) find_package(Vanetza REQUIRED) find_package(GeographicLib 1.37 REQUIRED) find_package(Boost COMPONENTS thread REQUIRED) +find_package(etsi_its_cam_ts_coding REQUIRED) +find_package(etsi_its_cam_ts_conversion REQUIRED) ament_auto_find_build_dependencies() find_package(std_msgs REQUIRED) @@ -46,7 +48,7 @@ ament_auto_add_library(autoware_v2x SHARED src/security.cpp ) -target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3) +target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3 etsi_its_cam_ts_coding::etsi_its_cam_ts_coding etsi_its_cam_ts_conversion::etsi_its_cam_ts_conversion) rclcpp_components_register_node(autoware_v2x PLUGIN "v2x::V2XNode" @@ -61,4 +63,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch config -) \ No newline at end of file +) diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index 5b1bce5..80a1e0f 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: - network_interface: "wlp4s0" - is_sender: true \ No newline at end of file + network_interface: "v2x_testing" + is_sender: true diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index a5941e2..665f13e 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -19,6 +19,8 @@ #include "autoware_v2x/router_context.hpp" #include +#include + namespace v2x { class V2XNode : public rclcpp::Node @@ -28,13 +30,14 @@ namespace v2x V2XApp *app; void publishObjects(std::vector *, int cpm_num); void publishCpmSenderObject(double, double, double); + void publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &); std::ofstream latency_log_file; private: void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg); - void vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg); + void vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg); void getVehicleDimensions(); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); @@ -45,6 +48,7 @@ namespace v2x rclcpp::Client::SharedPtr get_vehicle_dimensions_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; rclcpp::Publisher::SharedPtr cpm_sender_pub_; + rclcpp::Publisher::SharedPtr cam_rec_pub_; double pos_lat_; double pos_lon_; diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 3e20eb1..d15d223 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,7 +1,7 @@ - + - \ No newline at end of file + diff --git a/package.xml b/package.xml index 94604d1..c45acae 100644 --- a/package.xml +++ b/package.xml @@ -12,13 +12,14 @@ autoware_auto_perception_msgs autoware_auto_vehicle_msgs autoware_adapi_v1_msgs - tf2_msgs + tf2_msgs tf2 geometry_msgs rclcpp std_msgs rclcpp_components Vanetza + etsi_its_messages ament_lint_auto ament_lint_common diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 3f1243f..6faef39 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -21,6 +21,10 @@ #include #include +#include +#include +#include + #include #include @@ -83,9 +87,64 @@ namespace v2x return btp::ports::CAM; } - void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr) + void CamApplication::indicate(const Application::DataIndication &indication, Application::UpPacketPtr packet) { - // TODO: implement + asn1::PacketVisitor visitor; + std::shared_ptr rec_cam_ptr = boost::apply_visitor(visitor, *packet); + + if (!rec_cam_ptr) { + RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received invalid CAM"); + return; + } + + asn1::Cam rec_cam = *rec_cam_ptr; + RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received CAM from station with ID #%ld", rec_cam->header.stationID); + std::chrono::milliseconds now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + + cam_ts_CAM_t ts_cam; + + cam_ts_ItsPduHeader_t &header = ts_cam.header; + header.protocolVersion = rec_cam->header.protocolVersion; + header.messageId = rec_cam->header.messageID; + header.stationId = rec_cam->header.stationID; + + cam_ts_CamPayload_t &coopAwareness = ts_cam.cam; + coopAwareness.generationDeltaTime = rec_cam->cam.generationDeltaTime; + + cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer; + BasicContainer_t &rec_basic_container = rec_cam->cam.camParameters.basicContainer; + basic_container.stationType = rec_basic_container.stationType; + basic_container.referencePosition.latitude = rec_basic_container.referencePosition.latitude; + basic_container.referencePosition.longitude = rec_basic_container.referencePosition.longitude; + basic_container.referencePosition.altitude.altitudeValue = rec_basic_container.referencePosition.altitude.altitudeValue; + basic_container.referencePosition.altitude.altitudeConfidence = rec_basic_container.referencePosition.altitude.altitudeConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation; + + cam_ts_BasicVehicleContainerHighFrequency_t &bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + BasicVehicleContainerHighFrequency_t &rec_bvc = rec_cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + bvc.heading.headingValue = rec_bvc.heading.headingValue; + bvc.heading.headingConfidence = rec_bvc.heading.headingConfidence; + bvc.speed.speedValue = rec_bvc.speed.speedValue; + bvc.speed.speedConfidence = rec_bvc.speed.speedConfidence; + bvc.driveDirection = rec_bvc.driveDirection; + bvc.vehicleLength.vehicleLengthValue = rec_bvc.vehicleLength.vehicleLengthValue; + bvc.vehicleLength.vehicleLengthConfidenceIndication = rec_bvc.vehicleLength.vehicleLengthConfidenceIndication; + bvc.vehicleWidth = rec_bvc.vehicleWidth; + bvc.longitudinalAcceleration.value = rec_bvc.longitudinalAcceleration.longitudinalAccelerationValue; + bvc.longitudinalAcceleration.confidence = rec_bvc.longitudinalAcceleration.longitudinalAccelerationConfidence; + bvc.curvature.curvatureValue = rec_bvc.curvature.curvatureValue; + bvc.curvature.curvatureConfidence = rec_bvc.curvature.curvatureConfidence; + bvc.curvatureCalculationMode = rec_bvc.curvatureCalculationMode; + bvc.yawRate.yawRateValue = rec_bvc.yawRate.yawRateValue; + bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence; + + etsi_its_cam_ts_msgs::msg::CAM ros_msg; + etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg); + + node_->publishReceivedCam(ros_msg); } void CamApplication::updateMGRS(double *x, double *y) { @@ -136,7 +195,8 @@ namespace v2x RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0"); return; } - float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; + float longitudinal_acceleration; + if (dt != 0) longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; velocityReport_.stamp = msg->header.stamp; velocityReport_.heading_rate = msg->heading_rate; @@ -170,8 +230,6 @@ namespace v2x sending_ = true; - RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_); - vanetza::asn1::Cam message; ItsPduHeader_t &header = message->header; @@ -181,7 +239,6 @@ namespace v2x CoopAwareness_t &cam = message->cam; - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); cam.generationDeltaTime = generationDeltaTime_; BasicContainer_t &basic_container = cam.camParameters.basicContainer; @@ -315,6 +372,8 @@ namespace v2x throw std::runtime_error("[CamApplication::send] CAM application data request failed"); } + RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Successfully sent"); + sending_ = false; std::chrono::milliseconds ms = std::chrono::duration_cast ( diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 5b4d291..164fd75 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -41,7 +41,7 @@ namespace v2x // Topic subscriptions for CAMApplication velocity_report_sub_ = this->create_subscription("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1)); - vehicle_status_sub_ = this->create_subscription("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, this, _1)); + vehicle_status_sub_ = this->create_subscription("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatusCallback, this, _1)); get_vehicle_dimensions_ = this->create_client("/api/vehicle/dimensions"); if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(60))) { RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available."); @@ -53,8 +53,10 @@ namespace v2x cpm_objects_pub_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); // cpm_sender_pub_ = create_publisher("/v2x/cpm/sender", rclcpp::QoS{10}); + cam_rec_pub_ = create_publisher("/v2x/cam_ts/received", rclcpp::QoS{10}); + // Declare Parameters - this->declare_parameter("network_interface", "vmnet1"); + this->declare_parameter("network_interface", "v2x_testing"); this->declare_parameter("is_sender", true); // Launch V2XApp in a new thread @@ -166,6 +168,11 @@ namespace v2x cpm_objects_pub_->publish(output_dynamic_object_msg); } + void V2XNode::publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg) { + RCLCPP_INFO(get_logger(), "Publishing CAM to ROS network"); + cam_rec_pub_->publish(msg); + } + void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. @@ -185,7 +192,7 @@ namespace v2x app->velocityReportCallback(msg); } - void V2XNode::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { + void V2XNode::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { app->vehicleStatusCallback(msg); } @@ -201,8 +208,9 @@ namespace v2x auto dimensions = response->dimensions; if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0) this->getVehicleDimensions(); - else + else { app->setVehicleDimensions(dimensions); + } } catch (const std::exception &e) { RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what()); }