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

Merge pull request #5 from tlab-wide/update-to-universe 

Update to universe
This commit is contained in:
Yu Asabe 2022-03-27 19:19:18 -07:00 committed by GitHub
parent 1c56a7270f 56740cd436
commit ec110bb46f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 57 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/autoware_v2x/cpm_application.hpp
View File

@ -5,7 +5,7 @@
#include "rclcpp/rclcpp.hpp"
#include <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.hpp>
#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;

0
include/autoware_v2x/cpm_receiver.hpp
View File

4
include/autoware_v2x/v2x_app.hpp
View File

@ -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 <boost/asio/io_service.hpp>
#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;

9
include/autoware_v2x/v2x_node.hpp
View File

@ -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 <boost/asio/io_service.hpp>
#include "autoware_v2x/v2x_app.hpp"
@ -25,13 +25,12 @@ namespace v2x
void publishObjects(std::vector<CpmApplication::Object> *);
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<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_;
double pos_lat_;
double pos_lon_;

2
package.xml
View File

@ -9,7 +9,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>tf2_msgs</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>

44
src/cpm_application.cpp
View File

@ -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<asn1::Cpm> visitor;
std::shared_ptr<const asn1::Cpm> 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());

123
src/cpm_receiver.cpp
View File

@ -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 <vanetza/asn1/cpm.hpp>
#include <vanetza/facilities/cpm_functions.hpp>
#include <sstream>
#include <memory>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
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<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/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<CpmApplication> 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)

2
src/v2x_app.cpp
View File

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

40
src/v2x_node.cpp
View File

@ -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<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
publisher_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true);
@ -47,38 +47,38 @@ namespace v2x
}
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *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());