pub predicted obj

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

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

@@ -3,6 +3,7 @@
   <node pkg="reaction_analyzer" exec="reaction_analyzer_exe" name="reaction_analyzer" output="screen">
     <param from="$(var reaction_analyzer_param_path)"/>
     <remap from="output/points_raw" to="/perception/obstacle_segmentation/pointcloud"/>
+    <remap from="output/objects" to="/perception/object_recognition/objects"/>
     <remap from="input/kinematics" to="/localization/kinematic_state"/>
   </node>
 </launch>

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -20,6 +20,11 @@
 #include <pcl_ros/transforms.hpp>
 #include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
 
+#include <boost/uuid/string_generator.hpp>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
 #include <algorithm>
 #include <memory>
 
@@ -47,8 +52,11 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(const rclcpp::NodeOptions & options)
 
   ros_entity_pointcloud_ptr_ = std::make_shared<PointCloud2>();
   initPointcloud();
-
+  std::cout << "1" << std::endl;
+  initPredictedObjects();
+  std::cout << "2" << std::endl;
   pub_pointcloud_ = create_publisher<PointCloud2>("output/points_raw", rclcpp::QoS(1));
+  pub_predicted_objects_ = create_publisher<PredictedObjects>("output/objects", rclcpp::QoS(1));
   sub_kinematics_ = create_subscription<Odometry>(
     "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
   //  sub_cmd_ = create_subscription<AckermannControlCommand>(
@@ -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<float>();
     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<PredictedObjects>(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 <rclcpp_components/register_node_macro.hpp>