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

Add is_sender

This commit is contained in:
Yu Asabe 2022-02-27 12:26:11 +09:00
parent 91962c7286
commit 60eea65de3
3 changed files with 11 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/v2x.launch.xml
View File

@ -1,6 +1,8 @@
<launch>
<arg name="network_interface" default="wlp5s0"/>
<arg name="is_sender" default="true"/>
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
<param name="network_interface" value="$(var network_interface)"/>
<param name="is_sender" value="$(var is_sender)"/>
</node>
</launch>

18
src/v2x_app.cpp
View File

@ -61,7 +61,6 @@ namespace v2x
timestamp_sec -= 1072915200; // convert to etsi-epoch
long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000;
int gdt = timestamp_msec % 65536;
// RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
double rot_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y;
@ -74,21 +73,12 @@ namespace v2x
double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
char mgrs[30];
int zone = 54;
int grid_num_x = 4;
int grid_num_y = 39;
int grid_size = 100000;
int prec;
bool northp = true;
double x_mgrs, y_mgrs;
double lat, lon;
// sprintf(mgrs, "54SVE%.3f%.3f", x, y);
// RCLCPP_INFO(node_->get_logger(), "%s", mgrs);
// Convert MGRS coordinate to UTM coordinate.
// GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
// Reverse projection from UTM to geographic.
GeographicLib::UTMUPS::Reverse(
@ -99,9 +89,7 @@ namespace v2x
lat,
lon
);
RCLCPP_INFO(node_->get_logger(), "%f, %f", lat, lon);
if (cp && cp_started_) {
cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z);
@ -141,7 +129,9 @@ namespace v2x
context.set_link_layer(link_layer.get());
cp = new CpmApplication(node_, trigger.runtime());
bool is_sender;
node_->get_parameter("is_sender", is_sender);
cp = new CpmApplication(node_, trigger.runtime(), is_sender);
context.enable(cp);

9
src/v2x_node.cpp
View File

@ -34,6 +34,7 @@ namespace v2x
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true);
app = new V2XApp(this);
boost::thread v2xApp(boost::bind(&V2XApp::start, app));
@ -83,7 +84,7 @@ namespace v2x
}
current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
publisher_->publish(output_dynamic_object_msg);
}
@ -91,11 +92,11 @@ namespace v2x
void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::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());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());
// Measuring T_A1R1
RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
app->objectsCallback(msg);
}