From 468667f5bbc64a2c6aed5206be5096cf691e3aca Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Mon, 28 Mar 2022 11:16:33 +0900 Subject: [PATCH 1/3] Remove cpm_receiver since it is not used --- include/autoware_v2x/cpm_receiver.hpp | 0 src/cpm_receiver.cpp | 123 -------------------------- 2 files changed, 123 deletions(-) delete mode 100644 include/autoware_v2x/cpm_receiver.hpp delete mode 100644 src/cpm_receiver.cpp diff --git a/include/autoware_v2x/cpm_receiver.hpp b/include/autoware_v2x/cpm_receiver.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/cpm_receiver.cpp b/src/cpm_receiver.cpp deleted file mode 100644 index 3727d00..0000000 --- a/src/cpm_receiver.cpp +++ /dev/null @@ -1,123 +0,0 @@ -#include "autoware_v2x/cpm_sender.hpp" -#include "autoware_v2x/time_trigger.hpp" -#include "autoware_v2x/router_context.hpp" -#include "autoware_v2x/positioning.hpp" -#include "autoware_v2x/security.hpp" -#include "autoware_v2x/link_layer.hpp" -#include "autoware_v2x/cpm_application.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include -#include - -namespace gn = vanetza::geonet; - -using namespace vanetza; -using namespace vanetza::facilities; -using namespace std::chrono; - -namespace v2x -{ - V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : - rclcpp::Node("autoware_v2x_node", node_options), - io_service_(), - trigger_(io_service_), - device_name_("wlp4s0"), - device_(device_name_), - mac_address_(device_.address()), - link_layer_(create_link_layer(io_service_, device_, "ethernet")), - positioning_(create_position_provider(io_service_, trigger_.runtime())), - security_(create_security_entity(trigger_.runtime(), *positioning_)), - mib_(), - context_(mib_, trigger_, *positioning_, security_.get()), - cp_(new CpmApplication(this)), - app_() - { - using std::placeholders::_1; - subscription_ = this->create_subscription("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); - - subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); - RCLCPP_INFO(get_logger(), "V2X Node Launched"); - - // device_name_ = "wlp4s0"; - // device_(device_name_); - // mac_address_ = device_.address(); - - std::stringstream sout; - sout << mac_address_; - RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); - - // trigger_(io_service_); - - // link_layer_ = create_link_layer(io_service_, device_, "ethernet"); - mib_.itsGnLocalGnAddr.mid(mac_address_); - mib_.itsGnLocalGnAddr.is_manually_configured(true); - mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed; - mib_.itsGnSecurity = false; - mib_.itsGnProtocolVersion = 1; - - // context_.router_.set_address(mib_.itsGnLocalGnAddr); - context_.updateMIB(mib_); - - // positioning_ = create_position_provider(io_service_, trigger_.runtime()); - // security_ = create_security_entity(trigger_.runtime(), *positioning_); - - // RouterContext context(mib_, trigger_, *positioning_, security_.get()); - // RouterContext context_(mib_, trigger_, *positioning_, security_.get()); - context_.set_link_layer(link_layer_.get()); - - // std::unique_ptr cp_ { new CpmApplication(this) }; - // app_ = std::move(cp_); - - context_.enable(cp_.get()); - - - // io_service_.run(); - - // // Print MAC Address to logger - // std::stringstream sout; - // sout << mac_address; - // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); - } - - void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) - { - // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); - RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); - // Send CPM - cp_->send(msg, this, pos_lat_, pos_lon_); - } - - void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { - // RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z); - - float x = msg->transforms[0].transform.translation.x; - float y = msg->transforms[0].transform.translation.y; - float z = msg->transforms[0].transform.translation.z; - - char mgrs[20]; - int zone, prec; - bool northp; - double x_mgrs, y_mgrs; - double lat, lon; - sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); - RCLCPP_INFO(get_logger(), "MGRS: %s", mgrs); - - GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); - GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); - - pos_lat_ = lat; - pos_lon_ = lon; - - RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); - } -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) \ No newline at end of file From f3733b8a8ae9fbd7559976d8a61396a63a0b2eea Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Mon, 28 Mar 2022 11:16:48 +0900 Subject: [PATCH 2/3] Update to autoware_auto_perception_msgs --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index f08cf1f..ea904ab 100644 --- a/package.xml +++ b/package.xml @@ -9,7 +9,7 @@ ament_cmake_auto - autoware_perception_msgs + autoware_auto_perception_msgs tf2_msgs geometry_msgs rclcpp From 56740cd43682cb3b36638bbfc466cdd91ebb53c4 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Mon, 28 Mar 2022 11:17:51 +0900 Subject: [PATCH 3/3] Update to autoware_auto_msgs --- include/autoware_v2x/cpm_application.hpp | 8 ++--- include/autoware_v2x/v2x_app.hpp | 4 +-- include/autoware_v2x/v2x_node.hpp | 9 +++-- src/cpm_application.cpp | 42 ++++++++++++++---------- src/v2x_app.cpp | 2 +- src/v2x_node.cpp | 40 +++++++++++----------- 6 files changed, 56 insertions(+), 49 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 4f64a76..ce85419 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -5,7 +5,7 @@ #include "rclcpp/rclcpp.hpp" #include #include -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_v2x/positioning.hpp" namespace v2x @@ -16,17 +16,17 @@ namespace v2x public: CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); PortType port() override; + std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&); void indicate(const DataIndication &, UpPacketPtr) override; void set_interval(vanetza::Clock::duration); - void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); + void updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void updateMGRS(double *, double *); void updateRP(double *, double *, double *); void updateGenerationDeltaTime(int *, long long *); void updateHeading(double *); void send(); - struct Object - { + struct Object { int objectID; // 0-255 rclcpp::Time timestamp; double position_x; diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index 7526af4..bdacc50 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -3,7 +3,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/cpm_application.hpp" @@ -23,7 +23,7 @@ namespace v2x public: V2XApp(V2XNode *); void start(); - void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); + void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); CpmApplication *cp; diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 0427084..4c5440a 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -3,7 +3,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/v2x_app.hpp" @@ -25,13 +25,12 @@ namespace v2x void publishObjects(std::vector *); private: - void objectsCallback( - const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); + void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; rclcpp::Subscription::SharedPtr subscription_pos_; - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; double pos_lat_; double pos_lon_; diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 4d1cd7c..e1df7d4 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -65,13 +65,21 @@ namespace v2x { return btp::ports::CPM; } + std::string CpmApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) { + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); + } + void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) { asn1::PacketVisitor visitor; std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); if (cpm) { - // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content"); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received CPM"); rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); @@ -112,8 +120,8 @@ namespace v2x { double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0; if (orientation < 0.0) orientation += (2.0 * M_PI); // double orientation = heading / 10.0; - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); // Get PerceivedObjects receivedObjectsStack.clear(); @@ -150,7 +158,7 @@ namespace v2x { } node_->publishObjects(&receivedObjectsStack); } else { - RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); + // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); } if (reflect_packet_) { @@ -198,7 +206,7 @@ namespace v2x { ego_.heading = *yaw; } - void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { + void CpmApplication::updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { updating_objects_stack_ = true; if (sending_) { @@ -210,17 +218,20 @@ namespace v2x { if (msg->objects.size() > 0) { int i = 0; - for (auto obj : msg->objects) { + for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { + + // RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label); + CpmApplication::Object object; object.objectID = i; object.timestamp = msg->header.stamp; - object.position_x = obj.state.pose_covariance.pose.position.x; // MGRS - object.position_y = obj.state.pose_covariance.pose.position.y; - object.position_z = obj.state.pose_covariance.pose.position.z; - object.orientation_x = obj.state.pose_covariance.pose.orientation.x; - object.orientation_y = obj.state.pose_covariance.pose.orientation.y; - object.orientation_z = obj.state.pose_covariance.pose.orientation.z; - object.orientation_w = obj.state.pose_covariance.pose.orientation.w; + object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; // MGRS + object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; + object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; + object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; + object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; + object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; + object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w; object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); @@ -251,8 +262,6 @@ namespace v2x { } else { object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 } - // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.yawAngle: %d", object.yawAngle); - // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); long long msg_timestamp_sec = msg->header.stamp.sec; long long msg_timestamp_nsec = msg->header.stamp.nanosec; @@ -267,10 +276,9 @@ namespace v2x { } objectsStack.push_back(object); ++i; - - // RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle); } } + // RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index a56db42..89fd3fc 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -38,7 +38,7 @@ namespace v2x { } - void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { + void V2XApp::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); if (!tf_received_) { RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 7e4dd0e..c4644fd 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -27,11 +27,11 @@ namespace v2x { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { using std::placeholders::_1; - subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); + subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); - publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); + publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); this->declare_parameter("is_sender", true); @@ -47,38 +47,38 @@ namespace v2x } void V2XNode::publishObjects(std::vector *objectsStack) { - autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg; + autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg; std_msgs::msg::Header header; rclcpp::Time current_time = this->now(); output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.stamp = current_time; for (CpmApplication::Object obj : *objectsStack) { - autoware_perception_msgs::msg::DynamicObject object; - autoware_perception_msgs::msg::Semantic semantic; - autoware_perception_msgs::msg::Shape shape; - autoware_perception_msgs::msg::State state; + 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; - semantic.type = autoware_perception_msgs::msg::Semantic::CAR; - semantic.confidence = 0.99; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + classification.probability = 0.99; - shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = obj.shape_x / 10.0; shape.dimensions.y = obj.shape_y / 10.0; shape.dimensions.z = obj.shape_z / 10.0; - state.pose_covariance.pose.position.x = obj.position_x; - state.pose_covariance.pose.position.y = obj.position_y; - state.pose_covariance.pose.position.z = 0.1; + kinematics.initial_pose_with_covariance.pose.position.x = obj.position_x; + kinematics.initial_pose_with_covariance.pose.position.y = obj.position_y; + kinematics.initial_pose_with_covariance.pose.position.z = 0.1; - state.pose_covariance.pose.orientation.x = obj.orientation_x; - state.pose_covariance.pose.orientation.y = obj.orientation_y; - state.pose_covariance.pose.orientation.z = obj.orientation_z; - state.pose_covariance.pose.orientation.w = obj.orientation_w; + kinematics.initial_pose_with_covariance.pose.orientation.x = obj.orientation_x; + kinematics.initial_pose_with_covariance.pose.orientation.y = obj.orientation_y; + kinematics.initial_pose_with_covariance.pose.orientation.z = obj.orientation_z; + kinematics.initial_pose_with_covariance.pose.orientation.w = obj.orientation_w; - object.semantic = semantic; + object.classification.emplace_back(classification); object.shape = shape; - object.state = state; + object.kinematics = kinematics; output_dynamic_object_msg.objects.push_back(object); } @@ -89,7 +89,7 @@ namespace v2x publisher_->publish(output_dynamic_object_msg); } - void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { + void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { rclcpp::Time current_time = this->now(); rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());