From 8d62908ef6d38e908c1acef9aaaad76cc47d594b Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 20 Dec 2023 13:59:19 +0300 Subject: [PATCH] pub predicted obj --- .../include/reaction_analyzer_node.hpp | 3 +- .../launch/reaction_analyzer.launch.xml | 1 + .../src/reaction_analyzer_node.cpp | 103 ++++++++++-------- 3 files changed, 63 insertions(+), 44 deletions(-) diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 0b38a17be7844..b7c7d74909f7f 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -76,7 +76,8 @@ class ReactionAnalyzerNode : public rclcpp::Node // Obstacle Spawn EntityParams entity_params_; - geometry_msgs::msg::Point entity_origin; + geometry_msgs::msg::Pose entity_pose; + void initPointcloud(); void initPredictedObjects(); PointCloud2::SharedPtr ros_entity_pointcloud_ptr_; diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml index 948ce5daef7ad..073e8ab3b8294 100644 --- a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -3,6 +3,7 @@ + diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 9755db58136a2..ae283c9b4330c 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -20,6 +20,11 @@ #include #include +#include +#include +#include +#include + #include #include @@ -47,8 +52,11 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(const rclcpp::NodeOptions & options) ros_entity_pointcloud_ptr_ = std::make_shared(); initPointcloud(); - + std::cout << "1" << std::endl; + initPredictedObjects(); + std::cout << "2" << std::endl; pub_pointcloud_ = create_publisher("output/points_raw", rclcpp::QoS(1)); + pub_predicted_objects_ = create_publisher("output/objects", rclcpp::QoS(1)); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); // sub_cmd_ = create_subscription( @@ -72,15 +80,28 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(const rclcpp::NodeOptions & options) void ReactionAnalyzerNode::onTimer() { + + if (!odometry_){ + RCLCPP_DEBUG(get_logger(), "Not Ready!"); + return; + } + const auto current_pose = odometry_->pose.pose; - const bool spawn = tier4_autoware_utils::calcDistance3d(current_pose.position, entity_origin) < - node_params_.spawn_distance_threshold; - if (!spawn){ + + const bool spawn = + tier4_autoware_utils::calcDistance3d(current_pose.position, entity_pose.position) < + node_params_.spawn_distance_threshold; + if (!spawn) { // send empty PointCloud2 empty_pointcloud; empty_pointcloud.header.frame_id = "base_link"; + PredictedObjects empty_predicted_objects; + empty_predicted_objects.header.frame_id = "map"; + empty_pointcloud.header.stamp = this->now(); pub_pointcloud_->publish(empty_pointcloud); + empty_predicted_objects.header.stamp = this->now(); + pub_predicted_objects_->publish(empty_predicted_objects); } else { // transform pointcloud geometry_msgs::msg::TransformStamped transform_stamped{}; @@ -99,10 +120,25 @@ void ReactionAnalyzerNode::onTimer() tf2::transformToEigen(transform_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(affine_matrix, *ros_entity_pointcloud_ptr_, transformed_points); transformed_points.header.frame_id = "base_link"; + + // predicted objects + predicted_objects_ptr->header.frame_id = "map"; + transformed_points.header.stamp = this->now(); pub_pointcloud_->publish(transformed_points); + predicted_objects_ptr->header.stamp = this->now(); + pub_predicted_objects_->publish(*predicted_objects_ptr); } +} + +unique_identifier_msgs::msg::UUID generateUUIDMsg(const std::string & input) +{ + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(input); + unique_identifier_msgs::msg::UUID uuid_msg; + std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); + return uuid_msg; } void ReactionAnalyzerNode::initPointcloud() @@ -137,9 +173,14 @@ void ReactionAnalyzerNode::initPointcloud() } } } - entity_origin.set__x(entity_params_.x); - entity_origin.set__y(entity_params_.y); - entity_origin.set__z(entity_params_.z); + // set entity pose + entity_pose.position.set__x(entity_params_.x); + entity_pose.position.set__y(entity_params_.y); + entity_pose.position.set__z(entity_params_.z); + entity_pose.orientation.x = entity_q_orientation.x(); + entity_pose.orientation.y = entity_q_orientation.y(); + entity_pose.orientation.z = entity_q_orientation.z(); + entity_pose.orientation.w = entity_q_orientation.w(); pcl::toROSMsg(point_cloud, *ros_entity_pointcloud_ptr_); } @@ -150,47 +191,23 @@ void ReactionAnalyzerNode::initPredictedObjects() dimension.set__x(entity_params_.x_l); dimension.set__y(entity_params_.y_l); dimension.set__z(entity_params_.z_l); - + std::cout << "1.1" << std::endl; PredictedObject obj; obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.set__dimensions(dimension); - obj.classification.begin( + obj.existence_probability = 1.0; + obj.kinematics.initial_pose_with_covariance.pose = entity_pose; + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + classification.probability = 1.0; + obj.classification.emplace_back(classification); + obj.set__object_id(generateUUIDMsg("test_vehicle")); + PredictedObjects pred_objects; + pred_objects.objects.emplace_back(obj); + predicted_objects_ptr = std::make_shared(pred_objects); + std::cout << "1.2" << std::endl; - predicted_objects_ptr->objects.begin()->shape.dimensions. - // prepare transform matrix - tf2::Quaternion entity_q_orientation; - entity_q_orientation.setRPY( - tier4_autoware_utils::deg2rad(entity_params_.roll), - tier4_autoware_utils::deg2rad(entity_params_.pitch), - tier4_autoware_utils::deg2rad(entity_params_.yaw)); - tf2::Transform tf(entity_q_orientation); - const auto origin = tf2::Vector3(entity_params_.x, entity_params_.y, entity_params_.z); - tf.setOrigin(origin); - - const double it_x = entity_params_.x_l / sampling_dist; - const double it_y = entity_params_.y_l / sampling_dist; - const double it_z = entity_params_.z_l / sampling_dist; - - // Sample each face of the box without rotation - for (int i = 0; i <= it_z; ++i) { - for (int j = 0; j <= it_y; ++j) { - for (int k = 0; k <= it_x; ++k) { - const double p_x = -entity_params_.x_l / 2 + k * sampling_dist; - const double p_y = -entity_params_.y_l / 2 + j * sampling_dist; - const double p_z = -entity_params_.z_l / 2 + i * sampling_dist; - const auto tmp = tf2::Vector3(p_x, p_y, p_z); - tf2::Vector3 data_out = tf * tmp; - point_cloud.emplace_back(pcl::PointXYZ(data_out.x(), data_out.y(), data_out.z())); - } - } - } - entity_origin.set__x(entity_params_.x); - entity_origin.set__y(entity_params_.y); - entity_origin.set__z(entity_params_.z); - - pcl::toROSMsg(point_cloud, *ros_entity_pointcloud_ptr_); } - } // namespace reaction_analyzer #include