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>
This commit is contained in:
parent
73756de59e
commit
7b7bb53b39
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
/**:
|
||||
ros__parameters:
|
||||
network_interface: "wlp4s0"
|
||||
network_interface: "v2x_testing"
|
||||
is_sender: true
|
||||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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"/>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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> (
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue