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

Feat/cpm sender topic

include/autoware_v2x/v2x_node.hpp

@@ -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_;

src/cpm_application.cpp

@@ -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;

src/v2x_node.cpp

@@ -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;