feat: add predicted path for the PredictedObject and add publish_only_pointcloud_with_object

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

tools/reaction_analyzer/README.md

@@ -138,6 +138,7 @@ vehicle should be same.**
 | `topic_publisher.path_bag_with_object`                                       | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode.                                                                 |
 | `topic_publisher.pointcloud_publisher.pointcloud_publisher_type`             | string | Defines how the PointCloud2 messages are going to be published. Modes explained above.                                                        |
 | `topic_publisher.pointcloud_publisher.pointcloud_publisher_period`           | double | [s] Publishing period of the PointCloud2 messages.                                                                                            |
+| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object`   | bool   | Default false. Publish only the point cloud messages with the object.                                                                         |
 | `reaction_params.first_brake_params.debug_control_commands`                  | bool   | Debug publish flag.                                                                                                                           |
 | `reaction_params.first_brake_params.control_cmd_buffer_time_interval`        | double | [s] Time interval for buffering control commands.                                                                                             |
 | `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int    | Minimum number of control commands in descending order for triggering brake.                                                                  |

tools/reaction_analyzer/include/topic_publisher.hpp

@@ -70,10 +70,12 @@ enum class PublisherMessageType {
 
 struct TopicPublisherParams
 {
-  std::string path_bag_without_object;    // Path to the bag file without object
-  std::string path_bag_with_object;       // Path to the bag file with object
-  std::string pointcloud_publisher_type;  // Type of the pointcloud publisher
-  double pointcloud_publisher_period;     // Period of the pointcloud publisher
+  std::string path_bag_without_object;       // Path to the bag file without object
+  std::string path_bag_with_object;          // Path to the bag file with object
+  std::string pointcloud_publisher_type;     // Type of the pointcloud publisher
+  double pointcloud_publisher_period;        // Period of the pointcloud publisher
+  bool publish_only_pointcloud_with_object;  // Publish only pointcloud with object for only
+                                             // perception pipeline debug purpose make it true.
 };
 
 enum class PointcloudPublisherType {

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -26,18 +26,19 @@
       y_dimension: 1.7809072588403219
       z_dimension: 0.8328610206872963
     goal_pose:
-      x: 81462.78125
-      y: 49978.484375
+      x: 81824.90625
+      y: 50069.34375
       z: 0.0
       roll: 0.0
       pitch: 0.0
-      yaw: 35.0
+      yaw: 8.9305903
     topic_publisher:
       path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
       path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
       pointcloud_publisher:
         pointcloud_publisher_type: "sync_header_sync_publish" # "async_header_sync_publish", "sync_header_sync_publish" or "async_publish"
         pointcloud_publisher_period: 0.1 # s
+        publish_only_pointcloud_with_object: false # use it only for debug purposes. If true, only pointclouds with object will be published
     subscriber:
       reaction_params:
         first_brake_params:

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -417,11 +417,18 @@ void ReactionAnalyzerNode::initPredictedObjects()
   obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
   obj.existence_probability = 1.0;
   obj.kinematics.initial_pose_with_covariance.pose = entity_pose_;
+
+  autoware_auto_perception_msgs::msg::PredictedPath path;
+  path.confidence = 1.0;
+  path.path.emplace_back(entity_pose_);
+  obj.kinematics.predicted_paths.emplace_back(path);
+
   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_obstacle"));
+
   PredictedObjects pred_objects;
   pred_objects.objects.emplace_back(obj);
   predicted_objects_ptr_ = std::make_shared<PredictedObjects>(pred_objects);

tools/reaction_analyzer/src/topic_publisher.cpp

@@ -37,6 +37,9 @@ TopicPublisher::TopicPublisher(
   topic_publisher_params_.pointcloud_publisher_period =
     node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_period")
       .as_double();
+  topic_publisher_params_.publish_only_pointcloud_with_object =
+    node_->get_parameter("topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object")
+      .as_bool();
 
   // set pointcloud publisher type
   if (topic_publisher_params_.pointcloud_publisher_type == "sync_header_sync_publish") {
@@ -64,9 +67,11 @@ void TopicPublisher::pointcloudMessagesSyncPublisher(const PointcloudPublisherTy
       PublisherVarAccessor accessor;
       for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) {
         accessor.publishWithCurrentTime(
-          *publisher_var_pair.second.first, current_time, is_object_spawned);
+          *publisher_var_pair.second.first, current_time,
+          topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
         accessor.publishWithCurrentTime(
-          *publisher_var_pair.second.second, current_time, is_object_spawned);
+          *publisher_var_pair.second.second, current_time,
+          topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
       }
       if (is_object_spawned && !is_object_spawned_message_published) {
         is_object_spawned_message_published = true;
@@ -87,9 +92,11 @@ void TopicPublisher::pointcloudMessagesSyncPublisher(const PointcloudPublisherTy
         const auto header_time =
           current_time - std::chrono::nanoseconds(counter * phase_dif.count());
         accessor.publishWithCurrentTime(
-          *publisher_var_pair.second.first, header_time, is_object_spawned);
+          *publisher_var_pair.second.first, header_time,
+          topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
         accessor.publishWithCurrentTime(
-          *publisher_var_pair.second.second, header_time, is_object_spawned);
+          *publisher_var_pair.second.second, header_time,
+          topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
         counter++;
       }
       if (is_object_spawned && !is_object_spawned_message_published) {
@@ -113,8 +120,12 @@ void TopicPublisher::pointcloudMessagesAsyncPublisher(
   PublisherVarAccessor accessor;
   const auto current_time = node_->now();
   const bool is_object_spawned = spawn_object_cmd_;
-  accessor.publishWithCurrentTime(*lidar_output_pair_.first, current_time, is_object_spawned);
-  accessor.publishWithCurrentTime(*lidar_output_pair_.second, current_time, is_object_spawned);
+  accessor.publishWithCurrentTime(
+    *lidar_output_pair_.first, current_time,
+    topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
+  accessor.publishWithCurrentTime(
+    *lidar_output_pair_.second, current_time,
+    topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned);
 
   if (is_object_spawned && !is_object_spawned_message_published) {
     is_object_spawned_message_published = true;
@@ -207,9 +218,6 @@ void TopicPublisher::initRosbagPublishers()
 
       const auto message_type = getMessageTypeForTopic(current_topic);
       if (message_type == PublisherMessageType::UNKNOWN) {
-        RCLCPP_WARN(
-          node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
-          current_topic.c_str());
         continue;
       }
 
@@ -427,9 +435,6 @@ void TopicPublisher::initRosbagPublishers()
             break;
           }
           default:
-            RCLCPP_WARN(
-              node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
-              current_topic.c_str());
             break;
         }
       }
@@ -498,9 +503,6 @@ void TopicPublisher::initRosbagPublishers()
 
       const auto message_type = getMessageTypeForTopic(current_topic);
       if (message_type == PublisherMessageType::UNKNOWN) {
-        RCLCPP_WARN(
-          node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
-          current_topic.c_str());
         continue;
       }
       switch (message_type) {
@@ -740,9 +742,6 @@ void TopicPublisher::initRosbagPublishers()
           break;
         }
         default:
-          RCLCPP_WARN(
-            node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
-            current_topic.c_str());
           break;
       }
     }