TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #10 from tlab-wide/feat/cpm_sender_topic 

Feat/cpm sender topic
This commit is contained in:
Yu Asabe 2022-05-19 20:00:46 +09:00 committed by GitHub
parent 65c92be3f9 2e78973ef8
commit 41701fff9a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 53 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/autoware_v2x/v2x_node.hpp
View File

@ -23,6 +23,7 @@ namespace v2x
explicit V2XNode(const rclcpp::NodeOptions &node_options);
V2XApp *app;
void publishObjects(std::vector<CpmApplication::Object> *);
void publishCpmSenderObject(double, double, double);
private:
void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
@ -31,6 +32,7 @@ namespace v2x
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_v2x_cpm_sender_;
double pos_lat_;
double pos_lon_;

6
src/cpm_application.cpp
View File

@ -131,6 +131,11 @@ namespace v2x {
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation);
// Publish CPM Sender info to /v2x/cpm/sender through V2XNode function
node_->publishCpmSenderObject(x_mgrs, y_mgrs, orientation);
// Get PerceivedObjects
receivedObjectsStack.clear();
@ -452,6 +457,7 @@ namespace v2x {
StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
sdc = vanetza::asn1::allocate<StationDataContainer_t>();
sdc->present = StationDataContainer_PR_originatingVehicleContainer;
OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;

45
src/v2x_node.cpp
View File

@ -17,6 +17,8 @@
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "tf2/LinearMath/Quaternion.h"
namespace gn = vanetza::geonet;
using namespace vanetza;
@ -32,6 +34,7 @@ namespace v2x
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
publisher_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
publisher_v2x_cpm_sender_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true);
@ -46,6 +49,48 @@ namespace v2x
}
void V2XNode::publishCpmSenderObject(double x_mgrs, double y_mgrs, double orientation) {
autoware_auto_perception_msgs::msg::PredictedObjects cpm_sender_object_msg;
std_msgs::msg::Header header;
rclcpp::Time current_time = this->now();
cpm_sender_object_msg.header.frame_id = "map";
cpm_sender_object_msg.header.stamp = current_time;
autoware_auto_perception_msgs::msg::PredictedObject object;
autoware_auto_perception_msgs::msg::ObjectClassification classification;
autoware_auto_perception_msgs::msg::Shape shape;
autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics;
classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
classification.probability = 0.99;
shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
shape.dimensions.x = 5.0;
shape.dimensions.y = 2.0;
shape.dimensions.z = 1.7;
kinematics.initial_pose_with_covariance.pose.position.x = x_mgrs;
kinematics.initial_pose_with_covariance.pose.position.y = y_mgrs;
kinematics.initial_pose_with_covariance.pose.position.z = 0.1;
tf2::Quaternion quat;
quat.setRPY(0, 0, orientation);
kinematics.initial_pose_with_covariance.pose.orientation.x = quat.x();
kinematics.initial_pose_with_covariance.pose.orientation.y = quat.y();
kinematics.initial_pose_with_covariance.pose.orientation.z = quat.z();
kinematics.initial_pose_with_covariance.pose.orientation.w = quat.w();
object.classification.emplace_back(classification);
object.shape = shape;
object.kinematics = kinematics;
cpm_sender_object_msg.objects.push_back(object);
publisher_v2x_cpm_sender_->publish(cpm_sender_object_msg);
}
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg;
std_msgs::msg::Header header;