Add receiver for CAMApplication

- Using etsi_its_messages (https://github.com/ika-rwth-aachen/etsi_its_messages) to convert received ETSI CAMs to ROS messages
- Created example topic "/v2x/cam_ts/received"
- Some other small fixes

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>

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"

config/autoware_v2x.param.yaml

@@ -1,4 +1,4 @@
 /**:
   ros__parameters:
-    network_interface: "wlp4s0"
+    network_interface: "v2x_testing"
     is_sender: true

include/autoware_v2x/v2x_node.hpp

@@ -19,6 +19,8 @@
 #include "autoware_v2x/router_context.hpp"
 #include <fstream>
 
+#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
+
 namespace v2x
 {
   class V2XNode : public rclcpp::Node
@@ -28,13 +30,14 @@ namespace v2x
     V2XApp *app;
     void publishObjects(std::vector<CpmApplication::Object> *, 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<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedPtr get_vehicle_dimensions_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_sender_pub_;
+    rclcpp::Publisher<etsi_its_cam_ts_msgs::msg::CAM>::SharedPtr cam_rec_pub_;
 
     double pos_lat_;
     double pos_lon_;

launch/v2x.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="network_interface" default="wlp5s0"/>
+  <arg name="network_interface" default="v2x_testing"/>
   <arg name="is_sender" default="true"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>

package.xml

@@ -19,6 +19,7 @@
   <depend>std_msgs</depend>
   <depend>rclcpp_components</depend>
   <depend>Vanetza</depend>
+  <depend>etsi_its_messages</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/cam_application.cpp

@@ -21,6 +21,10 @@
 #include <GeographicLib/MGRS.hpp>
 #include <string>
 
+#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
+#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
+#include <etsi_its_cam_ts_conversion/convertCAM.h>
+
 #include <boost/units/cmath.hpp>
 #include <boost/units/systems/si/prefixes.hpp>
 
@@ -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<asn1::Cam> visitor;
+    std::shared_ptr<const asn1::Cam> 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::milliseconds>(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<std::chrono::milliseconds> (

src/v2x_node.cpp

@@ -41,7 +41,7 @@ namespace v2x
 
     // Topic subscriptions for CAMApplication
     velocity_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1));
-    vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, this, _1));
+    vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatusCallback, this, _1));
     get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/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<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
     // cpm_sender_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
 
+    cam_rec_pub_ = create_publisher<etsi_its_cam_ts_msgs::msg::CAM>("/v2x/cam_ts/received", rclcpp::QoS{10});
+
     // Declare Parameters
-    this->declare_parameter<std::string>("network_interface", "vmnet1");
+    this->declare_parameter<std::string>("network_interface", "v2x_testing");
     this->declare_parameter<bool>("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());
        }