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

Update to autoware_auto_msgs

This commit is contained in:
Yu Asabe 2022-03-28 11:17:51 +09:00
parent f3733b8a8a
commit 56740cd436
6 changed files with 56 additions and 49 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;

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

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());

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());